[bullet] 01/01: New upstream version 2.85.1+dfsg
Markus Koschany
apo at moszumanska.debian.org
Mon Dec 26 22:49:18 UTC 2016
This is an automated email from the git hooks/post-receive script.
apo pushed a commit to branch upstream
in repository bullet.
commit 8322dc08789e7e361148774312c1f0b5abc609c6
Author: Markus Koschany <apo at debian.org>
Date: Mon Dec 26 19:38:33 2016 +0100
New upstream version 2.85.1+dfsg
---
.travis.yml | 7 +-
AUTHORS.txt | 1 +
CMakeLists.txt | 37 +-
Extras/HACD/hacdHACD.h | 22 +-
Extras/HACD/hacdVector.h | 2 +-
Extras/HACD/hacdVector.inl | 4 +-
Extras/InverseDynamics/CMakeLists.txt | 2 +
Extras/InverseDynamics/CloneTreeCreator.cpp | 49 +
Extras/InverseDynamics/CloneTreeCreator.hpp | 27 +
Extras/InverseDynamics/DillCreator.cpp | 2 +-
Extras/InverseDynamics/DillCreator.hpp | 2 +-
Extras/InverseDynamics/IDRandomUtil.cpp | 1 +
Extras/InverseDynamics/IDRandomUtil.hpp | 4 +-
Extras/InverseDynamics/RandomTreeCreator.cpp | 81 +
Extras/InverseDynamics/RandomTreeCreator.hpp | 31 +
Extras/InverseDynamics/btMultiBodyFromURDF.hpp | 12 +-
Extras/InverseDynamics/btMultiBodyTreeCreator.cpp | 2 +-
Extras/InverseDynamics/btMultiBodyTreeCreator.hpp | 8 +-
.../InverseDynamics/invdyn_bullet_comparison.hpp | 3 +-
.../BulletFileLoader/autogenerated/bullet.h | 180 +-
Extras/Serialize/BulletFileLoader/bDNA.cpp | 27 +-
Extras/Serialize/BulletFileLoader/bFile.cpp | 23 +-
Extras/Serialize/BulletFileLoader/btBulletFile.cpp | 2 +-
Extras/Serialize/HeaderGenerator/apiGen.cpp | 2 +-
Extras/Serialize/makesdna/makesdna.cpp | 27 +-
Extras/VHACD/inc/btAlignedAllocator.h | 104 +
Extras/VHACD/inc/btAlignedObjectArray.h | 448 +
Extras/VHACD/inc/btConvexHullComputer.h | 97 +
Extras/VHACD/inc/btMinMax.h | 65 +
Extras/VHACD/inc/btScalar.h | 532 ++
Extras/VHACD/inc/btVector3.h | 715 ++
Extras/VHACD/inc/vhacdCircularList.h | 79 +
Extras/VHACD/inc/vhacdCircularList.inl | 161 +
Extras/VHACD/inc/vhacdICHull.h | 98 +
Extras/VHACD/inc/vhacdManifoldMesh.h | 142 +
Extras/VHACD/inc/vhacdMesh.h | 129 +
Extras/VHACD/inc/vhacdMutex.h | 146 +
Extras/VHACD/inc/vhacdSArray.h | 158 +
Extras/VHACD/inc/vhacdTimer.h | 121 +
Extras/VHACD/inc/vhacdVHACD.h | 350 +
Extras/VHACD/inc/vhacdVector.h | 103 +
Extras/VHACD/inc/vhacdVector.inl | 362 +
Extras/VHACD/inc/vhacdVolume.h | 419 +
Extras/VHACD/premake4.lua | 4 +
Extras/VHACD/public/VHACD.h | 121 +
Extras/VHACD/src/VHACD.cpp | 1433 ++++
Extras/VHACD/src/btAlignedAllocator.cpp | 176 +
Extras/VHACD/src/btConvexHullComputer.cpp | 2475 ++++++
Extras/VHACD/src/premake4.lua | 10 +
Extras/VHACD/src/vhacdICHull.cpp | 725 ++
Extras/VHACD/src/vhacdManifoldMesh.cpp | 202 +
Extras/VHACD/src/vhacdMesh.cpp | 323 +
Extras/VHACD/src/vhacdVolume.cpp | 1617 ++++
.../hacdVector.h => VHACD/test/inc/oclHelper.h} | 81 +-
Extras/VHACD/test/src/main.cpp | 648 ++
Extras/VHACD/test/src/oclHelper.cpp | 329 +
Extras/VHACD/test/src/premake4.lua | 25 +
Extras/premake4.lua | 1 +
README.md | 40 +-
VERSION | 2 +-
appveyor.yml | 4 +-
build_and_run_cmake.sh | 7 +
build_and_run_premake.sh | 7 +
build_visual_studio.bat | 5 +
build_visual_studio_vr_pybullet_double.bat | 8 +
data/Quadrotor/quadrotor.urdf | 62 +
data/Quadrotor/quadrotor_base.obj | 1696 ++++
data/checker_blue.png | Bin 0 -> 6379 bytes
data/checker_grid.jpg | Bin 0 -> 32768 bytes
data/checker_huge.gif | Bin 0 -> 1865 bytes
data/cube.mtl | 4 +-
data/cube.urdf | 32 +
data/cube_gripper_left.urdf | 43 +
data/cube_gripper_right.urdf | 43 +
data/cube_no_friction.urdf | 32 +
data/cube_small.sdf | 35 +
data/cube_small.urdf | 30 +
data/cube_soft.urdf | 32 +
data/door.urdf | 105 +
data/{plane.mtl => duck.mtl} | 6 +-
data/duck.obj | 8604 ++++++++++++++++++++
data/duckCM.png | Bin 0 -> 32504 bytes
data/duck_vhacd.obj | 609 ++
data/duck_vhacd.urdf | 32 +
data/{cube.mtl => floor.mtl} | 7 +-
data/floor.obj | 18 +
data/floor_diffuse.jpg | Bin 0 -> 29586 bytes
data/floor_diffuse.tga | Bin 0 -> 326988 bytes
data/floor_nm_tangent.tga | Bin 0 -> 8236 bytes
data/gripper/meshes/GUIDE_WSG50_110.stl | Bin 0 -> 23884 bytes
data/gripper/meshes/WSG-FMF.stl | Bin 0 -> 108184 bytes
data/gripper/meshes/WSG50_110.stl | Bin 0 -> 20684 bytes
data/gripper/meshes/l_gripper_tip_scaled.stl | Bin 0 -> 4234 bytes
data/gripper/wsg50_one_motor_gripper.sdf | 388 +
data/gripper/wsg50_one_motor_gripper_free_base.sdf | 386 +
data/gripper/wsg50_one_motor_gripper_new.sdf | 386 +
.../wsg50_one_motor_gripper_new_free_base.sdf | 383 +
data/gripper/wsg50_with_r2d2_gripper.sdf | 298 +
data/husky/husky.urdf | 373 +
data/husky/meshes/bumper.stl | Bin 0 -> 23934 bytes
data/husky/meshes/top_plate.stl | Bin 0 -> 153984 bytes
data/husky/meshes/user_rail.stl | Bin 0 -> 115184 bytes
data/husky/meshes/wheel.stl | Bin 0 -> 354584 bytes
data/{cube.mtl => jenga/jenga.mtl} | 8 +-
data/jenga/jenga.obj | 113 +
data/jenga/jenga.png | Bin 0 -> 286602 bytes
data/jenga/jenga.urdf | 29 +
data/kiva_shelf/0_Bullet3Demo.txt | 7 +
data/kiva_shelf/meshes/pod_lowres.stl | Bin 0 -> 509284 bytes
data/kiva_shelf/model.sdf | 205 +
data/kuka_iiwa/kuka_world.sdf | 414 +
data/kuka_iiwa/meshes/link_0.stl | Bin 0 -> 151984 bytes
data/kuka_iiwa/meshes/link_1.stl | Bin 0 -> 138034 bytes
data/kuka_iiwa/meshes/link_2.stl | Bin 0 -> 72534 bytes
data/kuka_iiwa/meshes/link_3.stl | Bin 0 -> 96984 bytes
data/kuka_iiwa/meshes/link_4.stl | Bin 0 -> 77434 bytes
data/kuka_iiwa/meshes/link_5.stl | Bin 0 -> 67984 bytes
data/kuka_iiwa/meshes/link_6.stl | Bin 0 -> 57934 bytes
data/kuka_iiwa/meshes/link_7.stl | Bin 0 -> 75684 bytes
data/kuka_iiwa/model.sdf | 459 ++
data/kuka_iiwa/model.urdf | 289 +
data/kuka_iiwa/model2.sdf | 818 ++
data/kuka_iiwa/model_for_sdf.urdf | 285 +
data/kuka_iiwa/model_vr_limits.urdf | 289 +
data/l_finger_collision.stl | Bin 0 -> 22334 bytes
data/lego/lego.obj | 3751 +++++++++
data/lego/lego.urdf | 32 +
data/lego/lego_vhacd.obj | 3072 +++++++
data/multibody.bullet | Bin 14520 -> 14660 bytes
data/plane.mtl | 24 +-
data/plane.obj | 18 +-
data/plane.urdf | 26 +
data/plane100.obj | 22 +-
data/plane100.urdf | 26 +
data/pr2_gripper.urdf | 142 +
data/r2d2.urdf | 4 +-
data/r2d2_multibody.bullet | Bin 309384 -> 598292 bytes
data/samurai.urdf | 26 +
data/samurai_monastry.obj | 8 -
data/slope.bullet | Bin 79960 -> 76064 bytes
data/sphere2.urdf | 60 +-
data/sphere2_rolling_friction.urdf | 29 +
data/sphere_small.urdf | 30 +
data/{cube.mtl => table/table.mtl} | 8 +-
data/table/table.obj | 48 +
data/table/table.png | Bin 0 -> 867274 bytes
data/table/table.urdf | 56 +
data/table_square/checker_grid.jpg | Bin 0 -> 32768 bytes
data/{cube.mtl => table_square/table.mtl} | 8 +-
data/table_square/table.obj | 48 +
data/table_square/table_square.urdf | 61 +
data/teddy_vhacd.urdf | 32 +
data/textured_sphere.mtl | 11 +
data/textured_sphere_flat.obj | 3755 +++++++++
data/textured_sphere_smooth.mtl | 11 +
data/textured_sphere_smooth.obj | 3725 +++++++++
data/two_cubes.sdf | 240 +
examples/BasicDemo/BasicExample.cpp | 23 +-
examples/BasicDemo/CMakeLists.txt | 71 +
examples/BasicDemo/main.cpp | 6 +
examples/BasicDemo/premake4.lua | 181 +
examples/Benchmarks/BenchmarkDemo.cpp | 2 +-
examples/CMakeLists.txt | 6 +-
examples/Collision/CollisionTutorialBullet2.cpp | 7 +-
examples/CommonInterfaces/CommonCameraInterface.h | 5 +
examples/CommonInterfaces/CommonExampleInterface.h | 44 +-
.../CommonInterfaces/CommonGUIHelperInterface.h | 45 +-
.../CommonInterfaces/CommonGraphicsAppInterface.h | 15 +-
examples/CommonInterfaces/CommonMultiBodyBase.h | 16 +-
.../CommonInterfaces/CommonParameterInterface.h | 2 +
examples/CommonInterfaces/CommonRenderInterface.h | 15 +-
examples/CommonInterfaces/CommonRigidBodyBase.h | 10 +-
examples/CommonInterfaces/CommonWindowInterface.h | 4 +-
examples/ExampleBrowser/CMakeLists.txt | 246 +-
.../ExampleBrowser/CollisionShape2TriangleMesh.cpp | 193 +
.../ExampleBrowser/CollisionShape2TriangleMesh.h | 10 +
examples/ExampleBrowser/ExampleEntries.cpp | 180 +-
examples/ExampleBrowser/ExampleEntries.h | 21 +-
.../GwenGUISupport/GraphingTexture.cpp | 1 +
.../GwenGUISupport/GwenParameterInterface.cpp | 21 +-
.../GwenGUISupport/GwenProfileWindow.cpp | 25 +-
.../GwenGUISupport/gwenInternalData.h | 5 +-
.../GwenGUISupport/gwenUserInterface.cpp | 160 +-
.../GwenGUISupport/gwenUserInterface.h | 1 +
.../ExampleBrowser/InProcessExampleBrowser.cpp | 422 +
examples/ExampleBrowser/InProcessExampleBrowser.h | 33 +
examples/ExampleBrowser/OpenGLExampleBrowser.cpp | 466 +-
examples/ExampleBrowser/OpenGLExampleBrowser.h | 6 +
examples/ExampleBrowser/OpenGLGuiHelper.cpp | 417 +-
examples/ExampleBrowser/OpenGLGuiHelper.h | 16 +-
examples/ExampleBrowser/main.cpp | 64 +-
examples/ExampleBrowser/premake4.lua | 190 +-
.../Bridge.cpp} | 98 +-
.../Bridge.h} | 14 +-
.../Chain.cpp} | 87 +-
.../Chain.h} | 14 +-
examples/ExtendedTutorials/InclinedPlane.cpp | 372 +
.../InclinedPlane.h} | 50 +-
examples/ExtendedTutorials/MultiPendulum.cpp | 435 +
.../MultiPendulum.h} | 50 +-
.../MultipleBoxes.cpp} | 68 +-
.../MultipleBoxes.h} | 14 +-
examples/ExtendedTutorials/NewtonsCradle.cpp | 380 +
.../NewtonsCradle.h} | 50 +-
examples/ExtendedTutorials/NewtonsRopeCradle.cpp | 387 +
.../NewtonsRopeCradle.h} | 50 +-
examples/ExtendedTutorials/RigidBodyFromObj.cpp | 165 +
.../RigidBodyFromObj.h} | 16 +-
.../SimpleBox.cpp} | 64 +-
.../SimpleBox.h} | 14 +-
examples/ExtendedTutorials/SimpleCloth.cpp | 162 +
.../SimpleCloth.h} | 14 +-
.../SimpleJoint.cpp} | 79 +-
.../SimpleJoint.h} | 14 +-
examples/ExtendedTutorials/premake4.lua | 224 +
examples/ForkLift/ForkLiftDemo.cpp | 1 +
examples/HelloWorld/HelloWorld.cpp | 24 +-
examples/Importers/ImportBsp/ImportBspExample.cpp | 2 +-
.../ImportColladaDemo/LoadMeshFromCollada.cpp | 100 +-
.../ImportColladaDemo/LoadMeshFromCollada.h | 2 +-
.../ImportMeshUtility/b3ImportMeshUtility.cpp | 88 +
.../ImportMeshUtility/b3ImportMeshUtility.h | 25 +
.../Importers/ImportObjDemo/ImportObjExample.cpp | 137 +-
.../Importers/ImportObjDemo/LoadMeshFromObj.cpp | 4 +-
.../Wavefront2GLInstanceGraphicsShape.cpp | 122 +-
.../Wavefront2GLInstanceGraphicsShape.h | 4 +-
.../ImportSDFSetup.cpp} | 166 +-
examples/Importers/ImportSDFDemo/ImportSDFSetup.h | 8 +
.../Importers/ImportSTLDemo/ImportSTLSetup.cpp | 31 +-
examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h | 23 +-
.../ImportURDFDemo/BulletUrdfImporter.cpp | 1279 +--
.../Importers/ImportURDFDemo/BulletUrdfImporter.h | 29 +-
.../Importers/ImportURDFDemo/ImportURDFSetup.cpp | 82 +-
.../ImportURDFDemo/LinkVisualShapesConverter.h | 9 +
.../ImportURDFDemo/MyMultiBodyCreator.cpp | 9 +-
.../Importers/ImportURDFDemo/MyMultiBodyCreator.h | 5 +
.../Importers/ImportURDFDemo/ROSURDFImporter.cpp | 899 --
.../Importers/ImportURDFDemo/ROSURDFImporter.h | 45 -
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp | 150 +-
examples/Importers/ImportURDFDemo/URDF2Bullet.h | 18 +-
.../ImportURDFDemo/URDFImporterInterface.h | 20 +-
examples/Importers/ImportURDFDemo/URDFJointTypes.h | 40 +
examples/Importers/ImportURDFDemo/UrdfParser.cpp | 1286 ++-
examples/Importers/ImportURDFDemo/UrdfParser.h | 111 +-
.../Importers/ImportURDFDemo/urdfStringSplit.cpp | 9 +-
.../Importers/ImportURDFDemo/urdfStringSplit.h | 4 +-
.../InverseDynamics/InverseDynamicsExample.cpp | 94 +-
examples/InverseDynamics/InverseDynamicsExample.h | 4 +-
examples/InverseDynamics/premake4.lua | 221 +
.../InverseKinematics/InverseKinematicsExample.cpp | 385 +
.../InverseKinematics/InverseKinematicsExample.h | 8 +
examples/LuaDemo/LuaPhysicsSetup.cpp | 21 +-
examples/MultiBody/MultiBodySoftContact.cpp | 173 +
examples/MultiBody/MultiBodySoftContact.h | 7 +
examples/MultiBody/MultiDofDemo.cpp | 23 +-
examples/MultiBody/TestJointTorqueSetup.cpp | 3 +-
examples/MultiThreading/MultiThreadingExample.cpp | 4 +-
examples/MultiThreading/b3PosixThreadSupport.cpp | 26 +-
examples/MultiThreading/b3PosixThreadSupport.h | 12 +-
examples/MultiThreading/b3Win32ThreadSupport.cpp | 35 +-
examples/MultiThreading/b3Win32ThreadSupport.h | 4 +-
examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp | 2 +-
examples/OpenGLWindow/CMakeLists.txt | 2 +-
examples/OpenGLWindow/GLInstanceGraphicsShape.h | 13 +
examples/OpenGLWindow/GLInstancingRenderer.cpp | 287 +-
examples/OpenGLWindow/GLInstancingRenderer.h | 17 +-
examples/OpenGLWindow/GLPrimitiveRenderer.cpp | 7 +-
examples/OpenGLWindow/MacOpenGLWindow.h | 3 +
examples/OpenGLWindow/MacOpenGLWindow.mm | 45 +-
examples/OpenGLWindow/Shaders/instancingPS.glsl | 3 +-
examples/OpenGLWindow/Shaders/instancingPS.h | 3 +-
examples/OpenGLWindow/Shaders/instancingVS.glsl | 2 +-
examples/OpenGLWindow/Shaders/instancingVS.h | 2 +-
.../Shaders/useShadowMapInstancingPS.glsl | 9 +-
.../Shaders/useShadowMapInstancingPS.h | 8 +-
.../Shaders/useShadowMapInstancingVS.glsl | 2 +-
.../Shaders/useShadowMapInstancingVS.h | 2 +-
examples/OpenGLWindow/SimpleCamera.cpp | 76 +-
examples/OpenGLWindow/SimpleCamera.h | 6 +
examples/OpenGLWindow/SimpleOpenGL2App.h | 1 +
examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp | 14 +
examples/OpenGLWindow/SimpleOpenGL2Renderer.h | 9 +
examples/OpenGLWindow/SimpleOpenGL3App.cpp | 100 +-
examples/OpenGLWindow/SimpleOpenGL3App.h | 2 +
examples/OpenGLWindow/TwFonts.cpp | 35 +-
examples/OpenGLWindow/Win32OpenGLWindow.cpp | 18 +
examples/OpenGLWindow/Win32OpenGLWindow.h | 3 +
examples/OpenGLWindow/Win32Window.cpp | 15 +-
examples/OpenGLWindow/X11OpenGLWindow.cpp | 36 +-
examples/OpenGLWindow/X11OpenGLWindow.h | 7 +-
.../OpenGLWindow/opengl_fontstashcallbacks.cpp | 5 +-
.../RenderingExamples/CoordinateSystemDemo.cpp | 2 +-
.../RenderingExamples/DynamicTexturedCubeDemo.cpp | 152 +
.../RenderingExamples/DynamicTexturedCubeDemo.h | 6 +
examples/RenderingExamples/RaytracerSetup.cpp | 18 +-
examples/RenderingExamples/TimeSeriesCanvas.cpp | 38 +-
examples/RenderingExamples/TimeSeriesCanvas.h | 6 +-
examples/RenderingExamples/TinyRendererSetup.cpp | 413 +
examples/RenderingExamples/TinyRendererSetup.h | 6 +
examples/RenderingExamples/TinyVRGui.cpp | 218 +
examples/RenderingExamples/TinyVRGui.h | 25 +
.../RigidBodySoftContact.cpp} | 87 +-
.../RigidBodySoftContact.h} | 14 +-
examples/RoboticsLearning/GripperGraspExample.cpp | 470 ++
.../GripperGraspExample.h} | 17 +-
examples/RoboticsLearning/KukaGraspExample.cpp | 319 +
.../KukaGraspExample.h} | 15 +-
examples/RoboticsLearning/R2D2GraspExample.cpp | 233 +
.../R2D2GraspExample.h} | 17 +-
examples/RoboticsLearning/b3RobotSimAPI.cpp | 999 +++
examples/RoboticsLearning/b3RobotSimAPI.h | 169 +
.../RollingFrictionDemo/RollingFrictionDemo.cpp | 3 +-
examples/SharedMemory/BodyJointInfoUtility.h | 2 +
examples/SharedMemory/IKTrajectoryHelper.cpp | 206 +
examples/SharedMemory/IKTrajectoryHelper.h | 36 +
examples/SharedMemory/InProcessMemory.cpp | 49 +
examples/SharedMemory/InProcessMemory.h | 19 +
examples/SharedMemory/PhysicsClient.h | 15 +-
examples/SharedMemory/PhysicsClientC_API.cpp | 872 +-
examples/SharedMemory/PhysicsClientC_API.h | 129 +-
examples/SharedMemory/PhysicsClientExample.cpp | 597 +-
examples/SharedMemory/PhysicsClientExample.h | 7 +
.../SharedMemory/PhysicsClientSharedMemory.cpp | 380 +-
examples/SharedMemory/PhysicsClientSharedMemory.h | 17 +-
examples/SharedMemory/PhysicsDirect.cpp | 326 +-
examples/SharedMemory/PhysicsDirect.h | 28 +-
examples/SharedMemory/PhysicsLoopBack.cpp | 31 +-
examples/SharedMemory/PhysicsLoopBack.h | 11 +-
.../SharedMemory/PhysicsServerCommandProcessor.cpp | 2311 +++++-
.../SharedMemory/PhysicsServerCommandProcessor.h | 12 +-
examples/SharedMemory/PhysicsServerExample.cpp | 1109 ++-
examples/SharedMemory/PhysicsServerExample.h | 1 +
.../SharedMemory/PhysicsServerSharedMemory.cpp | 57 +-
examples/SharedMemory/PhysicsServerSharedMemory.h | 5 +-
examples/SharedMemory/PosixSharedMemory.cpp | 2 +-
examples/SharedMemory/RobotControlExample.cpp | 1 -
examples/SharedMemory/SharedMemoryBlock.h | 1 -
examples/SharedMemory/SharedMemoryCommands.h | 215 +-
.../SharedMemoryInProcessPhysicsC_API.cpp | 105 +
.../SharedMemoryInProcessPhysicsC_API.h | 24 +
examples/SharedMemory/SharedMemoryPublic.h | 124 +-
.../TinyRendererVisualShapeConverter.cpp | 731 ++
.../TinyRendererVisualShapeConverter.h | 39 +
examples/SharedMemory/main.cpp | 3 +
examples/SharedMemory/premake4.lua | 250 +-
examples/SimpleOpenGL3/main.cpp | 172 +-
examples/StandaloneMain/hellovr_opengl_main.cpp | 2223 +++++
.../main_console_single_example.cpp} | 35 +-
.../StandaloneMain/main_opengl_single_example.cpp | 116 +
.../main_sw_tinyrenderer_single_example.cpp | 298 +
.../main_tinyrenderer_single_example.cpp | 409 +
.../LICENSE.txt} | 23 +-
examples/TinyRenderer/TinyRenderer.cpp | 295 +
examples/TinyRenderer/TinyRenderer.h | 56 +
examples/TinyRenderer/geometry.cpp | 7 +
examples/TinyRenderer/geometry.h | 222 +
examples/TinyRenderer/main.cpp | 237 +
examples/TinyRenderer/model.cpp | 171 +
examples/TinyRenderer/model.h | 54 +
examples/TinyRenderer/our_gl.cpp | 117 +
examples/TinyRenderer/our_gl.h | 22 +
examples/TinyRenderer/premake4.lua | 29 +
examples/TinyRenderer/tgaimage.cpp | 356 +
examples/TinyRenderer/tgaimage.h | 101 +
examples/Tutorial/Tutorial.cpp | 2 +-
examples/Utils/b3Clock.cpp | 35 +-
examples/Utils/b3Clock.h | 11 +-
examples/Utils/b3ResourcePath.cpp | 12 +-
examples/Vehicles/Hinge2Vehicle.cpp | 5 -
examples/pybullet/CMakeLists.txt | 88 +
examples/pybullet/premake4.lua | 107 +
examples/pybullet/pybullet.c | 2288 ++++++
examples/pybullet/robotcontrol.py | 30 +
examples/pybullet/saveWorld.py | 8 +
examples/pybullet/test.py | 36 +
examples/pybullet/testrender.py | 46 +
examples/pybullet/testrender_np.py | 50 +
.../BroadPhaseCollision/b3DynamicBvh.h | 2 -
.../BroadPhaseCollision/b3DynamicBvhBroadphase.h | 2 +-
.../NarrowPhaseCollision/shared/b3MprPenetration.h | 15 +-
src/Bullet3Common/b3CommandLineArgs.h | 22 +-
src/Bullet3Common/b3Scalar.h | 2 +-
src/Bullet3Common/shared/b3PlatformDefinitions.h | 4 +
src/Bullet3OpenCL/CMakeLists.txt | 2 +-
src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp | 10 +-
.../NarrowphaseCollision/b3ConvexHullContact.cpp | 7 +-
.../NarrowphaseCollision/b3GjkEpa.cpp | 6 +-
.../NarrowphaseCollision/b3QuantizedBvh.cpp | 2 +-
.../NarrowphaseCollision/kernels/mprKernels.h | 24 +-
.../kernels/primitiveContacts.h | 1 +
.../kernels/satClipHullContacts.h | 1 +
.../kernels/satConcaveKernels.h | 1 +
.../NarrowphaseCollision/kernels/satKernels.h | 1 +
.../RigidBody/b3GpuPgsConstraintSolver.cpp | 2 +-
.../RigidBody/kernels/batchingKernels.h | 1 +
.../RigidBody/kernels/batchingKernelsNew.h | 1 +
.../RigidBody/kernels/integrateKernel.h | 1 +
src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl | 4 +-
src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h | 4 +-
src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h | 1 +
.../RigidBody/kernels/solverSetup2.cl | 4 +-
src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h | 5 +-
src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl | 4 +-
src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h | 5 +-
.../RigidBody/kernels/updateAabbsKernel.h | 27 +-
.../Bullet2FileLoader/b3BulletFile.cpp | 4 +-
src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp | 22 +-
src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp | 24 +-
src/BulletCollision/BroadphaseCollision/btDbvt.h | 93 +-
.../BroadphaseCollision/btOverlappingPairCache.cpp | 4 +-
.../BroadphaseCollision/btQuantizedBvh.cpp | 2 +
.../CollisionDispatch/btCollisionDispatcher.cpp | 5 +-
.../CollisionDispatch/btCollisionObject.cpp | 6 +
.../CollisionDispatch/btCollisionObject.h | 68 +-
.../CollisionDispatch/btCollisionWorld.cpp | 128 +-
.../btCompoundCollisionAlgorithm.cpp | 6 +-
.../btCompoundCollisionAlgorithm.h | 4 +
.../btCompoundCompoundCollisionAlgorithm.cpp | 2 +
.../btConvexConcaveCollisionAlgorithm.cpp | 5 +-
.../btConvexConcaveCollisionAlgorithm.h | 18 +-
.../CollisionDispatch/btConvexConvexAlgorithm.cpp | 7 +-
.../CollisionDispatch/btConvexConvexAlgorithm.h | 3 +
.../btDefaultCollisionConfiguration.cpp | 1 +
.../CollisionDispatch/btManifoldResult.cpp | 42 +-
.../CollisionDispatch/btManifoldResult.h | 4 +
.../CollisionShapes/btBvhTriangleMeshShape.cpp | 9 +-
src/BulletCollision/CollisionShapes/btConeShape.h | 9 +
.../CollisionShapes/btConvexHullShape.cpp | 17 +-
.../CollisionShapes/btConvexHullShape.h | 5 +-
.../CollisionShapes/btMultiSphereShape.cpp | 4 +-
.../NarrowPhaseCollision/btGjkEpa2.cpp | 39 +-
.../NarrowPhaseCollision/btGjkPairDetector.cpp | 10 +-
.../NarrowPhaseCollision/btManifoldPoint.h | 55 +-
.../NarrowPhaseCollision/btPersistentManifold.h | 3 +-
.../btPolyhedralContactClipping.cpp | 10 +-
.../btPolyhedralContactClipping.h | 7 +-
src/BulletDynamics/CMakeLists.txt | 4 +
.../Character/btCharacterControllerInterface.h | 2 +-
.../Character/btKinematicCharacterController.cpp | 472 +-
.../Character/btKinematicCharacterController.h | 60 +-
.../ConstraintSolver/btContactSolverInfo.h | 2 +-
.../btGeneric6DofSpring2Constraint.h | 16 +-
.../btSequentialImpulseConstraintSolver.cpp | 98 +-
.../btSequentialImpulseConstraintSolver.h | 6 +-
.../ConstraintSolver/btSliderConstraint.h | 2 +
.../ConstraintSolver/btTypedConstraint.h | 5 -
src/BulletDynamics/Dynamics/btRigidBody.cpp | 2 +
src/BulletDynamics/Dynamics/btRigidBody.h | 3 +
src/BulletDynamics/Featherstone/btMultiBody.cpp | 92 +-
src/BulletDynamics/Featherstone/btMultiBody.h | 91 +-
.../Featherstone/btMultiBodyConstraint.cpp | 668 +-
.../Featherstone/btMultiBodyConstraint.h | 13 +-
.../Featherstone/btMultiBodyConstraintSolver.cpp | 433 +-
.../Featherstone/btMultiBodyConstraintSolver.h | 15 +-
.../Featherstone/btMultiBodyDynamicsWorld.cpp | 55 +-
.../Featherstone/btMultiBodyDynamicsWorld.h | 10 +
.../Featherstone/btMultiBodyFixedConstraint.cpp | 211 +
.../Featherstone/btMultiBodyFixedConstraint.h | 94 +
.../btMultiBodyJointLimitConstraint.cpp | 2 +-
.../Featherstone/btMultiBodyJointMotor.cpp | 34 +-
.../Featherstone/btMultiBodyJointMotor.h | 28 +-
src/BulletDynamics/Featherstone/btMultiBodyLink.h | 11 +-
.../Featherstone/btMultiBodyPoint2Point.cpp | 2 +-
.../Featherstone/btMultiBodySliderConstraint.cpp | 230 +
.../Featherstone/btMultiBodySliderConstraint.h | 105 +
src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp | 7 +-
src/BulletDynamics/MLCPSolvers/btMLCPSolver.h | 10 -
src/BulletInverseDynamics/IDConfig.hpp | 15 +-
src/BulletInverseDynamics/IDConfigEigen.hpp | 15 +-
src/BulletInverseDynamics/IDMath.cpp | 54 +
src/BulletInverseDynamics/IDMath.hpp | 10 +
src/BulletInverseDynamics/MultiBodyTree.cpp | 114 +
src/BulletInverseDynamics/MultiBodyTree.hpp | 61 +-
.../details/IDEigenInterface.hpp | 37 +-
.../details/IDLinearMathInterface.hpp | 62 +-
src/BulletInverseDynamics/details/IDMatVec.hpp | 93 +-
.../details/MultiBodyTreeImpl.cpp | 464 +-
.../details/MultiBodyTreeImpl.hpp | 47 +
src/LinearMath/btAlignedAllocator.cpp | 114 +-
src/LinearMath/btAlignedAllocator.h | 8 +-
src/LinearMath/btHashMap.h | 13 +
src/LinearMath/btIDebugDraw.h | 6 +-
src/LinearMath/btMatrix3x3.h | 7 +-
src/LinearMath/btPolarDecomposition.cpp | 3 +-
src/LinearMath/btPolarDecomposition.h | 7 +-
src/LinearMath/btQuaternion.h | 45 +-
src/LinearMath/btQuickprof.cpp | 8 +-
src/LinearMath/btQuickprof.h | 25 +-
src/LinearMath/btScalar.h | 7 +-
src/LinearMath/btSerializer.cpp | 1613 ++--
src/LinearMath/btSerializer.h | 2 +-
src/LinearMath/btVector3.h | 12 +-
test/CMakeLists.txt | 4 +-
test/InverseDynamics/CMakeLists.txt | 108 +-
test/InverseDynamics/premake4.lua | 40 +-
test/InverseDynamics/test_invdyn_bullet.cpp | 10 +-
test/InverseDynamics/test_invdyn_jacobian.cpp | 326 +
test/SharedMemory/CMakeLists.txt | 87 +
test/SharedMemory/gtestwrap.cpp | 23 +
test/SharedMemory/premake4.lua | 142 +-
test/SharedMemory/test.c | 172 +-
xcode.command | 5 +
502 files changed, 76437 insertions(+), 6544 deletions(-)
diff --git a/.travis.yml b/.travis.yml
index bb6dcde..d2ad176 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -5,10 +5,15 @@ os:
compiler:
- gcc
- clang
+addons:
+ apt:
+ packages:
+ - python3
+
script:
- echo "CXX="$CXX
- echo "CC="$CC
- - cmake . -G "Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror
+ - cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror
- make -j8
- ctest -j8 --output-on-failure
# Build again with double precision
diff --git a/AUTHORS.txt b/AUTHORS.txt
index 8e5b831..556e6f6 100644
--- a/AUTHORS.txt
+++ b/AUTHORS.txt
@@ -35,5 +35,6 @@ Marten Svanfeldt
Pierre Terdiman
Steven Thompson
Tamas Umenhoffer
+Yunfei Bai
If your name is missing, please send an email to erwin.coumans at gmail.com or file an issue at http://github.com/bulletphysics/bullet3
diff --git a/CMakeLists.txt b/CMakeLists.txt
index cf65308..41a4ba9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -5,10 +5,14 @@ set(CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS true)
SET(MSVC_INCREMENTAL_DEFAULT ON)
PROJECT(BULLET_PHYSICS)
-SET(BULLET_VERSION 2.83)
+SET(BULLET_VERSION 2.85)
IF(COMMAND cmake_policy)
cmake_policy(SET CMP0003 NEW)
+ if(POLICY CMP0042)
+ # Enable MACOSX_RPATH by default.
+ cmake_policy(SET CMP0042 NEW)
+ endif(POLICY CMP0042)
ENDIF(COMMAND cmake_policy)
@@ -87,6 +91,8 @@ IF(MSVC)
IF (USE_MSVC_FAST_FLOATINGPOINT)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /fp:fast")
ENDIF()
+
+ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd4244 /wd4267")
ENDIF(MSVC)
@@ -192,6 +198,35 @@ ENDIF()
OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON)
+OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF)
+
+
+IF(BUILD_PYBULLET)
+
+ FIND_PACKAGE(PythonLibs)
+
+ OPTION(BUILD_PYBULLET_NUMPY "Set when you want to build pybullet with NumPy support" OFF)
+
+ IF(BUILD_PYBULLET_NUMPY)
+ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_LIST_DIR}/build3/cmake)
+ #include(FindNumPy)
+ FIND_PACKAGE(NumPy)
+ if (PYTHON_NUMPY_FOUND)
+ message("NumPy found")
+ add_definitions(-DPYBULLET_USE_NUMPY)
+ else()
+ message("NumPy not found")
+ endif()
+ ENDIF()
+ OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF)
+
+ IF(WIN32)
+ SET(BUILD_SHARED_LIBS OFF CACHE BOOL "Shared Libs" FORCE)
+ ELSE(WIN32)
+ SET(BUILD_SHARED_LIBS ON CACHE BOOL "Shared Libs" FORCE)
+ ENDIF(WIN32)
+ENDIF(BUILD_PYBULLET)
+
IF(BUILD_BULLET3)
IF(APPLE)
MESSAGE("Mac OSX Version is ${_CURRENT_OSX_VERSION}")
diff --git a/Extras/HACD/hacdHACD.h b/Extras/HACD/hacdHACD.h
index 9c496f4..0b2415b 100644
--- a/Extras/HACD/hacdHACD.h
+++ b/Extras/HACD/hacdHACD.h
@@ -81,38 +81,38 @@ namespace HACD
//! Gives the triangles partitionas an array of size m_nTriangles where the i-th element specifies the cluster to which belong the i-th triangle
//! @return triangles partition
- const long * const GetPartition() const { return m_partition;}
+ const long * GetPartition() const { return m_partition;}
//! Sets the scale factor
//! @param scale scale factor
void SetScaleFactor(double scale) { m_scale = scale;}
//! Gives the scale factor
//! @return scale factor
- const double GetScaleFactor() const { return m_scale;}
+ double GetScaleFactor() const { return m_scale;}
//! Sets the call-back function
//! @param callBack pointer to the call-back function
void SetCallBack(CallBackFunction callBack) { m_callBack = callBack;}
//! Gives the call-back function
//! @return pointer to the call-back function
- const CallBackFunction GetCallBack() const { return m_callBack;}
-
+ CallBackFunction GetCallBack() const { return m_callBack;}
+
//! Specifies whether faces points should be added when computing the concavity
//! @param addFacesPoints true = faces points should be added
void SetAddFacesPoints(bool addFacesPoints) { m_addFacesPoints = addFacesPoints;}
//! Specifies wheter faces points should be added when computing the concavity
//! @return true = faces points should be added
- const bool GetAddFacesPoints() const { return m_addFacesPoints;}
+ bool GetAddFacesPoints() const { return m_addFacesPoints;}
//! Specifies whether extra points should be added when computing the concavity
//! @param addExteraDistPoints true = extra points should be added
void SetAddExtraDistPoints(bool addExtraDistPoints) { m_addExtraDistPoints = addExtraDistPoints;}
//! Specifies wheter extra points should be added when computing the concavity
//! @return true = extra points should be added
- const bool GetAddExtraDistPoints() const { return m_addExtraDistPoints;}
+ bool GetAddExtraDistPoints() const { return m_addExtraDistPoints;}
//! Specifies whether extra points should be added when computing the concavity
//! @param addExteraDistPoints true = extra points should be added
void SetAddNeighboursDistPoints(bool addNeighboursDistPoints) { m_addNeighboursDistPoints = addNeighboursDistPoints;}
//! Specifies wheter extra points should be added when computing the concavity
//! @return true = extra points should be added
- const bool GetAddNeighboursDistPoints() const { return m_addNeighboursDistPoints;}
+ bool GetAddNeighboursDistPoints() const { return m_addNeighboursDistPoints;}
//! Sets the points of the input mesh (Remark: the input points will be scaled and shifted. Use DenormalizeData() to invert those operations)
//! @param points pointer to the input points
void SetPoints(Vec3<Real> * points) { m_points = points;}
@@ -130,19 +130,19 @@ namespace HACD
void SetNPoints(size_t nPoints) { m_nPoints = nPoints;}
//! Gives the number of points in the input mesh.
//! @return number of points the input mesh
- const size_t GetNPoints() const { return m_nPoints;}
+ size_t GetNPoints() const { return m_nPoints;}
//! Sets the number of triangles in the input mesh.
//! @param nTriangles number of triangles in the input mesh
void SetNTriangles(size_t nTriangles) { m_nTriangles = nTriangles;}
//! Gives the number of triangles in the input mesh.
//! @return number of triangles the input mesh
- const size_t GetNTriangles() const { return m_nTriangles;}
+ size_t GetNTriangles() const { return m_nTriangles;}
//! Sets the minimum number of clusters to be generated.
//! @param nClusters minimum number of clusters
void SetNClusters(size_t nClusters) { m_nMinClusters = nClusters;}
//! Gives the number of generated clusters.
//! @return number of generated clusters
- const size_t GetNClusters() const { return m_nClusters;}
+ size_t GetNClusters() const { return m_nClusters;}
//! Sets the maximum allowed concavity.
//! @param concavity maximum concavity
void SetConcavity(double concavity) { m_concavity = concavity;}
@@ -172,7 +172,7 @@ namespace HACD
void SetNVerticesPerCH(size_t nVerticesPerCH) { m_nVerticesPerCH = nVerticesPerCH;}
//! Gives the maximum number of vertices for each generated convex-hull.
//! @return maximum # vertices per CH
- const size_t GetNVerticesPerCH() const { return m_nVerticesPerCH;}
+ size_t GetNVerticesPerCH() const { return m_nVerticesPerCH;}
//! Gives the number of vertices for the cluster number numCH.
//! @return number of vertices
size_t GetNPointsCH(size_t numCH) const;
diff --git a/Extras/HACD/hacdVector.h b/Extras/HACD/hacdVector.h
index 0c8dd02..bb0858b 100644
--- a/Extras/HACD/hacdVector.h
+++ b/Extras/HACD/hacdVector.h
@@ -58,7 +58,7 @@ namespace HACD
T m_data[3];
};
template<typename T>
- const bool Colinear(const Vec3<T> & a, const Vec3<T> & b, const Vec3<T> & c);
+ bool Colinear(const Vec3<T> & a, const Vec3<T> & b, const Vec3<T> & c);
template<typename T>
const T Volume(const Vec3<T> & a, const Vec3<T> & b, const Vec3<T> & c, const Vec3<T> & d);
diff --git a/Extras/HACD/hacdVector.inl b/Extras/HACD/hacdVector.inl
index ab0a94d..b1f0c1b 100644
--- a/Extras/HACD/hacdVector.inl
+++ b/Extras/HACD/hacdVector.inl
@@ -162,7 +162,7 @@ namespace HACD
inline Vec3<T>::Vec3() {}
template<typename T>
- inline const bool Colinear(const Vec3<T> & a, const Vec3<T> & b, const Vec3<T> & c)
+ inline bool Colinear(const Vec3<T> & a, const Vec3<T> & b, const Vec3<T> & c)
{
return ((c.Z() - a.Z()) * (b.Y() - a.Y()) - (b.Z() - a.Z()) * (c.Y() - a.Y()) == 0.0 /*EPS*/) &&
((b.Z() - a.Z()) * (c.X() - a.X()) - (b.X() - a.X()) * (c.Z() - a.Z()) == 0.0 /*EPS*/) &&
@@ -175,4 +175,4 @@ namespace HACD
return (a-d) * ((b-d) ^ (c-d));
}
}
-#endif //HACD_VECTOR_INL
\ No newline at end of file
+#endif //HACD_VECTOR_INL
diff --git a/Extras/InverseDynamics/CMakeLists.txt b/Extras/InverseDynamics/CMakeLists.txt
index 56ed7d7..1e2031f 100644
--- a/Extras/InverseDynamics/CMakeLists.txt
+++ b/Extras/InverseDynamics/CMakeLists.txt
@@ -4,6 +4,7 @@ INCLUDE_DIRECTORIES(
ADD_LIBRARY(
BulletInverseDynamicsUtils
+CloneTreeCreator.cpp
CoilCreator.cpp
MultiBodyTreeCreator.cpp
btMultiBodyTreeCreator.cpp
@@ -11,6 +12,7 @@ DillCreator.cpp
MultiBodyTreeDebugGraph.cpp
invdyn_bullet_comparison.cpp
IDRandomUtil.cpp
+RandomTreeCreator.cpp
SimpleTreeCreator.cpp
MultiBodyNameMap.cpp
User2InternalIndex.cpp
diff --git a/Extras/InverseDynamics/CloneTreeCreator.cpp b/Extras/InverseDynamics/CloneTreeCreator.cpp
new file mode 100644
index 0000000..899b5d9
--- /dev/null
+++ b/Extras/InverseDynamics/CloneTreeCreator.cpp
@@ -0,0 +1,49 @@
+#include "CloneTreeCreator.hpp"
+
+#include <cstdio>
+
+namespace btInverseDynamics {
+#define CHECK_NULLPTR() \
+ do { \
+ if (m_reference == 0x0) { \
+ error_message("m_reference == 0x0\n"); \
+ return -1; \
+ } \
+ } while (0)
+
+#define TRY(x) \
+ do { \
+ if (x == -1) { \
+ error_message("error calling " #x "\n"); \
+ return -1; \
+ } \
+ } while (0)
+CloneTreeCreator::CloneTreeCreator(const MultiBodyTree* reference) { m_reference = reference; }
+
+CloneTreeCreator::~CloneTreeCreator() {}
+
+int CloneTreeCreator::getNumBodies(int* num_bodies) const {
+ CHECK_NULLPTR();
+ *num_bodies = m_reference->numBodies();
+ return 0;
+}
+
+int CloneTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
+ vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
+ vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
+ mat33* body_I_body, int* user_int, void** user_ptr) const {
+ CHECK_NULLPTR();
+ TRY(m_reference->getParentIndex(body_index, parent_index));
+ TRY(m_reference->getJointType(body_index, joint_type));
+ TRY(m_reference->getParentRParentBodyRef(body_index, parent_r_parent_body_ref));
+ TRY(m_reference->getBodyTParentRef(body_index, body_T_parent_ref));
+ TRY(m_reference->getBodyAxisOfMotion(body_index, body_axis_of_motion));
+ TRY(m_reference->getBodyMass(body_index, mass));
+ TRY(m_reference->getBodyFirstMassMoment(body_index, body_r_body_com));
+ TRY(m_reference->getBodySecondMassMoment(body_index, body_I_body));
+ TRY(m_reference->getUserInt(body_index, user_int));
+ TRY(m_reference->getUserPtr(body_index, user_ptr));
+
+ return 0;
+}
+}
diff --git a/Extras/InverseDynamics/CloneTreeCreator.hpp b/Extras/InverseDynamics/CloneTreeCreator.hpp
new file mode 100644
index 0000000..cb96d56
--- /dev/null
+++ b/Extras/InverseDynamics/CloneTreeCreator.hpp
@@ -0,0 +1,27 @@
+#ifndef CLONETREE_CREATOR_HPP_
+#define CLONETREE_CREATOR_HPP_
+
+#include "BulletInverseDynamics/IDConfig.hpp"
+#include "MultiBodyTreeCreator.hpp"
+
+namespace btInverseDynamics {
+/// Generate an identical multibody tree from a reference system.
+class CloneTreeCreator : public MultiBodyTreeCreator {
+public:
+ /// ctor
+ /// @param reference the MultiBodyTree to clone
+ CloneTreeCreator(const MultiBodyTree*reference);
+ ~CloneTreeCreator();
+ ///\copydoc MultiBodyTreeCreator::getNumBodies
+ int getNumBodies(int* num_bodies) const;
+ ///\copydoc MultiBodyTreeCreator::getBody
+ int getBody(const int body_index, int* parent_index, JointType* joint_type,
+ vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
+ idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
+ void** user_ptr) const;
+
+private:
+ const MultiBodyTree *m_reference;
+};
+}
+#endif // CLONETREE_CREATOR_HPP_
diff --git a/Extras/InverseDynamics/DillCreator.cpp b/Extras/InverseDynamics/DillCreator.cpp
index a1ddf00..46f68e6 100644
--- a/Extras/InverseDynamics/DillCreator.cpp
+++ b/Extras/InverseDynamics/DillCreator.cpp
@@ -44,7 +44,7 @@ int DillCreator::getNumBodies(int* num_bodies) const {
return 0;
}
-int DillCreator::getBody(int body_index, int* parent_index, JointType* joint_type,
+int DillCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
mat33* body_I_body, int* user_int, void** user_ptr) const {
diff --git a/Extras/InverseDynamics/DillCreator.hpp b/Extras/InverseDynamics/DillCreator.hpp
index d52e0d0..fbe0e8e 100644
--- a/Extras/InverseDynamics/DillCreator.hpp
+++ b/Extras/InverseDynamics/DillCreator.hpp
@@ -22,7 +22,7 @@ public:
///\copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int* num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
- int getBody(int body_index, int* parent_index, JointType* joint_type,
+ int getBody(const int body_index, int* parent_index, JointType* joint_type,
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
void** user_ptr) const;
diff --git a/Extras/InverseDynamics/IDRandomUtil.cpp b/Extras/InverseDynamics/IDRandomUtil.cpp
index c3110e5..fcf8795 100644
--- a/Extras/InverseDynamics/IDRandomUtil.cpp
+++ b/Extras/InverseDynamics/IDRandomUtil.cpp
@@ -15,6 +15,7 @@ static const float mass_min = 0.001;
static const float mass_max = 1.0;
void randomInit() { srand(time(NULL)); }
+void randomInit(unsigned seed) { srand(seed); }
int randomInt(int low, int high) { return rand() % (high + 1 - low) + low; }
diff --git a/Extras/InverseDynamics/IDRandomUtil.hpp b/Extras/InverseDynamics/IDRandomUtil.hpp
index a9d363a..72a97fa 100644
--- a/Extras/InverseDynamics/IDRandomUtil.hpp
+++ b/Extras/InverseDynamics/IDRandomUtil.hpp
@@ -2,8 +2,10 @@
#define ID_RANDOM_UTIL_HPP_
#include "BulletInverseDynamics/IDConfig.hpp"
namespace btInverseDynamics {
-/// seed random number generator
+/// seed random number generator using time()
void randomInit();
+/// seed random number generator with identical value to get repeatable results
+void randomInit(unsigned seed);
/// Generate (not quite) uniformly distributed random integers in [low, high]
/// Note: this is a low-quality implementation using only rand(), as
/// C++11 <random> is not supported in bullet.
diff --git a/Extras/InverseDynamics/RandomTreeCreator.cpp b/Extras/InverseDynamics/RandomTreeCreator.cpp
new file mode 100644
index 0000000..b2731c5
--- /dev/null
+++ b/Extras/InverseDynamics/RandomTreeCreator.cpp
@@ -0,0 +1,81 @@
+#include "RandomTreeCreator.hpp"
+
+#include <cstdio>
+
+#include "IDRandomUtil.hpp"
+
+namespace btInverseDynamics {
+
+RandomTreeCreator::RandomTreeCreator(const int max_bodies, bool random_seed) {
+ // seed generator
+ if(random_seed) {
+ randomInit(); // seeds with time()
+ } else {
+ randomInit(1); // seeds with 1
+ }
+ m_num_bodies = randomInt(1, max_bodies);
+}
+
+RandomTreeCreator::~RandomTreeCreator() {}
+
+int RandomTreeCreator::getNumBodies(int* num_bodies) const {
+ *num_bodies = m_num_bodies;
+ return 0;
+}
+
+int RandomTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
+ vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
+ vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
+ mat33* body_I_body, int* user_int, void** user_ptr) const {
+ if(0 == body_index) { //root body
+ *parent_index = -1;
+ } else {
+ *parent_index = randomInt(0, body_index - 1);
+ }
+
+ switch (randomInt(0, 3)) {
+ case 0:
+ *joint_type = FIXED;
+ break;
+ case 1:
+ *joint_type = REVOLUTE;
+ break;
+ case 2:
+ *joint_type = PRISMATIC;
+ break;
+ case 3:
+ *joint_type = FLOATING;
+ break;
+ default:
+ error_message("randomInt() result out of range\n");
+ return -1;
+ }
+
+ (*parent_r_parent_body_ref)(0) = randomFloat(-1.0, 1.0);
+ (*parent_r_parent_body_ref)(1) = randomFloat(-1.0, 1.0);
+ (*parent_r_parent_body_ref)(2) = randomFloat(-1.0, 1.0);
+
+ bodyTParentFromAxisAngle(randomAxis(), randomFloat(-BT_ID_PI, BT_ID_PI), body_T_parent_ref);
+
+ *body_axis_of_motion = randomAxis();
+ *mass = randomMass();
+ (*body_r_body_com)(0) = randomFloat(-1.0, 1.0);
+ (*body_r_body_com)(1) = randomFloat(-1.0, 1.0);
+ (*body_r_body_com)(2) = randomFloat(-1.0, 1.0);
+ const double a = randomFloat(-BT_ID_PI, BT_ID_PI);
+ const double b = randomFloat(-BT_ID_PI, BT_ID_PI);
+ const double c = randomFloat(-BT_ID_PI, BT_ID_PI);
+ vec3 ii = randomInertiaPrincipal();
+ mat33 ii_diag;
+ setZero(ii_diag);
+ ii_diag(0,0)=ii(0);
+ ii_diag(1,1)=ii(1);
+ ii_diag(2,2)=ii(2);
+ *body_I_body = transformX(a) * transformY(b) * transformZ(c) * ii_diag *
+ transformZ(-c) * transformY(-b) * transformX(-a);
+ *user_int = 0;
+ *user_ptr = 0;
+
+ return 0;
+}
+}
diff --git a/Extras/InverseDynamics/RandomTreeCreator.hpp b/Extras/InverseDynamics/RandomTreeCreator.hpp
new file mode 100644
index 0000000..bbadb99
--- /dev/null
+++ b/Extras/InverseDynamics/RandomTreeCreator.hpp
@@ -0,0 +1,31 @@
+#ifndef RANDOMTREE_CREATOR_HPP_
+#define RANDOMTREE_CREATOR_HPP_
+
+#include "BulletInverseDynamics/IDConfig.hpp"
+#include "MultiBodyTreeCreator.hpp"
+
+namespace btInverseDynamics {
+/// Generate a random MultiBodyTree with fixed or floating base and fixed, prismatic or revolute
+/// joints
+/// Uses a pseudo random number generator seeded from a random device.
+class RandomTreeCreator : public MultiBodyTreeCreator {
+public:
+ /// ctor
+ /// @param max_bodies maximum number of bodies
+ /// @param gravity gravitational acceleration
+ /// @param use_seed if true, seed random number generator
+ RandomTreeCreator(const int max_bodies, bool use_seed=false);
+ ~RandomTreeCreator();
+ ///\copydoc MultiBodyTreeCreator::getNumBodies
+ int getNumBodies(int* num_bodies) const;
+ ///\copydoc MultiBodyTreeCreator::getBody
+ int getBody(const int body_index, int* parent_index, JointType* joint_type,
+ vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
+ idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
+ void** user_ptr) const;
+
+private:
+ int m_num_bodies;
+};
+}
+#endif // RANDOMTREE_CREATOR_HPP_
diff --git a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp
index 978bad7..d627ea9 100644
--- a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp
+++ b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp
@@ -6,11 +6,11 @@
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
-#include "../CommonInterfaces/CommonGUIHelperInterface.h"
-#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
-#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
-#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
-#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "../../examples/CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h"
+#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h"
+#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
/// Create a btMultiBody model from URDF.
/// This is adapted from Bullet URDF loader example
@@ -44,7 +44,7 @@ public:
void init() {
this->createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(m_gravity);
- BulletURDFImporter urdf_importer(&m_nogfx);
+ BulletURDFImporter urdf_importer(&m_nogfx,0);
URDFImporterInterface &u2b(urdf_importer);
bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed);
diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
index b91a10c..ef3ebf6 100644
--- a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
+++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
@@ -237,7 +237,7 @@ int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const {
return 0;
}
-int btMultiBodyTreeCreator::getBody(int body_index, int *parent_index, JointType *joint_type,
+int btMultiBodyTreeCreator::getBody(const int body_index, int *parent_index, JointType *joint_type,
vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref,
vec3 *body_axis_of_motion, idScalar *mass,
vec3 *body_r_body_com, mat33 *body_I_body, int *user_int,
diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp
index 0e2d614..dbc82ab 100644
--- a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp
+++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp
@@ -25,10 +25,10 @@ public:
/// \copydoc MultiBodyTreeCreator::getNumBodies
int getNumBodies(int *num_bodies) const;
///\copydoc MultiBodyTreeCreator::getBody
- int getBody(int body_index, int *parent_index, JointType *joint_type,
- vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref, vec3 *body_axis_of_motion,
- idScalar *mass, vec3 *body_r_body_com, mat33 *body_I_body, int *user_int,
- void **user_ptr) const;
+ int getBody(const int body_index, int *parent_index, JointType *joint_type,
+ vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref,
+ vec3 *body_axis_of_motion, idScalar *mass, vec3 *body_r_body_com,
+ mat33 *body_I_body, int *user_int, void **user_ptr) const;
private:
// internal struct holding data extracted from btMultiBody
diff --git a/Extras/InverseDynamics/invdyn_bullet_comparison.hpp b/Extras/InverseDynamics/invdyn_bullet_comparison.hpp
index 2a74af5..1a6b7d4 100644
--- a/Extras/InverseDynamics/invdyn_bullet_comparison.hpp
+++ b/Extras/InverseDynamics/invdyn_bullet_comparison.hpp
@@ -1,12 +1,13 @@
#ifndef INVDYN_BULLET_COMPARISON_HPP
#define INVDYN_BULLET_COMPARISON_HPP
+#include "BulletInverseDynamics/IDConfig.hpp"
+
class btMultiBody;
class btVector3;
namespace btInverseDynamics {
class MultiBodyTree;
-class vecx;
/// this function compares the forward dynamics computations implemented in btMultiBody to
/// the inverse dynamics implementation in MultiBodyTree. This is done in three steps
diff --git a/Extras/Serialize/BulletFileLoader/autogenerated/bullet.h b/Extras/Serialize/BulletFileLoader/autogenerated/bullet.h
index 99fb8cc..49fb0de 100644
--- a/Extras/Serialize/BulletFileLoader/autogenerated/bullet.h
+++ b/Extras/Serialize/BulletFileLoader/autogenerated/bullet.h
@@ -67,6 +67,8 @@ typedef struct bInvalidHandle {
class btConvexHullShapeData;
class btCollisionObjectDoubleData;
class btCollisionObjectFloatData;
+ class btContactSolverInfoDoubleData;
+ class btContactSolverInfoFloatData;
class btDynamicsWorldDoubleData;
class btDynamicsWorldFloatData;
class btRigidBodyFloatData;
@@ -93,8 +95,6 @@ typedef struct bInvalidHandle {
class btSliderConstraintDoubleData;
class btGearConstraintFloatData;
class btGearConstraintDoubleData;
- class btContactSolverInfoDoubleData;
- class btContactSolverInfoFloatData;
class SoftBodyMaterialData;
class SoftBodyNodeData;
class SoftBodyLinkData;
@@ -600,6 +600,81 @@ typedef struct bInvalidHandle {
};
+// -------------------------------------------------- //
+ class btContactSolverInfoDoubleData
+ {
+ public:
+ double m_tau;
+ double m_damping;
+ double m_friction;
+ double m_timeStep;
+ double m_restitution;
+ double m_maxErrorReduction;
+ double m_sor;
+ double m_erp;
+ double m_erp2;
+ double m_globalCfm;
+ double m_splitImpulsePenetrationThreshold;
+ double m_splitImpulseTurnErp;
+ double m_linearSlop;
+ double m_warmstartingFactor;
+ double m_maxGyroscopicForce;
+ double m_singleAxisRollingFrictionThreshold;
+ int m_numIterations;
+ int m_solverMode;
+ int m_restingContactRestitutionThreshold;
+ int m_minimumSolverBatchSize;
+ int m_splitImpulse;
+ char m_padding[4];
+ };
+
+
+// -------------------------------------------------- //
+ class btContactSolverInfoFloatData
+ {
+ public:
+ float m_tau;
+ float m_damping;
+ float m_friction;
+ float m_timeStep;
+ float m_restitution;
+ float m_maxErrorReduction;
+ float m_sor;
+ float m_erp;
+ float m_erp2;
+ float m_globalCfm;
+ float m_splitImpulsePenetrationThreshold;
+ float m_splitImpulseTurnErp;
+ float m_linearSlop;
+ float m_warmstartingFactor;
+ float m_maxGyroscopicForce;
+ float m_singleAxisRollingFrictionThreshold;
+ int m_numIterations;
+ int m_solverMode;
+ int m_restingContactRestitutionThreshold;
+ int m_minimumSolverBatchSize;
+ int m_splitImpulse;
+ char m_padding[4];
+ };
+
+
+// -------------------------------------------------- //
+ class btDynamicsWorldDoubleData
+ {
+ public:
+ btContactSolverInfoDoubleData m_solverInfo;
+ btVector3DoubleData m_gravity;
+ };
+
+
+// -------------------------------------------------- //
+ class btDynamicsWorldFloatData
+ {
+ public:
+ btContactSolverInfoFloatData m_solverInfo;
+ btVector3FloatData m_gravity;
+ };
+
// -------------------------------------------------- //
class btRigidBodyFloatData
@@ -1062,82 +1137,6 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
- class btContactSolverInfoDoubleData
- {
- public:
- double m_tau;
- double m_damping;
- double m_friction;
- double m_timeStep;
- double m_restitution;
- double m_maxErrorReduction;
- double m_sor;
- double m_erp;
- double m_erp2;
- double m_globalCfm;
- double m_splitImpulsePenetrationThreshold;
- double m_splitImpulseTurnErp;
- double m_linearSlop;
- double m_warmstartingFactor;
- double m_maxGyroscopicForce;
- double m_singleAxisRollingFrictionThreshold;
- int m_numIterations;
- int m_solverMode;
- int m_restingContactRestitutionThreshold;
- int m_minimumSolverBatchSize;
- int m_splitImpulse;
- char m_padding[4];
- };
-
-
-// -------------------------------------------------- //
- class btContactSolverInfoFloatData
- {
- public:
- float m_tau;
- float m_damping;
- float m_friction;
- float m_timeStep;
- float m_restitution;
- float m_maxErrorReduction;
- float m_sor;
- float m_erp;
- float m_erp2;
- float m_globalCfm;
- float m_splitImpulsePenetrationThreshold;
- float m_splitImpulseTurnErp;
- float m_linearSlop;
- float m_warmstartingFactor;
- float m_maxGyroscopicForce;
- float m_singleAxisRollingFrictionThreshold;
- int m_numIterations;
- int m_solverMode;
- int m_restingContactRestitutionThreshold;
- int m_minimumSolverBatchSize;
- int m_splitImpulse;
- char m_padding[4];
- };
-
- // -------------------------------------------------- //
- class btDynamicsWorldDoubleData
- {
- public:
- btContactSolverInfoDoubleData m_solverInfo;
- btVector3DoubleData m_gravity;
- };
-
-
-// -------------------------------------------------- //
- class btDynamicsWorldFloatData
- {
- public:
- btContactSolverInfoFloatData m_solverInfo;
- btVector3FloatData m_gravity;
- };
-
-
-
-// -------------------------------------------------- //
class SoftBodyMaterialData
{
public:
@@ -1354,9 +1353,6 @@ typedef struct bInvalidHandle {
btVector3DoubleData m_thisPivotToThisComOffset;
btVector3DoubleData m_jointAxisTop[6];
btVector3DoubleData m_jointAxisBottom[6];
- char *m_linkName;
- char *m_jointName;
- btCollisionObjectDoubleData *m_linkCollider;
btVector3DoubleData m_linkInertia;
double m_linkMass;
int m_parentIndex;
@@ -1366,6 +1362,12 @@ typedef struct bInvalidHandle {
double m_jointPos[7];
double m_jointVel[6];
double m_jointTorque[6];
+ double m_jointDamping;
+ double m_jointFriction;
+ char *m_linkName;
+ char *m_jointName;
+ btCollisionObjectDoubleData *m_linkCollider;
+ char *m_paddingPtr;
};
@@ -1378,9 +1380,6 @@ typedef struct bInvalidHandle {
btVector3FloatData m_thisPivotToThisComOffset;
btVector3FloatData m_jointAxisTop[6];
btVector3FloatData m_jointAxisBottom[6];
- char *m_linkName;
- char *m_jointName;
- btCollisionObjectFloatData *m_linkCollider;
btVector3FloatData m_linkInertia;
int m_dofCount;
float m_linkMass;
@@ -1390,6 +1389,12 @@ typedef struct bInvalidHandle {
float m_jointVel[6];
float m_jointTorque[6];
int m_posVarCount;
+ float m_jointDamping;
+ float m_jointFriction;
+ char *m_linkName;
+ char *m_jointName;
+ btCollisionObjectFloatData *m_linkCollider;
+ char *m_paddingPtr;
};
@@ -1397,13 +1402,14 @@ typedef struct bInvalidHandle {
class btMultiBodyDoubleData
{
public:
+ btTransformDoubleData m_baseWorldTransform;
+ btVector3DoubleData m_baseInertia;
+ double m_baseMass;
char *m_baseName;
btMultiBodyLinkDoubleData *m_links;
btCollisionObjectDoubleData *m_baseCollider;
- btTransformDoubleData m_baseWorldTransform;
- btVector3DoubleData m_baseInertia;
+ char *m_paddingPtr;
int m_numLinks;
- double m_baseMass;
char m_padding[4];
};
diff --git a/Extras/Serialize/BulletFileLoader/bDNA.cpp b/Extras/Serialize/BulletFileLoader/bDNA.cpp
index f30f7fe..86d15cf 100644
--- a/Extras/Serialize/BulletFileLoader/bDNA.cpp
+++ b/Extras/Serialize/BulletFileLoader/bDNA.cpp
@@ -389,18 +389,10 @@ void bDNA::init(char *data, int len, bool swap)
cp++;
}
-
- {
- nr= (long)cp;
- //long mask=3;
- nr= ((nr+3)&~3)-nr;
- while (nr--)
- {
- cp++;
- }
- }
-
+
+ cp = btAlignPointer(cp,4);
+
/*
TYPE (4 bytes)
<nr> amount of types (int)
@@ -426,17 +418,8 @@ void bDNA::init(char *data, int len, bool swap)
cp++;
}
-{
- nr= (long)cp;
- // long mask=3;
- nr= ((nr+3)&~3)-nr;
- while (nr--)
- {
- cp++;
- }
- }
-
-
+ cp = btAlignPointer(cp,4);
+
/*
TLEN (4 bytes)
<len> (short) the lengths of types
diff --git a/Extras/Serialize/BulletFileLoader/bFile.cpp b/Extras/Serialize/BulletFileLoader/bFile.cpp
index 469ee67..cb8010d 100644
--- a/Extras/Serialize/BulletFileLoader/bFile.cpp
+++ b/Extras/Serialize/BulletFileLoader/bFile.cpp
@@ -460,15 +460,7 @@ void bFile::swapDNA(char* ptr)
}
- {
- nr= (long)cp;
- //long mask=3;
- nr= ((nr+3)&~3)-nr;
- while (nr--)
- {
- cp++;
- }
- }
+ cp = btAlignPointer(cp,4);
/*
@@ -497,16 +489,7 @@ void bFile::swapDNA(char* ptr)
cp++;
}
-{
- nr= (long)cp;
- // long mask=3;
- nr= ((nr+3)&~3)-nr;
- while (nr--)
- {
- cp++;
- }
- }
-
+ cp = btAlignPointer(cp,4);
/*
TLEN (4 bytes)
@@ -644,7 +627,7 @@ void bFile::preSwap()
swap(dataPtrHead, dataChunk,ignoreEndianFlag);
} else
{
- printf("unknown chunk\n");
+ //printf("unknown chunk\n");
}
}
diff --git a/Extras/Serialize/BulletFileLoader/btBulletFile.cpp b/Extras/Serialize/BulletFileLoader/btBulletFile.cpp
index f3ed47c..18cb153 100644
--- a/Extras/Serialize/BulletFileLoader/btBulletFile.cpp
+++ b/Extras/Serialize/BulletFileLoader/btBulletFile.cpp
@@ -224,7 +224,7 @@ void btBulletFile::parseData()
// }
} else
{
- printf("unknown chunk\n");
+ //printf("unknown chunk\n");
mLibPointers.insert(dataChunk.oldPtr, (bStructHandle*)dataPtrHead);
}
diff --git a/Extras/Serialize/HeaderGenerator/apiGen.cpp b/Extras/Serialize/HeaderGenerator/apiGen.cpp
index 267b99f..16c643e 100644
--- a/Extras/Serialize/HeaderGenerator/apiGen.cpp
+++ b/Extras/Serialize/HeaderGenerator/apiGen.cpp
@@ -220,7 +220,7 @@ int main(int argc,char** argv)
fprintf(dump, "%s\n", data);
- char* filename = "../../../../data/slope.bullet";
+ char* filename = "../../../../data/r2d2_multibody.bullet";
if (argc==2)
filename = argv[1];
diff --git a/Extras/Serialize/makesdna/makesdna.cpp b/Extras/Serialize/makesdna/makesdna.cpp
index 50d86d4..105167d 100644
--- a/Extras/Serialize/makesdna/makesdna.cpp
+++ b/Extras/Serialize/makesdna/makesdna.cpp
@@ -70,22 +70,17 @@ typedef unsigned __int32 uint32_t;
typedef unsigned __int64 uint64_t;
#ifndef _INTPTR_T_DEFINED
-#ifdef _WIN64
-typedef __int64 intptr_t;
+ #ifdef _WIN64
+ typedef __int64 btintptr_t;
+ #else
+ typedef long btintptr_t;
+ #endif
+
#else
-typedef long intptr_t;
-#endif
-#define _INTPTR_T_DEFINED
+ typedef intptr_t btintptr_t;
#endif
-#ifndef _UINTPTR_T_DEFINED
-#ifdef _WIN64
-typedef unsigned __int64 uintptr_t;
-#else
-typedef unsigned long uintptr_t;
-#endif
-#define _UINTPTR_T_DEFINED
-#endif
+
#elif defined(__linux__) || defined(__NetBSD__)
@@ -1070,7 +1065,7 @@ int make_structDNA(char *baseDirectory, FILE *file)
/* calculate size of datablock with strings */
cp= names[nr_names-1];
cp+= strlen(names[nr_names-1]) + 1; /* +1: null-terminator */
- len= (intptr_t) (cp - (char*) names[0]);
+ len= (btintptr_t) (cp - (char*) names[0]);
len= (len+3) & ~3;
dna_write(file, names[0], len);
@@ -1083,7 +1078,7 @@ int make_structDNA(char *baseDirectory, FILE *file)
/* calculate datablock size */
cp= types[nr_types-1];
cp+= strlen(types[nr_types-1]) + 1; /* +1: null-terminator */
- len= (intptr_t) (cp - (char*) types[0]);
+ len= (btintptr_t) (cp - (char*) types[0]);
len= (len+3) & ~3;
dna_write(file, types[0], len);
@@ -1105,7 +1100,7 @@ int make_structDNA(char *baseDirectory, FILE *file)
/* calc datablock size */
sp= structs[nr_structs-1];
sp+= 2+ 2*( sp[1] );
- len= (intptr_t) ((char*) sp - (char*) structs[0]);
+ len= (btintptr_t) ((char*) sp - (char*) structs[0]);
len= (len+3) & ~3;
dna_write(file, structs[0], len);
diff --git a/Extras/VHACD/inc/btAlignedAllocator.h b/Extras/VHACD/inc/btAlignedAllocator.h
new file mode 100644
index 0000000..4af0472
--- /dev/null
+++ b/Extras/VHACD/inc/btAlignedAllocator.h
@@ -0,0 +1,104 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_ALIGNED_ALLOCATOR
+#define BT_ALIGNED_ALLOCATOR
+
+///we probably replace this with our own aligned memory allocator
+///so we replace _aligned_malloc and _aligned_free with our own
+///that is better portable and more predictable
+
+#include "btScalar.h"
+//#define BT_DEBUG_MEMORY_ALLOCATIONS 1
+#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
+
+#define btAlignedAlloc(a, b) \
+ btAlignedAllocInternal(a, b, __LINE__, __FILE__)
+
+#define btAlignedFree(ptr) \
+ btAlignedFreeInternal(ptr, __LINE__, __FILE__)
+
+void* btAlignedAllocInternal(size_t size, int alignment, int line, char* filename);
+
+void btAlignedFreeInternal(void* ptr, int line, char* filename);
+
+#else
+void* btAlignedAllocInternal(size_t size, int alignment);
+void btAlignedFreeInternal(void* ptr);
+
+#define btAlignedAlloc(size, alignment) btAlignedAllocInternal(size, alignment)
+#define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
+
+#endif
+typedef int size_type;
+
+typedef void*(btAlignedAllocFunc)(size_t size, int alignment);
+typedef void(btAlignedFreeFunc)(void* memblock);
+typedef void*(btAllocFunc)(size_t size);
+typedef void(btFreeFunc)(void* memblock);
+
+///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom
+void btAlignedAllocSetCustom(btAllocFunc* allocFunc, btFreeFunc* freeFunc);
+///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
+void btAlignedAllocSetCustomAligned(btAlignedAllocFunc* allocFunc, btAlignedFreeFunc* freeFunc);
+
+///The btAlignedAllocator is a portable class for aligned memory allocations.
+///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned.
+template <typename T, unsigned Alignment>
+class btAlignedAllocator {
+
+ typedef btAlignedAllocator<T, Alignment> self_type;
+
+public:
+ //just going down a list:
+ btAlignedAllocator() {}
+ /*
+ btAlignedAllocator( const self_type & ) {}
+ */
+
+ template <typename Other>
+ btAlignedAllocator(const btAlignedAllocator<Other, Alignment>&) {}
+
+ typedef const T* const_pointer;
+ typedef const T& const_reference;
+ typedef T* pointer;
+ typedef T& reference;
+ typedef T value_type;
+
+ pointer address(reference ref) const { return &ref; }
+ const_pointer address(const_reference ref) const { return &ref; }
+ pointer allocate(size_type n, const_pointer* hint = 0)
+ {
+ (void)hint;
+ return reinterpret_cast<pointer>(btAlignedAlloc(sizeof(value_type) * n, Alignment));
+ }
+ void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); }
+ void deallocate(pointer ptr)
+ {
+ btAlignedFree(reinterpret_cast<void*>(ptr));
+ }
+ void destroy(pointer ptr) { ptr->~value_type(); }
+
+ template <typename O>
+ struct rebind {
+ typedef btAlignedAllocator<O, Alignment> other;
+ };
+ template <typename O>
+ self_type& operator=(const btAlignedAllocator<O, Alignment>&) { return *this; }
+
+ friend bool operator==(const self_type&, const self_type&) { return true; }
+};
+
+#endif //BT_ALIGNED_ALLOCATOR
diff --git a/Extras/VHACD/inc/btAlignedObjectArray.h b/Extras/VHACD/inc/btAlignedObjectArray.h
new file mode 100644
index 0000000..49413c8
--- /dev/null
+++ b/Extras/VHACD/inc/btAlignedObjectArray.h
@@ -0,0 +1,448 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_OBJECT_ARRAY__
+#define BT_OBJECT_ARRAY__
+
+#include "btAlignedAllocator.h"
+#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
+
+///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW
+///then the btAlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
+///You can enable BT_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
+///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and
+///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
+
+#define BT_USE_PLACEMENT_NEW 1
+//#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
+#define BT_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful
+
+#ifdef BT_USE_MEMCPY
+#include <memory.h>
+#include <string.h>
+#endif //BT_USE_MEMCPY
+
+#ifdef BT_USE_PLACEMENT_NEW
+#include <new> //for placement new
+#endif //BT_USE_PLACEMENT_NEW
+
+///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods
+///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
+template <typename T>
+//template <class T>
+class btAlignedObjectArray {
+ btAlignedAllocator<T, 16> m_allocator;
+
+ int m_size;
+ int m_capacity;
+ T* m_data;
+ //PCK: added this line
+ bool m_ownsMemory;
+
+#ifdef BT_ALLOW_ARRAY_COPY_OPERATOR
+public:
+ SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T>& other)
+ {
+ copyFromArray(other);
+ return *this;
+ }
+#else //BT_ALLOW_ARRAY_COPY_OPERATOR
+private:
+ SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T>& other);
+#endif //BT_ALLOW_ARRAY_COPY_OPERATOR
+
+protected:
+ SIMD_FORCE_INLINE int allocSize(int size)
+ {
+ return (size ? size * 2 : 1);
+ }
+ SIMD_FORCE_INLINE void copy(int start, int end, T* dest) const
+ {
+ int i;
+ for (i = start; i < end; ++i)
+#ifdef BT_USE_PLACEMENT_NEW
+ new (&dest[i]) T(m_data[i]);
+#else
+ dest[i] = m_data[i];
+#endif //BT_USE_PLACEMENT_NEW
+ }
+
+ SIMD_FORCE_INLINE void init()
+ {
+ //PCK: added this line
+ m_ownsMemory = true;
+ m_data = 0;
+ m_size = 0;
+ m_capacity = 0;
+ }
+ SIMD_FORCE_INLINE void destroy(int first, int last)
+ {
+ int i;
+ for (i = first; i < last; i++) {
+ m_data[i].~T();
+ }
+ }
+
+ SIMD_FORCE_INLINE void* allocate(int size)
+ {
+ if (size)
+ return m_allocator.allocate(size);
+ return 0;
+ }
+
+ SIMD_FORCE_INLINE void deallocate()
+ {
+ if (m_data) {
+ //PCK: enclosed the deallocation in this block
+ if (m_ownsMemory) {
+ m_allocator.deallocate(m_data);
+ }
+ m_data = 0;
+ }
+ }
+
+public:
+ btAlignedObjectArray()
+ {
+ init();
+ }
+
+ ~btAlignedObjectArray()
+ {
+ clear();
+ }
+
+ ///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
+ btAlignedObjectArray(const btAlignedObjectArray& otherArray)
+ {
+ init();
+
+ int otherSize = otherArray.size();
+ resize(otherSize);
+ otherArray.copy(0, otherSize, m_data);
+ }
+
+ /// return the number of elements in the array
+ SIMD_FORCE_INLINE int size() const
+ {
+ return m_size;
+ }
+
+ SIMD_FORCE_INLINE const T& at(int n) const
+ {
+ btAssert(n >= 0);
+ btAssert(n < size());
+ return m_data[n];
+ }
+
+ SIMD_FORCE_INLINE T& at(int n)
+ {
+ btAssert(n >= 0);
+ btAssert(n < size());
+ return m_data[n];
+ }
+
+ SIMD_FORCE_INLINE const T& operator[](int n) const
+ {
+ btAssert(n >= 0);
+ btAssert(n < size());
+ return m_data[n];
+ }
+
+ SIMD_FORCE_INLINE T& operator[](int n)
+ {
+ btAssert(n >= 0);
+ btAssert(n < size());
+ return m_data[n];
+ }
+
+ ///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
+ SIMD_FORCE_INLINE void clear()
+ {
+ destroy(0, size());
+
+ deallocate();
+
+ init();
+ }
+
+ SIMD_FORCE_INLINE void pop_back()
+ {
+ btAssert(m_size > 0);
+ m_size--;
+ m_data[m_size].~T();
+ }
+
+ ///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
+ ///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
+ SIMD_FORCE_INLINE void resize(int newsize, const T& fillData = T())
+ {
+ int curSize = size();
+
+ if (newsize < curSize) {
+ for (int i = newsize; i < curSize; i++) {
+ m_data[i].~T();
+ }
+ }
+ else {
+ if (newsize > size()) {
+ reserve(newsize);
+ }
+#ifdef BT_USE_PLACEMENT_NEW
+ for (int i = curSize; i < newsize; i++) {
+ new (&m_data[i]) T(fillData);
+ }
+#endif //BT_USE_PLACEMENT_NEW
+ }
+
+ m_size = newsize;
+ }
+
+ SIMD_FORCE_INLINE T& expandNonInitializing()
+ {
+ int sz = size();
+ if (sz == capacity()) {
+ reserve(allocSize(size()));
+ }
+ m_size++;
+
+ return m_data[sz];
+ }
+
+ SIMD_FORCE_INLINE T& expand(const T& fillValue = T())
+ {
+ int sz = size();
+ if (sz == capacity()) {
+ reserve(allocSize(size()));
+ }
+ m_size++;
+#ifdef BT_USE_PLACEMENT_NEW
+ new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
+#endif
+
+ return m_data[sz];
+ }
+
+ SIMD_FORCE_INLINE void push_back(const T& _Val)
+ {
+ int sz = size();
+ if (sz == capacity()) {
+ reserve(allocSize(size()));
+ }
+
+#ifdef BT_USE_PLACEMENT_NEW
+ new (&m_data[m_size]) T(_Val);
+#else
+ m_data[size()] = _Val;
+#endif //BT_USE_PLACEMENT_NEW
+
+ m_size++;
+ }
+
+ /// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
+ SIMD_FORCE_INLINE int capacity() const
+ {
+ return m_capacity;
+ }
+
+ SIMD_FORCE_INLINE void reserve(int _Count)
+ { // determine new minimum length of allocated storage
+ if (capacity() < _Count) { // not enough room, reallocate
+ T* s = (T*)allocate(_Count);
+
+ copy(0, size(), s);
+
+ destroy(0, size());
+
+ deallocate();
+
+ //PCK: added this line
+ m_ownsMemory = true;
+
+ m_data = s;
+
+ m_capacity = _Count;
+ }
+ }
+
+ class less {
+ public:
+ bool operator()(const T& a, const T& b)
+ {
+ return (a < b);
+ }
+ };
+
+ template <typename L>
+ void quickSortInternal(const L& CompareFunc, int lo, int hi)
+ {
+ // lo is the lower index, hi is the upper index
+ // of the region of array a that is to be sorted
+ int i = lo, j = hi;
+ T x = m_data[(lo + hi) / 2];
+
+ // partition
+ do {
+ while (CompareFunc(m_data[i], x))
+ i++;
+ while (CompareFunc(x, m_data[j]))
+ j--;
+ if (i <= j) {
+ swap(i, j);
+ i++;
+ j--;
+ }
+ } while (i <= j);
+
+ // recursion
+ if (lo < j)
+ quickSortInternal(CompareFunc, lo, j);
+ if (i < hi)
+ quickSortInternal(CompareFunc, i, hi);
+ }
+
+ template <typename L>
+ void quickSort(const L& CompareFunc)
+ {
+ //don't sort 0 or 1 elements
+ if (size() > 1) {
+ quickSortInternal(CompareFunc, 0, size() - 1);
+ }
+ }
+
+ ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+ template <typename L>
+ void downHeap(T* pArr, int k, int n, const L& CompareFunc)
+ {
+ /* PRE: a[k+1..N] is a heap */
+ /* POST: a[k..N] is a heap */
+
+ T temp = pArr[k - 1];
+ /* k has child(s) */
+ while (k <= n / 2) {
+ int child = 2 * k;
+
+ if ((child < n) && CompareFunc(pArr[child - 1], pArr[child])) {
+ child++;
+ }
+ /* pick larger child */
+ if (CompareFunc(temp, pArr[child - 1])) {
+ /* move child up */
+ pArr[k - 1] = pArr[child - 1];
+ k = child;
+ }
+ else {
+ break;
+ }
+ }
+ pArr[k - 1] = temp;
+ } /*downHeap*/
+
+ void swap(int index0, int index1)
+ {
+#ifdef BT_USE_MEMCPY
+ char temp[sizeof(T)];
+ memcpy(temp, &m_data[index0], sizeof(T));
+ memcpy(&m_data[index0], &m_data[index1], sizeof(T));
+ memcpy(&m_data[index1], temp, sizeof(T));
+#else
+ T temp = m_data[index0];
+ m_data[index0] = m_data[index1];
+ m_data[index1] = temp;
+#endif //BT_USE_PLACEMENT_NEW
+ }
+
+ template <typename L>
+ void heapSort(const L& CompareFunc)
+ {
+ /* sort a[0..N-1], N.B. 0 to N-1 */
+ int k;
+ int n = m_size;
+ for (k = n / 2; k > 0; k--) {
+ downHeap(m_data, k, n, CompareFunc);
+ }
+
+ /* a[1..N] is now a heap */
+ while (n >= 1) {
+ swap(0, n - 1); /* largest of a[0..n-1] */
+
+ n = n - 1;
+ /* restore a[1..i-1] heap */
+ downHeap(m_data, 1, n, CompareFunc);
+ }
+ }
+
+ ///non-recursive binary search, assumes sorted array
+ int findBinarySearch(const T& key) const
+ {
+ int first = 0;
+ int last = size() - 1;
+
+ //assume sorted array
+ while (first <= last) {
+ int mid = (first + last) / 2; // compute mid point.
+ if (key > m_data[mid])
+ first = mid + 1; // repeat search in top half.
+ else if (key < m_data[mid])
+ last = mid - 1; // repeat search in bottom half.
+ else
+ return mid; // found it. return position /////
+ }
+ return size(); // failed to find key
+ }
+
+ int findLinearSearch(const T& key) const
+ {
+ int index = size();
+ int i;
+
+ for (i = 0; i < size(); i++) {
+ if (m_data[i] == key) {
+ index = i;
+ break;
+ }
+ }
+ return index;
+ }
+
+ void remove(const T& key)
+ {
+
+ int findIndex = findLinearSearch(key);
+ if (findIndex < size()) {
+ swap(findIndex, size() - 1);
+ pop_back();
+ }
+ }
+
+ //PCK: whole function
+ void initializeFromBuffer(void* buffer, int size, int capacity)
+ {
+ clear();
+ m_ownsMemory = false;
+ m_data = (T*)buffer;
+ m_size = size;
+ m_capacity = capacity;
+ }
+
+ void copyFromArray(const btAlignedObjectArray& otherArray)
+ {
+ int otherSize = otherArray.size();
+ resize(otherSize);
+ otherArray.copy(0, otherSize, m_data);
+ }
+};
+
+#endif //BT_OBJECT_ARRAY__
diff --git a/Extras/VHACD/inc/btConvexHullComputer.h b/Extras/VHACD/inc/btConvexHullComputer.h
new file mode 100644
index 0000000..d59b81e
--- /dev/null
+++ b/Extras/VHACD/inc/btConvexHullComputer.h
@@ -0,0 +1,97 @@
+/*
+Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_CONVEX_HULL_COMPUTER_H
+#define BT_CONVEX_HULL_COMPUTER_H
+
+#include "btAlignedObjectArray.h"
+#include "btVector3.h"
+
+/// Convex hull implementation based on Preparata and Hong
+/// See http://code.google.com/p/bullet/issues/detail?id=275
+/// Ole Kniemeyer, MAXON Computer GmbH
+class btConvexHullComputer {
+private:
+ btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp);
+
+public:
+ class Edge {
+ private:
+ int next;
+ int reverse;
+ int targetVertex;
+
+ friend class btConvexHullComputer;
+
+ public:
+ int getSourceVertex() const
+ {
+ return (this + reverse)->targetVertex;
+ }
+
+ int getTargetVertex() const
+ {
+ return targetVertex;
+ }
+
+ const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
+ {
+ return this + next;
+ }
+
+ const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
+ {
+ return (this + reverse)->getNextEdgeOfVertex();
+ }
+
+ const Edge* getReverseEdge() const
+ {
+ return this + reverse;
+ }
+ };
+
+ // Vertices of the output hull
+ btAlignedObjectArray<btVector3> vertices;
+
+ // Edges of the output hull
+ btAlignedObjectArray<Edge> edges;
+
+ // Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
+ btAlignedObjectArray<int> faces;
+
+ /*
+ Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
+ between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken
+ by that amount (each face is moved by "shrink" length units towards the center along its normal).
+ If "shrinkClamp" is positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where "innerRadius"
+ is the minimum distance of a face to the center of the convex hull.
+
+ The returned value is the amount by which the hull has been shrunken. If it is negative, the amount was so large
+ that the resulting convex hull is empty.
+
+ The output convex hull can be found in the member variables "vertices", "edges", "faces".
+ */
+ btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
+ {
+ return compute(coords, false, stride, count, shrink, shrinkClamp);
+ }
+
+ // same as above, but double precision
+ btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
+ {
+ return compute(coords, true, stride, count, shrink, shrinkClamp);
+ }
+};
+
+#endif //BT_CONVEX_HULL_COMPUTER_H
diff --git a/Extras/VHACD/inc/btMinMax.h b/Extras/VHACD/inc/btMinMax.h
new file mode 100644
index 0000000..40b0ceb
--- /dev/null
+++ b/Extras/VHACD/inc/btMinMax.h
@@ -0,0 +1,65 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_GEN_MINMAX_H
+#define BT_GEN_MINMAX_H
+
+#include "btScalar.h"
+
+template <class T>
+SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b)
+{
+ return a < b ? a : b;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b)
+{
+ return a > b ? a : b;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub)
+{
+ return a < lb ? lb : (ub < a ? ub : a);
+}
+
+template <class T>
+SIMD_FORCE_INLINE void btSetMin(T& a, const T& b)
+{
+ if (b < a) {
+ a = b;
+ }
+}
+
+template <class T>
+SIMD_FORCE_INLINE void btSetMax(T& a, const T& b)
+{
+ if (a < b) {
+ a = b;
+ }
+}
+
+template <class T>
+SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub)
+{
+ if (a < lb) {
+ a = lb;
+ }
+ else if (ub < a) {
+ a = ub;
+ }
+}
+
+#endif //BT_GEN_MINMAX_H
diff --git a/Extras/VHACD/inc/btScalar.h b/Extras/VHACD/inc/btScalar.h
new file mode 100644
index 0000000..ad30324
--- /dev/null
+++ b/Extras/VHACD/inc/btScalar.h
@@ -0,0 +1,532 @@
+/*
+Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SCALAR_H
+#define BT_SCALAR_H
+
+#ifdef BT_MANAGED_CODE
+//Aligned data types not supported in managed code
+#pragma unmanaged
+#endif
+
+#include <float.h>
+#include <math.h>
+#include <stdlib.h> //size_t for MSVC 6.0
+
+/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
+#define BT_BULLET_VERSION 279
+
+inline int btGetVersion()
+{
+ return BT_BULLET_VERSION;
+}
+
+#if defined(DEBUG) || defined(_DEBUG)
+#define BT_DEBUG
+#endif
+
+#ifdef _WIN32
+
+#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined(_MSC_VER) && _MSC_VER < 1300)
+
+#define SIMD_FORCE_INLINE inline
+#define ATTRIBUTE_ALIGNED16(a) a
+#define ATTRIBUTE_ALIGNED64(a) a
+#define ATTRIBUTE_ALIGNED128(a) a
+#else
+//#define BT_HAS_ALIGNED_ALLOCATOR
+#pragma warning(disable : 4324) // disable padding warning
+// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
+// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
+// #pragma warning(disable:4786) // Disable the "debug name too long" warning
+
+#define SIMD_FORCE_INLINE __forceinline
+#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
+#define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
+#define ATTRIBUTE_ALIGNED128(a) __declspec(align(128)) a
+#ifdef _XBOX
+#define BT_USE_VMX128
+
+#include <ppcintrinsics.h>
+#define BT_HAVE_NATIVE_FSEL
+#define btFsel(a, b, c) __fsel((a), (b), (c))
+#else
+
+#if (defined(_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined(BT_USE_DOUBLE_PRECISION))
+#define BT_USE_SSE
+#include <emmintrin.h>
+#endif
+
+#endif //_XBOX
+
+#endif //__MINGW32__
+
+#include <assert.h>
+#ifdef BT_DEBUG
+#define btAssert assert
+#else
+#define btAssert(x)
+#endif
+//btFullAssert is optional, slows down a lot
+#define btFullAssert(x)
+
+#define btLikely(_c) _c
+#define btUnlikely(_c) _c
+
+#else
+
+#if defined(__CELLOS_LV2__)
+#define SIMD_FORCE_INLINE inline __attribute__((always_inline))
+#define ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
+#define ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
+#define ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
+#ifndef assert
+#include <assert.h>
+#endif
+#ifdef BT_DEBUG
+#ifdef __SPU__
+#include <spu_printf.h>
+#define printf spu_printf
+#define btAssert(x) \
+ { \
+ if (!(x)) { \
+ printf("Assert " __FILE__ ":%u (" #x ")\n", __LINE__); \
+ spu_hcmpeq(0, 0); \
+ } \
+ }
+#else
+#define btAssert assert
+#endif
+
+#else
+#define btAssert(x)
+#endif
+//btFullAssert is optional, slows down a lot
+#define btFullAssert(x)
+
+#define btLikely(_c) _c
+#define btUnlikely(_c) _c
+
+#else
+
+#ifdef USE_LIBSPE2
+
+#define SIMD_FORCE_INLINE __inline
+#define ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
+#define ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
+#define ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
+#ifndef assert
+#include <assert.h>
+#endif
+#ifdef BT_DEBUG
+#define btAssert assert
+#else
+#define btAssert(x)
+#endif
+//btFullAssert is optional, slows down a lot
+#define btFullAssert(x)
+
+#define btLikely(_c) __builtin_expect((_c), 1)
+#define btUnlikely(_c) __builtin_expect((_c), 0)
+
+#else
+//non-windows systems
+
+#if (defined(__APPLE__) && defined(__i386__) && (!defined(BT_USE_DOUBLE_PRECISION)))
+#define BT_USE_SSE
+#include <emmintrin.h>
+
+#define SIMD_FORCE_INLINE inline
+///@todo: check out alignment methods for other platforms/compilers
+#define ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
+#define ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
+#define ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
+#ifndef assert
+#include <assert.h>
+#endif
+
+#if defined(DEBUG) || defined(_DEBUG)
+#define btAssert assert
+#else
+#define btAssert(x)
+#endif
+
+//btFullAssert is optional, slows down a lot
+#define btFullAssert(x)
+#define btLikely(_c) _c
+#define btUnlikely(_c) _c
+
+#else
+
+#define SIMD_FORCE_INLINE inline
+///@todo: check out alignment methods for other platforms/compilers
+///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
+///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+#define ATTRIBUTE_ALIGNED16(a) a
+#define ATTRIBUTE_ALIGNED64(a) a
+#define ATTRIBUTE_ALIGNED128(a) a
+#ifndef assert
+#include <assert.h>
+#endif
+
+#if defined(DEBUG) || defined(_DEBUG)
+#define btAssert assert
+#else
+#define btAssert(x)
+#endif
+
+//btFullAssert is optional, slows down a lot
+#define btFullAssert(x)
+#define btLikely(_c) _c
+#define btUnlikely(_c) _c
+#endif //__APPLE__
+
+#endif // LIBSPE2
+
+#endif //__CELLOS_LV2__
+#endif
+
+///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
+#if defined(BT_USE_DOUBLE_PRECISION)
+typedef double btScalar;
+//this number could be bigger in double precision
+#define BT_LARGE_FLOAT 1e30
+#else
+typedef float btScalar;
+//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
+#define BT_LARGE_FLOAT 1e18f
+#endif
+
+#define BT_DECLARE_ALIGNED_ALLOCATOR() \
+ SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); } \
+ SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \
+ SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \
+ SIMD_FORCE_INLINE void operator delete(void*, void*) {} \
+ SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); } \
+ SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); } \
+ SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \
+ SIMD_FORCE_INLINE void operator delete[](void*, void*) {}
+
+#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
+
+SIMD_FORCE_INLINE btScalar btSqrt(btScalar x)
+{
+ return sqrt(x);
+}
+SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
+SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
+SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
+SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x)
+{
+ if (x < btScalar(-1))
+ x = btScalar(-1);
+ if (x > btScalar(1))
+ x = btScalar(1);
+ return acos(x);
+}
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x)
+{
+ if (x < btScalar(-1))
+ x = btScalar(-1);
+ if (x > btScalar(1))
+ x = btScalar(1);
+ return asin(x);
+}
+SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
+SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
+SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
+SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
+SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return pow(x, y); }
+SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmod(x, y); }
+
+#else
+
+SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
+{
+#ifdef USE_APPROXIMATION
+ double x, z, tempf;
+ unsigned long* tfptr = ((unsigned long*)&tempf) + 1;
+
+ tempf = y;
+ *tfptr = (0xbfcdd90a - *tfptr) >> 1; /* estimate of 1/sqrt(y) */
+ x = tempf;
+ z = y * btScalar(0.5);
+ x = (btScalar(1.5) * x) - (x * x) * (x * z); /* iteration formula */
+ x = (btScalar(1.5) * x) - (x * x) * (x * z);
+ x = (btScalar(1.5) * x) - (x * x) * (x * z);
+ x = (btScalar(1.5) * x) - (x * x) * (x * z);
+ x = (btScalar(1.5) * x) - (x * x) * (x * z);
+ return x * y;
+#else
+ return sqrtf(y);
+#endif
+}
+SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
+SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
+SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
+SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
+SIMD_FORCE_INLINE btScalar btAcos(btScalar x)
+{
+ if (x < btScalar(-1))
+ x = btScalar(-1);
+ if (x > btScalar(1))
+ x = btScalar(1);
+ return acosf(x);
+}
+SIMD_FORCE_INLINE btScalar btAsin(btScalar x)
+{
+ if (x < btScalar(-1))
+ x = btScalar(-1);
+ if (x > btScalar(1))
+ x = btScalar(1);
+ return asinf(x);
+}
+SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
+SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
+SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
+SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
+SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return powf(x, y); }
+SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmodf(x, y); }
+
+#endif
+
+#define SIMD_2_PI btScalar(6.283185307179586232)
+#define SIMD_PI (SIMD_2_PI * btScalar(0.5))
+#define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25))
+#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
+#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI)
+#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
+
+#define btRecipSqrt(x) ((btScalar)(btScalar(1.0) / btSqrt(btScalar(x)))) /* reciprocal square root */
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define SIMD_EPSILON DBL_EPSILON
+#define SIMD_INFINITY DBL_MAX
+#else
+#define SIMD_EPSILON FLT_EPSILON
+#define SIMD_INFINITY FLT_MAX
+#endif
+
+SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x)
+{
+ btScalar coeff_1 = SIMD_PI / 4.0f;
+ btScalar coeff_2 = 3.0f * coeff_1;
+ btScalar abs_y = btFabs(y);
+ btScalar angle;
+ if (x >= 0.0f) {
+ btScalar r = (x - abs_y) / (x + abs_y);
+ angle = coeff_1 - coeff_1 * r;
+ }
+ else {
+ btScalar r = (x + abs_y) / (abs_y - x);
+ angle = coeff_2 - coeff_1 * r;
+ }
+ return (y < 0.0f) ? -angle : angle;
+}
+
+SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
+
+SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps)
+{
+ return (((a) <= eps) && !((a) < -eps));
+}
+SIMD_FORCE_INLINE bool btGreaterEqual(btScalar a, btScalar eps)
+{
+ return (!((a) <= eps));
+}
+
+SIMD_FORCE_INLINE int btIsNegative(btScalar x)
+{
+ return x < btScalar(0.0) ? 1 : 0;
+}
+
+SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
+SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
+
+#define BT_DECLARE_HANDLE(name) \
+ typedef struct name##__ { \
+ int unused; \
+ } * name
+
+#ifndef btFsel
+SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
+{
+ return a >= 0 ? b : c;
+}
+#endif
+#define btFsels(a, b, c) (btScalar) btFsel(a, b, c)
+
+SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
+{
+ long int i = 1;
+ const char* p = (const char*)&i;
+ if (p[0] == 1) // Lowest address contains the least significant byte
+ return true;
+ else
+ return false;
+}
+
+///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
+///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
+SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
+{
+ // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
+ // Rely on positive value or'ed with its negative having sign bit on
+ // and zero value or'ed with its negative (which is still zero) having sign bit off
+ // Use arithmetic shift right, shifting the sign bit through all 32 bits
+ unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+ unsigned testEqz = ~testNz;
+ return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+}
+SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
+{
+ unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+ unsigned testEqz = ~testNz;
+ return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+}
+SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
+{
+#ifdef BT_HAVE_NATIVE_FSEL
+ return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
+#else
+ return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
+#endif
+}
+
+template <typename T>
+SIMD_FORCE_INLINE void btSwap(T& a, T& b)
+{
+ T tmp = a;
+ a = b;
+ b = tmp;
+}
+
+//PCK: endian swapping functions
+SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)
+{
+ return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
+}
+
+SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val)
+{
+ return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
+}
+
+SIMD_FORCE_INLINE unsigned btSwapEndian(int val)
+{
+ return btSwapEndian((unsigned)val);
+}
+
+SIMD_FORCE_INLINE unsigned short btSwapEndian(short val)
+{
+ return btSwapEndian((unsigned short)val);
+}
+
+///btSwapFloat uses using char pointers to swap the endianness
+////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
+///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
+///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
+///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
+///so instead of returning a float/double, we return integer/long long integer
+SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d)
+{
+ unsigned int a = 0;
+ unsigned char* dst = (unsigned char*)&a;
+ unsigned char* src = (unsigned char*)&d;
+
+ dst[0] = src[3];
+ dst[1] = src[2];
+ dst[2] = src[1];
+ dst[3] = src[0];
+ return a;
+}
+
+// unswap using char pointers
+SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a)
+{
+ float d = 0.0f;
+ unsigned char* src = (unsigned char*)&a;
+ unsigned char* dst = (unsigned char*)&d;
+
+ dst[0] = src[3];
+ dst[1] = src[2];
+ dst[2] = src[1];
+ dst[3] = src[0];
+
+ return d;
+}
+
+// swap using char pointers
+SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst)
+{
+ unsigned char* src = (unsigned char*)&d;
+
+ dst[0] = src[7];
+ dst[1] = src[6];
+ dst[2] = src[5];
+ dst[3] = src[4];
+ dst[4] = src[3];
+ dst[5] = src[2];
+ dst[6] = src[1];
+ dst[7] = src[0];
+}
+
+// unswap using char pointers
+SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char* src)
+{
+ double d = 0.0;
+ unsigned char* dst = (unsigned char*)&d;
+
+ dst[0] = src[7];
+ dst[1] = src[6];
+ dst[2] = src[5];
+ dst[3] = src[4];
+ dst[4] = src[3];
+ dst[5] = src[2];
+ dst[6] = src[1];
+ dst[7] = src[0];
+
+ return d;
+}
+
+// returns normalized value in range [-SIMD_PI, SIMD_PI]
+SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
+{
+ angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
+ if (angleInRadians < -SIMD_PI) {
+ return angleInRadians + SIMD_2_PI;
+ }
+ else if (angleInRadians > SIMD_PI) {
+ return angleInRadians - SIMD_2_PI;
+ }
+ else {
+ return angleInRadians;
+ }
+}
+
+///rudimentary class to provide type info
+struct btTypedObject {
+ btTypedObject(int objectType)
+ : m_objectType(objectType)
+ {
+ }
+ int m_objectType;
+ inline int getObjectType() const
+ {
+ return m_objectType;
+ }
+};
+#endif //BT_SCALAR_H
diff --git a/Extras/VHACD/inc/btVector3.h b/Extras/VHACD/inc/btVector3.h
new file mode 100644
index 0000000..159d67b
--- /dev/null
+++ b/Extras/VHACD/inc/btVector3.h
@@ -0,0 +1,715 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_VECTOR3_H
+#define BT_VECTOR3_H
+
+#include "btMinMax.h"
+#include "btScalar.h"
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btVector3Data btVector3DoubleData
+#define btVector3DataName "btVector3DoubleData"
+#else
+#define btVector3Data btVector3FloatData
+#define btVector3DataName "btVector3FloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+/**@brief btVector3 can be used to represent 3D points and vectors.
+ * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
+ * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
+ */
+ATTRIBUTE_ALIGNED16(class)
+btVector3
+{
+public:
+#if defined(__SPU__) && defined(__CELLOS_LV2__)
+ btScalar m_floats[4];
+
+public:
+ SIMD_FORCE_INLINE const vec_float4& get128() const
+ {
+ return *((const vec_float4*)&m_floats[0]);
+ }
+
+public:
+#else //__CELLOS_LV2__ __SPU__
+#ifdef BT_USE_SSE // _WIN32
+ union {
+ __m128 mVec128;
+ btScalar m_floats[4];
+ };
+ SIMD_FORCE_INLINE __m128 get128() const
+ {
+ return mVec128;
+ }
+ SIMD_FORCE_INLINE void set128(__m128 v128)
+ {
+ mVec128 = v128;
+ }
+#else
+ btScalar m_floats[4];
+#endif
+#endif //__CELLOS_LV2__ __SPU__
+
+public:
+ /**@brief No initialization constructor */
+ SIMD_FORCE_INLINE btVector3() {}
+
+ /**@brief Constructor from scalars
+ * @param x X value
+ * @param y Y value
+ * @param z Z value
+ */
+ SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
+ {
+ m_floats[0] = x;
+ m_floats[1] = y;
+ m_floats[2] = z;
+ m_floats[3] = btScalar(0.);
+ }
+
+ /**@brief Add a vector to this one
+ * @param The vector to add to this one */
+ SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
+ {
+
+ m_floats[0] += v.m_floats[0];
+ m_floats[1] += v.m_floats[1];
+ m_floats[2] += v.m_floats[2];
+ return *this;
+ }
+
+ /**@brief Subtract a vector from this one
+ * @param The vector to subtract */
+ SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
+ {
+ m_floats[0] -= v.m_floats[0];
+ m_floats[1] -= v.m_floats[1];
+ m_floats[2] -= v.m_floats[2];
+ return *this;
+ }
+ /**@brief Scale the vector
+ * @param s Scale factor */
+ SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
+ {
+ m_floats[0] *= s;
+ m_floats[1] *= s;
+ m_floats[2] *= s;
+ return *this;
+ }
+
+ /**@brief Inversely scale the vector
+ * @param s Scale factor to divide by */
+ SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s)
+ {
+ btFullAssert(s != btScalar(0.0));
+ return * this *= btScalar(1.0) / s;
+ }
+
+ /**@brief Return the dot product
+ * @param v The other vector in the dot product */
+ SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
+ {
+ return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] + m_floats[2] * v.m_floats[2];
+ }
+
+ /**@brief Return the length of the vector squared */
+ SIMD_FORCE_INLINE btScalar length2() const
+ {
+ return dot(*this);
+ }
+
+ /**@brief Return the length of the vector */
+ SIMD_FORCE_INLINE btScalar length() const
+ {
+ return btSqrt(length2());
+ }
+
+ /**@brief Return the distance squared between the ends of this and another vector
+ * This is symantically treating the vector like a point */
+ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
+
+ /**@brief Return the distance between the ends of this and another vector
+ * This is symantically treating the vector like a point */
+ SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
+
+ SIMD_FORCE_INLINE btVector3& safeNormalize()
+ {
+ btVector3 absVec = this->absolute();
+ int maxIndex = absVec.maxAxis();
+ if (absVec[maxIndex] > 0) {
+ *this /= absVec[maxIndex];
+ return * this /= length();
+ }
+ setValue(1, 0, 0);
+ return *this;
+ }
+
+ /**@brief Normalize this vector
+ * x^2 + y^2 + z^2 = 1 */
+ SIMD_FORCE_INLINE btVector3& normalize()
+ {
+ return * this /= length();
+ }
+
+ /**@brief Return a normalized version of this vector */
+ SIMD_FORCE_INLINE btVector3 normalized() const;
+
+ /**@brief Return a rotated version of this vector
+ * @param wAxis The axis to rotate about
+ * @param angle The angle to rotate by */
+ SIMD_FORCE_INLINE btVector3 rotate(const btVector3& wAxis, const btScalar angle) const;
+
+ /**@brief Return the angle between this and another vector
+ * @param v The other vector */
+ SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const
+ {
+ btScalar s = btSqrt(length2() * v.length2());
+ btFullAssert(s != btScalar(0.0));
+ return btAcos(dot(v) / s);
+ }
+ /**@brief Return a vector will the absolute values of each element */
+ SIMD_FORCE_INLINE btVector3 absolute() const
+ {
+ return btVector3(
+ btFabs(m_floats[0]),
+ btFabs(m_floats[1]),
+ btFabs(m_floats[2]));
+ }
+ /**@brief Return the cross product between this and another vector
+ * @param v The other vector */
+ SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
+ {
+ return btVector3(
+ m_floats[1] * v.m_floats[2] - m_floats[2] * v.m_floats[1],
+ m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
+ m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
+ }
+
+ SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
+ {
+ return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
+ }
+
+ /**@brief Return the axis with the smallest value
+ * Note return values are 0,1,2 for x, y, or z */
+ SIMD_FORCE_INLINE int minAxis() const
+ {
+ return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
+ }
+
+ /**@brief Return the axis with the largest value
+ * Note return values are 0,1,2 for x, y, or z */
+ SIMD_FORCE_INLINE int maxAxis() const
+ {
+ return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
+ }
+
+ SIMD_FORCE_INLINE int furthestAxis() const
+ {
+ return absolute().minAxis();
+ }
+
+ SIMD_FORCE_INLINE int closestAxis() const
+ {
+ return absolute().maxAxis();
+ }
+
+ SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
+ {
+ btScalar s = btScalar(1.0) - rt;
+ m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
+ m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
+ m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
+ //don't do the unused w component
+ // m_co[3] = s * v0[3] + rt * v1[3];
+ }
+
+ /**@brief Return the linear interpolation between this and another vector
+ * @param v The other vector
+ * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
+ SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
+ {
+ return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
+ m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
+ m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
+ }
+
+ /**@brief Elementwise multiply this vector by the other
+ * @param v The other vector */
+ SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
+ {
+ m_floats[0] *= v.m_floats[0];
+ m_floats[1] *= v.m_floats[1];
+ m_floats[2] *= v.m_floats[2];
+ return *this;
+ }
+
+ /**@brief Return the x value */
+ SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
+ /**@brief Return the y value */
+ SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
+ /**@brief Return the z value */
+ SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
+ /**@brief Set the x value */
+ SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x; };
+ /**@brief Set the y value */
+ SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y; };
+ /**@brief Set the z value */
+ SIMD_FORCE_INLINE void setZ(btScalar z) { m_floats[2] = z; };
+ /**@brief Set the w value */
+ SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w; };
+ /**@brief Return the x value */
+ SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
+ /**@brief Return the y value */
+ SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
+ /**@brief Return the z value */
+ SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
+ /**@brief Return the w value */
+ SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
+
+ //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
+ //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
+ ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+ SIMD_FORCE_INLINE operator btScalar*() { return &m_floats[0]; }
+ SIMD_FORCE_INLINE operator const btScalar*() const { return &m_floats[0]; }
+
+ SIMD_FORCE_INLINE bool operator==(const btVector3& other) const
+ {
+ return ((m_floats[3] == other.m_floats[3]) && (m_floats[2] == other.m_floats[2]) && (m_floats[1] == other.m_floats[1]) && (m_floats[0] == other.m_floats[0]));
+ }
+
+ SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const
+ {
+ return !(*this == other);
+ }
+
+ /**@brief Set each element to the max of the current values and the values of another btVector3
+ * @param other The other btVector3 to compare with
+ */
+ SIMD_FORCE_INLINE void setMax(const btVector3& other)
+ {
+ btSetMax(m_floats[0], other.m_floats[0]);
+ btSetMax(m_floats[1], other.m_floats[1]);
+ btSetMax(m_floats[2], other.m_floats[2]);
+ btSetMax(m_floats[3], other.w());
+ }
+ /**@brief Set each element to the min of the current values and the values of another btVector3
+ * @param other The other btVector3 to compare with
+ */
+ SIMD_FORCE_INLINE void setMin(const btVector3& other)
+ {
+ btSetMin(m_floats[0], other.m_floats[0]);
+ btSetMin(m_floats[1], other.m_floats[1]);
+ btSetMin(m_floats[2], other.m_floats[2]);
+ btSetMin(m_floats[3], other.w());
+ }
+
+ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z)
+ {
+ m_floats[0] = x;
+ m_floats[1] = y;
+ m_floats[2] = z;
+ m_floats[3] = btScalar(0.);
+ }
+
+ void getSkewSymmetricMatrix(btVector3 * v0, btVector3 * v1, btVector3 * v2) const
+ {
+ v0->setValue(0., -z(), y());
+ v1->setValue(z(), 0., -x());
+ v2->setValue(-y(), x(), 0.);
+ }
+
+ void setZero()
+ {
+ setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+ }
+
+ SIMD_FORCE_INLINE bool isZero() const
+ {
+ return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
+ }
+
+ SIMD_FORCE_INLINE bool fuzzyZero() const
+ {
+ return length2() < SIMD_EPSILON;
+ }
+
+ SIMD_FORCE_INLINE void serialize(struct btVector3Data & dataOut) const;
+
+ SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn);
+
+ SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData & dataOut) const;
+
+ SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn);
+
+ SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData & dataOut) const;
+
+ SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn);
+};
+
+/**@brief Return the sum of two vectors (Point symantics)*/
+SIMD_FORCE_INLINE btVector3
+operator+(const btVector3& v1, const btVector3& v2)
+{
+ return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
+}
+
+/**@brief Return the elementwise product of two vectors */
+SIMD_FORCE_INLINE btVector3
+operator*(const btVector3& v1, const btVector3& v2)
+{
+ return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
+}
+
+/**@brief Return the difference between two vectors */
+SIMD_FORCE_INLINE btVector3
+operator-(const btVector3& v1, const btVector3& v2)
+{
+ return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
+}
+/**@brief Return the negative of the vector */
+SIMD_FORCE_INLINE btVector3
+operator-(const btVector3& v)
+{
+ return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
+}
+
+/**@brief Return the vector scaled by s */
+SIMD_FORCE_INLINE btVector3
+operator*(const btVector3& v, const btScalar& s)
+{
+ return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
+}
+
+/**@brief Return the vector scaled by s */
+SIMD_FORCE_INLINE btVector3
+operator*(const btScalar& s, const btVector3& v)
+{
+ return v * s;
+}
+
+/**@brief Return the vector inversely scaled by s */
+SIMD_FORCE_INLINE btVector3
+operator/(const btVector3& v, const btScalar& s)
+{
+ btFullAssert(s != btScalar(0.0));
+ return v * (btScalar(1.0) / s);
+}
+
+/**@brief Return the vector inversely scaled by s */
+SIMD_FORCE_INLINE btVector3
+operator/(const btVector3& v1, const btVector3& v2)
+{
+ return btVector3(v1.m_floats[0] / v2.m_floats[0], v1.m_floats[1] / v2.m_floats[1], v1.m_floats[2] / v2.m_floats[2]);
+}
+
+/**@brief Return the dot product between two vectors */
+SIMD_FORCE_INLINE btScalar
+btDot(const btVector3& v1, const btVector3& v2)
+{
+ return v1.dot(v2);
+}
+
+/**@brief Return the distance squared between two vectors */
+SIMD_FORCE_INLINE btScalar
+btDistance2(const btVector3& v1, const btVector3& v2)
+{
+ return v1.distance2(v2);
+}
+
+/**@brief Return the distance between two vectors */
+SIMD_FORCE_INLINE btScalar
+btDistance(const btVector3& v1, const btVector3& v2)
+{
+ return v1.distance(v2);
+}
+
+/**@brief Return the angle between two vectors */
+SIMD_FORCE_INLINE btScalar
+btAngle(const btVector3& v1, const btVector3& v2)
+{
+ return v1.angle(v2);
+}
+
+/**@brief Return the cross product of two vectors */
+SIMD_FORCE_INLINE btVector3
+btCross(const btVector3& v1, const btVector3& v2)
+{
+ return v1.cross(v2);
+}
+
+SIMD_FORCE_INLINE btScalar
+btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
+{
+ return v1.triple(v2, v3);
+}
+
+/**@brief Return the linear interpolation between two vectors
+ * @param v1 One vector
+ * @param v2 The other vector
+ * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
+SIMD_FORCE_INLINE btVector3
+lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
+{
+ return v1.lerp(v2, t);
+}
+
+SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
+{
+ return (v - *this).length2();
+}
+
+SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
+{
+ return (v - *this).length();
+}
+
+SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
+{
+ return *this / length();
+}
+
+SIMD_FORCE_INLINE btVector3 btVector3::rotate(const btVector3& wAxis, const btScalar angle) const
+{
+ // wAxis must be a unit lenght vector
+
+ btVector3 o = wAxis * wAxis.dot(*this);
+ btVector3 x = *this - o;
+ btVector3 y;
+
+ y = wAxis.cross(*this);
+
+ return (o + x * btCos(angle) + y * btSin(angle));
+}
+
+class btVector4 : public btVector3 {
+public:
+ SIMD_FORCE_INLINE btVector4() {}
+
+ SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w)
+ : btVector3(x, y, z)
+ {
+ m_floats[3] = w;
+ }
+
+ SIMD_FORCE_INLINE btVector4 absolute4() const
+ {
+ return btVector4(
+ btFabs(m_floats[0]),
+ btFabs(m_floats[1]),
+ btFabs(m_floats[2]),
+ btFabs(m_floats[3]));
+ }
+
+ btScalar getW() const { return m_floats[3]; }
+
+ SIMD_FORCE_INLINE int maxAxis4() const
+ {
+ int maxIndex = -1;
+ btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
+ if (m_floats[0] > maxVal) {
+ maxIndex = 0;
+ maxVal = m_floats[0];
+ }
+ if (m_floats[1] > maxVal) {
+ maxIndex = 1;
+ maxVal = m_floats[1];
+ }
+ if (m_floats[2] > maxVal) {
+ maxIndex = 2;
+ maxVal = m_floats[2];
+ }
+ if (m_floats[3] > maxVal) {
+ maxIndex = 3;
+ }
+ return maxIndex;
+ }
+
+ SIMD_FORCE_INLINE int minAxis4() const
+ {
+ int minIndex = -1;
+ btScalar minVal = btScalar(BT_LARGE_FLOAT);
+ if (m_floats[0] < minVal) {
+ minIndex = 0;
+ minVal = m_floats[0];
+ }
+ if (m_floats[1] < minVal) {
+ minIndex = 1;
+ minVal = m_floats[1];
+ }
+ if (m_floats[2] < minVal) {
+ minIndex = 2;
+ minVal = m_floats[2];
+ }
+ if (m_floats[3] < minVal) {
+ minIndex = 3;
+ }
+
+ return minIndex;
+ }
+
+ SIMD_FORCE_INLINE int closestAxis4() const
+ {
+ return absolute4().maxAxis4();
+ }
+
+ /**@brief Set x,y,z and zero w
+ * @param x Value of x
+ * @param y Value of y
+ * @param z Value of z
+ */
+
+ /* void getValue(btScalar *m) const
+ {
+ m[0] = m_floats[0];
+ m[1] = m_floats[1];
+ m[2] =m_floats[2];
+ }
+*/
+ /**@brief Set the values
+ * @param x Value of x
+ * @param y Value of y
+ * @param z Value of z
+ * @param w Value of w
+ */
+ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w)
+ {
+ m_floats[0] = x;
+ m_floats[1] = y;
+ m_floats[2] = z;
+ m_floats[3] = w;
+ }
+};
+
+///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
+{
+#ifdef BT_USE_DOUBLE_PRECISION
+ unsigned char* dest = (unsigned char*)&destVal;
+ unsigned char* src = (unsigned char*)&sourceVal;
+ dest[0] = src[7];
+ dest[1] = src[6];
+ dest[2] = src[5];
+ dest[3] = src[4];
+ dest[4] = src[3];
+ dest[5] = src[2];
+ dest[6] = src[1];
+ dest[7] = src[0];
+#else
+ unsigned char* dest = (unsigned char*)&destVal;
+ unsigned char* src = (unsigned char*)&sourceVal;
+ dest[0] = src[3];
+ dest[1] = src[2];
+ dest[2] = src[1];
+ dest[3] = src[0];
+#endif //BT_USE_DOUBLE_PRECISION
+}
+///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
+{
+ for (int i = 0; i < 4; i++) {
+ btSwapScalarEndian(sourceVec[i], destVec[i]);
+ }
+}
+
+///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector)
+{
+
+ btVector3 swappedVec;
+ for (int i = 0; i < 4; i++) {
+ btSwapScalarEndian(vector[i], swappedVec[i]);
+ }
+ vector = swappedVec;
+}
+
+template <class T>
+SIMD_FORCE_INLINE void btPlaneSpace1(const T& n, T& p, T& q)
+{
+ if (btFabs(n[2]) > SIMDSQRT12) {
+ // choose p in y-z plane
+ btScalar a = n[1] * n[1] + n[2] * n[2];
+ btScalar k = btRecipSqrt(a);
+ p[0] = 0;
+ p[1] = -n[2] * k;
+ p[2] = n[1] * k;
+ // set q = n x p
+ q[0] = a * k;
+ q[1] = -n[0] * p[2];
+ q[2] = n[0] * p[1];
+ }
+ else {
+ // choose p in x-y plane
+ btScalar a = n[0] * n[0] + n[1] * n[1];
+ btScalar k = btRecipSqrt(a);
+ p[0] = -n[1] * k;
+ p[1] = n[0] * k;
+ p[2] = 0;
+ // set q = n x p
+ q[0] = -n[2] * p[1];
+ q[1] = n[2] * p[0];
+ q[2] = a * k;
+ }
+}
+
+struct btVector3FloatData {
+ float m_floats[4];
+};
+
+struct btVector3DoubleData {
+ double m_floats[4];
+};
+
+SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const
+{
+ ///could also do a memcpy, check if it is worth it
+ for (int i = 0; i < 4; i++)
+ dataOut.m_floats[i] = float(m_floats[i]);
+}
+
+SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn)
+{
+ for (int i = 0; i < 4; i++)
+ m_floats[i] = btScalar(dataIn.m_floats[i]);
+}
+
+SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const
+{
+ ///could also do a memcpy, check if it is worth it
+ for (int i = 0; i < 4; i++)
+ dataOut.m_floats[i] = double(m_floats[i]);
+}
+
+SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn)
+{
+ for (int i = 0; i < 4; i++)
+ m_floats[i] = btScalar(dataIn.m_floats[i]);
+}
+
+SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const
+{
+ ///could also do a memcpy, check if it is worth it
+ for (int i = 0; i < 4; i++)
+ dataOut.m_floats[i] = m_floats[i];
+}
+
+SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn)
+{
+ for (int i = 0; i < 4; i++)
+ m_floats[i] = dataIn.m_floats[i];
+}
+
+#endif //BT_VECTOR3_H
diff --git a/Extras/VHACD/inc/vhacdCircularList.h b/Extras/VHACD/inc/vhacdCircularList.h
new file mode 100644
index 0000000..0f5ddf9
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdCircularList.h
@@ -0,0 +1,79 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#pragma once
+#ifndef VHACD_CIRCULAR_LIST_H
+#define VHACD_CIRCULAR_LIST_H
+#include <stdlib.h>
+namespace VHACD {
+//! CircularListElement class.
+template <typename T>
+class CircularListElement {
+public:
+ T& GetData() { return m_data; }
+ const T& GetData() const { return m_data; }
+ CircularListElement<T>*& GetNext() { return m_next; }
+ CircularListElement<T>*& GetPrev() { return m_prev; }
+ const CircularListElement<T>*& GetNext() const { return m_next; }
+ const CircularListElement<T>*& GetPrev() const { return m_prev; }
+ //! Constructor
+ CircularListElement(const T& data) { m_data = data; }
+ CircularListElement(void) {}
+ //! Destructor
+ ~CircularListElement(void) {}
+private:
+ T m_data;
+ CircularListElement<T>* m_next;
+ CircularListElement<T>* m_prev;
+
+ CircularListElement(const CircularListElement& rhs);
+};
+//! CircularList class.
+template <typename T>
+class CircularList {
+public:
+ CircularListElement<T>*& GetHead() { return m_head; }
+ const CircularListElement<T>* GetHead() const { return m_head; }
+ bool IsEmpty() const { return (m_size == 0); }
+ size_t GetSize() const { return m_size; }
+ const T& GetData() const { return m_head->GetData(); }
+ T& GetData() { return m_head->GetData(); }
+ bool Delete();
+ bool Delete(CircularListElement<T>* element);
+ CircularListElement<T>* Add(const T* data = 0);
+ CircularListElement<T>* Add(const T& data);
+ bool Next();
+ bool Prev();
+ void Clear()
+ {
+ while (Delete())
+ ;
+ };
+ const CircularList& operator=(const CircularList& rhs);
+ //! Constructor
+ CircularList()
+ {
+ m_head = 0;
+ m_size = 0;
+ }
+ CircularList(const CircularList& rhs);
+ //! Destructor
+ ~CircularList(void) { Clear(); };
+private:
+ CircularListElement<T>* m_head; //!< a pointer to the head of the circular list
+ size_t m_size; //!< number of element in the circular list
+};
+}
+#include "vhacdCircularList.inl"
+#endif // VHACD_CIRCULAR_LIST_H
\ No newline at end of file
diff --git a/Extras/VHACD/inc/vhacdCircularList.inl b/Extras/VHACD/inc/vhacdCircularList.inl
new file mode 100644
index 0000000..2be5180
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdCircularList.inl
@@ -0,0 +1,161 @@
+#pragma once
+#ifndef HACD_CIRCULAR_LIST_INL
+#define HACD_CIRCULAR_LIST_INL
+namespace VHACD
+{
+ template < typename T >
+ inline bool CircularList<T>::Delete(CircularListElement<T> * element)
+ {
+ if (!element)
+ {
+ return false;
+ }
+ if (m_size > 1)
+ {
+ CircularListElement<T> * next = element->GetNext();
+ CircularListElement<T> * prev = element->GetPrev();
+ delete element;
+ m_size--;
+ if (element == m_head)
+ {
+ m_head = next;
+ }
+ next->GetPrev() = prev;
+ prev->GetNext() = next;
+ return true;
+ }
+ else if (m_size == 1)
+ {
+ delete m_head;
+ m_size--;
+ m_head = 0;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+ template < typename T >
+ inline bool CircularList<T>::Delete()
+ {
+ if (m_size > 1)
+ {
+ CircularListElement<T> * next = m_head->GetNext();
+ CircularListElement<T> * prev = m_head->GetPrev();
+ delete m_head;
+ m_size--;
+ m_head = next;
+ next->GetPrev() = prev;
+ prev->GetNext() = next;
+ return true;
+ }
+ else if (m_size == 1)
+ {
+ delete m_head;
+ m_size--;
+ m_head = 0;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+ template < typename T >
+ inline CircularListElement<T> * CircularList<T>::Add(const T * data)
+ {
+ if (m_size == 0)
+ {
+ if (data)
+ {
+ m_head = new CircularListElement<T>(*data);
+ }
+ else
+ {
+ m_head = new CircularListElement<T>();
+ }
+ m_head->GetNext() = m_head->GetPrev() = m_head;
+ }
+ else
+ {
+ CircularListElement<T> * next = m_head->GetNext();
+ CircularListElement<T> * element = m_head;
+ if (data)
+ {
+ m_head = new CircularListElement<T>(*data);
+ }
+ else
+ {
+ m_head = new CircularListElement<T>();
+ }
+ m_head->GetNext() = next;
+ m_head->GetPrev() = element;
+ element->GetNext() = m_head;
+ next->GetPrev() = m_head;
+ }
+ m_size++;
+ return m_head;
+ }
+ template < typename T >
+ inline CircularListElement<T> * CircularList<T>::Add(const T & data)
+ {
+ const T * pData = &data;
+ return Add(pData);
+ }
+ template < typename T >
+ inline bool CircularList<T>::Next()
+ {
+ if (m_size == 0)
+ {
+ return false;
+ }
+ m_head = m_head->GetNext();
+ return true;
+ }
+ template < typename T >
+ inline bool CircularList<T>::Prev()
+ {
+ if (m_size == 0)
+ {
+ return false;
+ }
+ m_head = m_head->GetPrev();
+ return true;
+ }
+ template < typename T >
+ inline CircularList<T>::CircularList(const CircularList& rhs)
+ {
+ if (rhs.m_size > 0)
+ {
+ CircularListElement<T> * current = rhs.m_head;
+ do
+ {
+ current = current->GetNext();
+ Add(current->GetData());
+ }
+ while ( current != rhs.m_head );
+ }
+ }
+ template < typename T >
+ inline const CircularList<T>& CircularList<T>::operator=(const CircularList& rhs)
+ {
+ if (&rhs != this)
+ {
+ Clear();
+ if (rhs.m_size > 0)
+ {
+ CircularListElement<T> * current = rhs.m_head;
+ do
+ {
+ current = current->GetNext();
+ Add(current->GetData());
+ }
+ while ( current != rhs.m_head );
+ }
+ }
+ return (*this);
+ }
+}
+#endif
\ No newline at end of file
diff --git a/Extras/VHACD/inc/vhacdICHull.h b/Extras/VHACD/inc/vhacdICHull.h
new file mode 100644
index 0000000..a3d9416
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdICHull.h
@@ -0,0 +1,98 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#pragma once
+#ifndef VHACD_ICHULL_H
+#define VHACD_ICHULL_H
+#include "vhacdManifoldMesh.h"
+#include "vhacdVector.h"
+
+namespace VHACD {
+//! Incremental Convex Hull algorithm (cf. http://cs.smith.edu/~orourke/books/ftp.html ).
+enum ICHullError {
+ ICHullErrorOK = 0,
+ ICHullErrorCoplanarPoints,
+ ICHullErrorNoVolume,
+ ICHullErrorInconsistent,
+ ICHullErrorNotEnoughPoints
+};
+class ICHull {
+public:
+ static const double sc_eps;
+ //!
+ bool IsFlat() { return m_isFlat; }
+ //! Returns the computed mesh
+ TMMesh& GetMesh() { return m_mesh; }
+ //! Add one point to the convex-hull
+ bool AddPoint(const Vec3<double>& point) { return AddPoints(&point, 1); }
+ //! Add one point to the convex-hull
+ bool AddPoint(const Vec3<double>& point, int id);
+ //! Add points to the convex-hull
+ bool AddPoints(const Vec3<double>* points, size_t nPoints);
+ //!
+ ICHullError Process();
+ //!
+ ICHullError Process(const unsigned int nPointsCH, const double minVolume = 0.0);
+ //!
+ bool IsInside(const Vec3<double>& pt0, const double eps = 0.0);
+ //!
+ const ICHull& operator=(ICHull& rhs);
+
+ //! Constructor
+ ICHull();
+ //! Destructor
+ ~ICHull(void){};
+
+private:
+ //! DoubleTriangle builds the initial double triangle. It first finds 3 noncollinear points and makes two faces out of them, in opposite order. It then finds a fourth point that is not coplanar with that face. The vertices are stored in the face structure in counterclockwise order so that the volume between the face and the point is negative. Lastly, the 3 newfaces to the fourth point are constructed and the data structures are cleaned up.
+ ICHullError DoubleTriangle();
+ //! MakeFace creates a new face structure from three vertices (in ccw order). It returns a pointer to the face.
+ CircularListElement<TMMTriangle>* MakeFace(CircularListElement<TMMVertex>* v0,
+ CircularListElement<TMMVertex>* v1,
+ CircularListElement<TMMVertex>* v2,
+ CircularListElement<TMMTriangle>* fold);
+ //!
+ CircularListElement<TMMTriangle>* MakeConeFace(CircularListElement<TMMEdge>* e, CircularListElement<TMMVertex>* v);
+ //!
+ bool ProcessPoint();
+ //!
+ bool ComputePointVolume(double& totalVolume, bool markVisibleFaces);
+ //!
+ bool FindMaxVolumePoint(const double minVolume = 0.0);
+ //!
+ bool CleanEdges();
+ //!
+ bool CleanVertices(unsigned int& addedPoints);
+ //!
+ bool CleanTriangles();
+ //!
+ bool CleanUp(unsigned int& addedPoints);
+ //!
+ bool MakeCCW(CircularListElement<TMMTriangle>* f,
+ CircularListElement<TMMEdge>* e,
+ CircularListElement<TMMVertex>* v);
+ void Clear();
+
+private:
+ static const int sc_dummyIndex;
+ TMMesh m_mesh;
+ SArray<CircularListElement<TMMEdge>*> m_edgesToDelete;
+ SArray<CircularListElement<TMMEdge>*> m_edgesToUpdate;
+ SArray<CircularListElement<TMMTriangle>*> m_trianglesToDelete;
+ Vec3<double> m_normal;
+ bool m_isFlat;
+ ICHull(const ICHull& rhs);
+};
+}
+#endif // VHACD_ICHULL_H
diff --git a/Extras/VHACD/inc/vhacdManifoldMesh.h b/Extras/VHACD/inc/vhacdManifoldMesh.h
new file mode 100644
index 0000000..f178ad3
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdManifoldMesh.h
@@ -0,0 +1,142 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+*/
+#pragma once
+#ifndef VHACD_MANIFOLD_MESH_H
+#define VHACD_MANIFOLD_MESH_H
+#include "vhacdCircularList.h"
+#include "vhacdSArray.h"
+#include "vhacdVector.h"
+namespace VHACD {
+class TMMTriangle;
+class TMMEdge;
+class TMMesh;
+class ICHull;
+
+//! Vertex data structure used in a triangular manifold mesh (TMM).
+class TMMVertex {
+public:
+ void Initialize();
+ TMMVertex(void);
+ ~TMMVertex(void);
+
+private:
+ Vec3<double> m_pos;
+ int m_name;
+ size_t m_id;
+ CircularListElement<TMMEdge>* m_duplicate; // pointer to incident cone edge (or NULL)
+ bool m_onHull;
+ bool m_tag;
+ TMMVertex(const TMMVertex& rhs);
+ friend class ICHull;
+ friend class TMMesh;
+ friend class TMMTriangle;
+ friend class TMMEdge;
+};
+
+//! Edge data structure used in a triangular manifold mesh (TMM).
+class TMMEdge {
+public:
+ void Initialize();
+ TMMEdge(void);
+ ~TMMEdge(void);
+
+private:
+ size_t m_id;
+ CircularListElement<TMMTriangle>* m_triangles[2];
+ CircularListElement<TMMVertex>* m_vertices[2];
+ CircularListElement<TMMTriangle>* m_newFace;
+ TMMEdge(const TMMEdge& rhs);
+ friend class ICHull;
+ friend class TMMTriangle;
+ friend class TMMVertex;
+ friend class TMMesh;
+};
+
+//! Triangle data structure used in a triangular manifold mesh (TMM).
+class TMMTriangle {
+public:
+ void Initialize();
+ TMMTriangle(void);
+ ~TMMTriangle(void);
+
+private:
+ size_t m_id;
+ CircularListElement<TMMEdge>* m_edges[3];
+ CircularListElement<TMMVertex>* m_vertices[3];
+ bool m_visible;
+
+ TMMTriangle(const TMMTriangle& rhs);
+ friend class ICHull;
+ friend class TMMesh;
+ friend class TMMVertex;
+ friend class TMMEdge;
+};
+//! triangular manifold mesh data structure.
+class TMMesh {
+public:
+ //! Returns the number of vertices>
+ inline size_t GetNVertices() const { return m_vertices.GetSize(); }
+ //! Returns the number of edges
+ inline size_t GetNEdges() const { return m_edges.GetSize(); }
+ //! Returns the number of triangles
+ inline size_t GetNTriangles() const { return m_triangles.GetSize(); }
+ //! Returns the vertices circular list
+ inline const CircularList<TMMVertex>& GetVertices() const { return m_vertices; }
+ //! Returns the edges circular list
+ inline const CircularList<TMMEdge>& GetEdges() const { return m_edges; }
+ //! Returns the triangles circular list
+ inline const CircularList<TMMTriangle>& GetTriangles() const { return m_triangles; }
+ //! Returns the vertices circular list
+ inline CircularList<TMMVertex>& GetVertices() { return m_vertices; }
+ //! Returns the edges circular list
+ inline CircularList<TMMEdge>& GetEdges() { return m_edges; }
+ //! Returns the triangles circular list
+ inline CircularList<TMMTriangle>& GetTriangles() { return m_triangles; }
+ //! Add vertex to the mesh
+ CircularListElement<TMMVertex>* AddVertex() { return m_vertices.Add(); }
+ //! Add vertex to the mesh
+ CircularListElement<TMMEdge>* AddEdge() { return m_edges.Add(); }
+ //! Add vertex to the mesh
+ CircularListElement<TMMTriangle>* AddTriangle() { return m_triangles.Add(); }
+ //! Print mesh information
+ void Print();
+ //!
+ void GetIFS(Vec3<double>* const points, Vec3<int>* const triangles);
+ //!
+ void Clear();
+ //!
+ void Copy(TMMesh& mesh);
+ //!
+ bool CheckConsistancy();
+ //!
+ bool Normalize();
+ //!
+ bool Denormalize();
+ //! Constructor
+ TMMesh();
+ //! Destructor
+ virtual ~TMMesh(void);
+
+private:
+ CircularList<TMMVertex> m_vertices;
+ CircularList<TMMEdge> m_edges;
+ CircularList<TMMTriangle> m_triangles;
+
+ // not defined
+ TMMesh(const TMMesh& rhs);
+ friend class ICHull;
+};
+}
+#endif // VHACD_MANIFOLD_MESH_H
\ No newline at end of file
diff --git a/Extras/VHACD/inc/vhacdMesh.h b/Extras/VHACD/inc/vhacdMesh.h
new file mode 100644
index 0000000..0975d8e
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdMesh.h
@@ -0,0 +1,129 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#pragma once
+#ifndef VHACD_MESH_H
+#define VHACD_MESH_H
+#include "vhacdSArray.h"
+#include "vhacdVector.h"
+
+#define VHACD_DEBUG_MESH
+
+namespace VHACD {
+enum AXIS {
+ AXIS_X = 0,
+ AXIS_Y = 1,
+ AXIS_Z = 2
+};
+struct Plane {
+ double m_a;
+ double m_b;
+ double m_c;
+ double m_d;
+ AXIS m_axis;
+ short m_index;
+};
+#ifdef VHACD_DEBUG_MESH
+struct Material {
+
+ Vec3<double> m_diffuseColor;
+ double m_ambientIntensity;
+ Vec3<double> m_specularColor;
+ Vec3<double> m_emissiveColor;
+ double m_shininess;
+ double m_transparency;
+ Material(void)
+ {
+ m_diffuseColor.X() = 0.5;
+ m_diffuseColor.Y() = 0.5;
+ m_diffuseColor.Z() = 0.5;
+ m_specularColor.X() = 0.5;
+ m_specularColor.Y() = 0.5;
+ m_specularColor.Z() = 0.5;
+ m_ambientIntensity = 0.4;
+ m_emissiveColor.X() = 0.0;
+ m_emissiveColor.Y() = 0.0;
+ m_emissiveColor.Z() = 0.0;
+ m_shininess = 0.4;
+ m_transparency = 0.0;
+ };
+};
+#endif // VHACD_DEBUG_MESH
+
+//! Triangular mesh data structure
+class Mesh {
+public:
+ void AddPoint(const Vec3<double>& pt) { m_points.PushBack(pt); };
+ void SetPoint(size_t index, const Vec3<double>& pt) { m_points[index] = pt; };
+ const Vec3<double>& GetPoint(size_t index) const { return m_points[index]; };
+ Vec3<double>& GetPoint(size_t index) { return m_points[index]; };
+ size_t GetNPoints() const { return m_points.Size(); };
+ double* GetPoints() { return (double*)m_points.Data(); } // ugly
+ const double* const GetPoints() const { return (double*)m_points.Data(); } // ugly
+ const Vec3<double>* const GetPointsBuffer() const { return m_points.Data(); } //
+ Vec3<double>* const GetPointsBuffer() { return m_points.Data(); } //
+ void AddTriangle(const Vec3<int>& tri) { m_triangles.PushBack(tri); };
+ void SetTriangle(size_t index, const Vec3<int>& tri) { m_triangles[index] = tri; };
+ const Vec3<int>& GetTriangle(size_t index) const { return m_triangles[index]; };
+ Vec3<int>& GetTriangle(size_t index) { return m_triangles[index]; };
+ size_t GetNTriangles() const { return m_triangles.Size(); };
+ int* GetTriangles() { return (int*)m_triangles.Data(); } // ugly
+ const int* const GetTriangles() const { return (int*)m_triangles.Data(); } // ugly
+ const Vec3<int>* const GetTrianglesBuffer() const { return m_triangles.Data(); }
+ Vec3<int>* const GetTrianglesBuffer() { return m_triangles.Data(); }
+ const Vec3<double>& GetCenter() const { return m_center; }
+ const Vec3<double>& GetMinBB() const { return m_minBB; }
+ const Vec3<double>& GetMaxBB() const { return m_maxBB; }
+ void ClearPoints() { m_points.Clear(); }
+ void ClearTriangles() { m_triangles.Clear(); }
+ void Clear()
+ {
+ ClearPoints();
+ ClearTriangles();
+ }
+ void ResizePoints(size_t nPts) { m_points.Resize(nPts); }
+ void ResizeTriangles(size_t nTri) { m_triangles.Resize(nTri); }
+ void CopyPoints(SArray<Vec3<double> >& points) const { points = m_points; }
+ double GetDiagBB() const { return m_diag; }
+ double ComputeVolume() const;
+ void ComputeConvexHull(const double* const pts,
+ const size_t nPts);
+ void Clip(const Plane& plane,
+ SArray<Vec3<double> >& positivePart,
+ SArray<Vec3<double> >& negativePart) const;
+ bool IsInside(const Vec3<double>& pt) const;
+ double ComputeDiagBB();
+
+#ifdef VHACD_DEBUG_MESH
+ bool LoadOFF(const std::string& fileName, bool invert);
+ bool SaveVRML2(const std::string& fileName) const;
+ bool SaveVRML2(std::ofstream& fout, const Material& material) const;
+ bool SaveOFF(const std::string& fileName) const;
+#endif // VHACD_DEBUG_MESH
+
+ //! Constructor.
+ Mesh();
+ //! Destructor.
+ ~Mesh(void);
+
+private:
+ SArray<Vec3<double> > m_points;
+ SArray<Vec3<int> > m_triangles;
+ Vec3<double> m_minBB;
+ Vec3<double> m_maxBB;
+ Vec3<double> m_center;
+ double m_diag;
+};
+}
+#endif
\ No newline at end of file
diff --git a/Extras/VHACD/inc/vhacdMutex.h b/Extras/VHACD/inc/vhacdMutex.h
new file mode 100644
index 0000000..a28f6be
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdMutex.h
@@ -0,0 +1,146 @@
+/*!
+**
+** Copyright (c) 2009 by John W. Ratcliff mailto:jratcliffscarab at gmail.com
+**
+** Portions of this source has been released with the PhysXViewer application, as well as
+** Rocket, CreateDynamics, ODF, and as a number of sample code snippets.
+**
+** If you find this code useful or you are feeling particularily generous I would
+** ask that you please go to http://www.amillionpixels.us and make a donation
+** to Troy DeMolay.
+**
+** DeMolay is a youth group for young men between the ages of 12 and 21.
+** It teaches strong moral principles, as well as leadership skills and
+** public speaking. The donations page uses the 'pay for pixels' paradigm
+** where, in this case, a pixel is only a single penny. Donations can be
+** made for as small as $4 or as high as a $100 block. Each person who donates
+** will get a link to their own site as well as acknowledgement on the
+** donations blog located here http://www.amillionpixels.blogspot.com/
+**
+** If you wish to contact me you can use the following methods:
+**
+** Skype ID: jratcliff63367
+** Yahoo: jratcliff63367
+** AOL: jratcliff1961
+** email: jratcliffscarab at gmail.com
+**
+**
+** The MIT license:
+**
+** Permission is hereby granted, free of charge, to any person obtaining a copy
+** of this software and associated documentation files (the "Software"), to deal
+** in the Software without restriction, including without limitation the rights
+** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+** copies of the Software, and to permit persons to whom the Software is furnished
+** to do so, subject to the following conditions:
+**
+** The above copyright notice and this permission notice shall be included in all
+** copies or substantial portions of the Software.
+
+** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+*/
+
+#pragma once
+#ifndef VHACD_MUTEX_H
+#define VHACD_MUTEX_H
+
+#if defined(WIN32)
+
+//#define _WIN32_WINNT 0x400
+#include <windows.h>
+#pragma comment(lib, "winmm.lib")
+#endif
+
+#if defined(__linux__)
+//#include <sys/time.h>
+#include <errno.h>
+#include <time.h>
+#include <unistd.h>
+#define __stdcall
+#endif
+
+#if defined(__APPLE__) || defined(__linux__)
+#include <pthread.h>
+#endif
+
+#if defined(__APPLE__)
+#define PTHREAD_MUTEX_RECURSIVE_NP PTHREAD_MUTEX_RECURSIVE
+#endif
+
+#define VHACD_DEBUG
+
+//#define VHACD_NDEBUG
+#ifdef VHACD_NDEBUG
+#define VHACD_VERIFY(x) (x)
+#else
+#define VHACD_VERIFY(x) assert((x))
+#endif
+
+namespace VHACD {
+class Mutex {
+public:
+ Mutex(void)
+ {
+#if defined(WIN32) || defined(_XBOX)
+ InitializeCriticalSection(&m_mutex);
+#elif defined(__APPLE__) || defined(__linux__)
+ pthread_mutexattr_t mutexAttr; // Mutex Attribute
+ VHACD_VERIFY(pthread_mutexattr_init(&mutexAttr) == 0);
+ VHACD_VERIFY(pthread_mutexattr_settype(&mutexAttr, PTHREAD_MUTEX_RECURSIVE_NP) == 0);
+ VHACD_VERIFY(pthread_mutex_init(&m_mutex, &mutexAttr) == 0);
+ VHACD_VERIFY(pthread_mutexattr_destroy(&mutexAttr) == 0);
+#endif
+ }
+ ~Mutex(void)
+ {
+#if defined(WIN32) || defined(_XBOX)
+ DeleteCriticalSection(&m_mutex);
+#elif defined(__APPLE__) || defined(__linux__)
+ VHACD_VERIFY(pthread_mutex_destroy(&m_mutex) == 0);
+#endif
+ }
+ void Lock(void)
+ {
+#if defined(WIN32) || defined(_XBOX)
+ EnterCriticalSection(&m_mutex);
+#elif defined(__APPLE__) || defined(__linux__)
+ VHACD_VERIFY(pthread_mutex_lock(&m_mutex) == 0);
+#endif
+ }
+ bool TryLock(void)
+ {
+#if defined(WIN32) || defined(_XBOX)
+ bool bRet = false;
+ //assert(("TryEnterCriticalSection seems to not work on XP???", 0));
+ bRet = TryEnterCriticalSection(&m_mutex) ? true : false;
+ return bRet;
+#elif defined(__APPLE__) || defined(__linux__)
+ int result = pthread_mutex_trylock(&m_mutex);
+ return (result == 0);
+#endif
+ }
+
+ void Unlock(void)
+ {
+#if defined(WIN32) || defined(_XBOX)
+ LeaveCriticalSection(&m_mutex);
+#elif defined(__APPLE__) || defined(__linux__)
+ VHACD_VERIFY(pthread_mutex_unlock(&m_mutex) == 0);
+#endif
+ }
+
+private:
+#if defined(WIN32) || defined(_XBOX)
+ CRITICAL_SECTION m_mutex;
+#elif defined(__APPLE__) || defined(__linux__)
+ pthread_mutex_t m_mutex;
+#endif
+};
+}
+#endif // VHACD_MUTEX_H
diff --git a/Extras/VHACD/inc/vhacdSArray.h b/Extras/VHACD/inc/vhacdSArray.h
new file mode 100644
index 0000000..4d84d1a
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdSArray.h
@@ -0,0 +1,158 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#pragma once
+#ifndef VHACD_SARRAY_H
+#define VHACD_SARRAY_H
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define SARRAY_DEFAULT_MIN_SIZE 16
+
+namespace VHACD {
+//! SArray.
+template <typename T, size_t N = 64>
+class SArray {
+public:
+ T& operator[](size_t i)
+ {
+ T* const data = Data();
+ return data[i];
+ }
+ const T& operator[](size_t i) const
+ {
+ const T* const data = Data();
+ return data[i];
+ }
+ size_t Size() const
+ {
+ return m_size;
+ }
+ T* const Data()
+ {
+ return (m_maxSize == N) ? m_data0 : m_data;
+ }
+ const T* const Data() const
+ {
+ return (m_maxSize == N) ? m_data0 : m_data;
+ }
+ void Clear()
+ {
+ m_size = 0;
+ delete[] m_data;
+ m_data = 0;
+ m_maxSize = N;
+ }
+ void PopBack()
+ {
+ --m_size;
+ }
+ void Allocate(size_t size)
+ {
+ if (size > m_maxSize) {
+ T* temp = new T[size];
+ memcpy(temp, Data(), m_size * sizeof(T));
+ delete[] m_data;
+ m_data = temp;
+ m_maxSize = size;
+ }
+ }
+ void Resize(size_t size)
+ {
+ Allocate(size);
+ m_size = size;
+ }
+
+ void PushBack(const T& value)
+ {
+ if (m_size == m_maxSize) {
+ size_t maxSize = (m_maxSize << 1);
+ T* temp = new T[maxSize];
+ memcpy(temp, Data(), m_maxSize * sizeof(T));
+ delete[] m_data;
+ m_data = temp;
+ m_maxSize = maxSize;
+ }
+ T* const data = Data();
+ data[m_size++] = value;
+ }
+ bool Find(const T& value, size_t& pos)
+ {
+ T* const data = Data();
+ for (pos = 0; pos < m_size; ++pos)
+ if (value == data[pos])
+ return true;
+ return false;
+ }
+ bool Insert(const T& value)
+ {
+ size_t pos;
+ if (Find(value, pos))
+ return false;
+ PushBack(value);
+ return true;
+ }
+ bool Erase(const T& value)
+ {
+ size_t pos;
+ T* const data = Data();
+ if (Find(value, pos)) {
+ for (size_t j = pos + 1; j < m_size; ++j)
+ data[j - 1] = data[j];
+ --m_size;
+ return true;
+ }
+ return false;
+ }
+ void operator=(const SArray& rhs)
+ {
+ if (m_maxSize < rhs.m_size) {
+ delete[] m_data;
+ m_maxSize = rhs.m_maxSize;
+ m_data = new T[m_maxSize];
+ }
+ m_size = rhs.m_size;
+ memcpy(Data(), rhs.Data(), m_size * sizeof(T));
+ }
+ void Initialize()
+ {
+ m_data = 0;
+ m_size = 0;
+ m_maxSize = N;
+ }
+ SArray(const SArray& rhs)
+ {
+ m_data = 0;
+ m_size = 0;
+ m_maxSize = N;
+ *this = rhs;
+ }
+ SArray()
+ {
+ Initialize();
+ }
+ ~SArray()
+ {
+ delete[] m_data;
+ }
+
+private:
+ T m_data0[N];
+ T* m_data;
+ size_t m_size;
+ size_t m_maxSize;
+};
+}
+#endif
\ No newline at end of file
diff --git a/Extras/VHACD/inc/vhacdTimer.h b/Extras/VHACD/inc/vhacdTimer.h
new file mode 100644
index 0000000..ba0e2c3
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdTimer.h
@@ -0,0 +1,121 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#pragma once
+#ifndef VHACD_TIMER_H
+#define VHACD_TIMER_H
+
+#ifdef _WIN32
+#ifndef WIN32_LEAN_AND_MEAN
+#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers
+#endif
+#include <windows.h>
+#elif __MACH__
+#include <mach/clock.h>
+#include <mach/mach.h>
+#else
+#include <sys/time.h>
+#include <time.h>
+#endif
+
+namespace VHACD {
+#ifdef _WIN32
+class Timer {
+public:
+ Timer(void)
+ {
+ m_start.QuadPart = 0;
+ m_stop.QuadPart = 0;
+ QueryPerformanceFrequency(&m_freq);
+ };
+ ~Timer(void){};
+ void Tic()
+ {
+ QueryPerformanceCounter(&m_start);
+ }
+ void Toc()
+ {
+ QueryPerformanceCounter(&m_stop);
+ }
+ double GetElapsedTime() // in ms
+ {
+ LARGE_INTEGER delta;
+ delta.QuadPart = m_stop.QuadPart - m_start.QuadPart;
+ return (1000.0 * delta.QuadPart) / (double)m_freq.QuadPart;
+ }
+
+private:
+ LARGE_INTEGER m_start;
+ LARGE_INTEGER m_stop;
+ LARGE_INTEGER m_freq;
+};
+
+#elif __MACH__
+class Timer {
+public:
+ Timer(void)
+ {
+ memset(this, 0, sizeof(Timer));
+ host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &m_cclock);
+ };
+ ~Timer(void)
+ {
+ mach_port_deallocate(mach_task_self(), m_cclock);
+ };
+ void Tic()
+ {
+ clock_get_time(m_cclock, &m_start);
+ }
+ void Toc()
+ {
+ clock_get_time(m_cclock, &m_stop);
+ }
+ double GetElapsedTime() // in ms
+ {
+ return 1000.0 * (m_stop.tv_sec - m_start.tv_sec + (1.0E-9) * (m_stop.tv_nsec - m_start.tv_nsec));
+ }
+
+private:
+ clock_serv_t m_cclock;
+ mach_timespec_t m_start;
+ mach_timespec_t m_stop;
+};
+#else
+class Timer {
+public:
+ Timer(void)
+ {
+ memset(this, 0, sizeof(Timer));
+ };
+ ~Timer(void){};
+ void Tic()
+ {
+ clock_gettime(CLOCK_REALTIME, &m_start);
+ }
+ void Toc()
+ {
+ clock_gettime(CLOCK_REALTIME, &m_stop);
+ }
+ double GetElapsedTime() // in ms
+ {
+ return 1000.0 * (m_stop.tv_sec - m_start.tv_sec + (1.0E-9) * (m_stop.tv_nsec - m_start.tv_nsec));
+ }
+
+private:
+ struct timespec m_start;
+ struct timespec m_stop;
+};
+#endif
+}
+#endif // VHACD_TIMER_H
diff --git a/Extras/VHACD/inc/vhacdVHACD.h b/Extras/VHACD/inc/vhacdVHACD.h
new file mode 100644
index 0000000..8c6c7fe
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdVHACD.h
@@ -0,0 +1,350 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+All rights reserved.
+
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+2. 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.
+
+3. The names of the contributors may not 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 HOLDER 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 PROF [...]
+*/
+#pragma once
+#ifndef VHACD_VHACD_H
+#define VHACD_VHACD_H
+
+#ifdef OPENCL_FOUND
+#ifdef __MACH__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+#endif //OPENCL_FOUND
+
+#include "vhacdMutex.h"
+#include "vhacdVolume.h"
+
+#define USE_THREAD 1
+#define OCL_MIN_NUM_PRIMITIVES 4096
+#define CH_APP_MIN_NUM_PRIMITIVES 64000
+namespace VHACD {
+class VHACD : public IVHACD {
+public:
+ //! Constructor.
+ VHACD()
+ {
+#if USE_THREAD == 1 && _OPENMP
+ m_ompNumProcessors = 2 * omp_get_num_procs();
+ omp_set_num_threads(m_ompNumProcessors);
+#else //USE_THREAD == 1 && _OPENMP
+ m_ompNumProcessors = 1;
+#endif //USE_THREAD == 1 && _OPENMP
+#ifdef CL_VERSION_1_1
+ m_oclWorkGroupSize = 0;
+ m_oclDevice = 0;
+ m_oclQueue = 0;
+ m_oclKernelComputePartialVolumes = 0;
+ m_oclKernelComputeSum = 0;
+#endif //CL_VERSION_1_1
+ Init();
+ }
+ //! Destructor.
+ ~VHACD(void) {}
+ unsigned int GetNConvexHulls() const
+ {
+ return (unsigned int)m_convexHulls.Size();
+ }
+ void Cancel()
+ {
+ SetCancel(true);
+ }
+ void GetConvexHull(const unsigned int index, ConvexHull& ch) const
+ {
+ Mesh* mesh = m_convexHulls[index];
+ ch.m_nPoints = (unsigned int)mesh->GetNPoints();
+ ch.m_nTriangles = (unsigned int)mesh->GetNTriangles();
+ ch.m_points = mesh->GetPoints();
+ ch.m_triangles = mesh->GetTriangles();
+ }
+ void Clean(void)
+ {
+ delete m_volume;
+ delete m_pset;
+ size_t nCH = m_convexHulls.Size();
+ for (size_t p = 0; p < nCH; ++p) {
+ delete m_convexHulls[p];
+ }
+ m_convexHulls.Clear();
+ Init();
+ }
+ void Release(void)
+ {
+ delete this;
+ }
+ bool Compute(const float* const points,
+ const unsigned int stridePoints,
+ const unsigned int nPoints,
+ const int* const triangles,
+ const unsigned int strideTriangles,
+ const unsigned int nTriangles,
+ const Parameters& params);
+ bool Compute(const double* const points,
+ const unsigned int stridePoints,
+ const unsigned int nPoints,
+ const int* const triangles,
+ const unsigned int strideTriangles,
+ const unsigned int nTriangles,
+ const Parameters& params);
+ bool OCLInit(void* const oclDevice,
+ IUserLogger* const logger = 0);
+ bool OCLRelease(IUserLogger* const logger = 0);
+
+private:
+ void SetCancel(bool cancel)
+ {
+ m_cancelMutex.Lock();
+ m_cancel = cancel;
+ m_cancelMutex.Unlock();
+ }
+ bool GetCancel()
+ {
+
+ m_cancelMutex.Lock();
+ bool cancel = m_cancel;
+ m_cancelMutex.Unlock();
+ return cancel;
+ }
+ void Update(const double stageProgress,
+ const double operationProgress,
+ const Parameters& params)
+ {
+ m_stageProgress = stageProgress;
+ m_operationProgress = operationProgress;
+ if (params.m_callback) {
+ params.m_callback->Update(m_overallProgress,
+ m_stageProgress,
+ m_operationProgress,
+ m_stage.c_str(),
+ m_operation.c_str());
+ }
+ }
+ void Init()
+ {
+ memset(m_rot, 0, sizeof(double) * 9);
+ m_dim = 64;
+ m_volume = 0;
+ m_volumeCH0 = 0.0;
+ m_pset = 0;
+ m_overallProgress = 0.0;
+ m_stageProgress = 0.0;
+ m_operationProgress = 0.0;
+ m_stage = "";
+ m_operation = "";
+ m_barycenter[0] = m_barycenter[1] = m_barycenter[2] = 0.0;
+ m_rot[0][0] = m_rot[1][1] = m_rot[2][2] = 1.0;
+ SetCancel(false);
+ }
+ void ComputePrimitiveSet(const Parameters& params);
+ void ComputeACD(const Parameters& params);
+ void MergeConvexHulls(const Parameters& params);
+ void SimplifyConvexHulls(const Parameters& params);
+ void ComputeBestClippingPlane(const PrimitiveSet* inputPSet,
+ const double volume,
+ const SArray<Plane>& planes,
+ const Vec3<double>& preferredCuttingDirection,
+ const double w,
+ const double alpha,
+ const double beta,
+ const int convexhullDownsampling,
+ const double progress0,
+ const double progress1,
+ Plane& bestPlane,
+ double& minConcavity,
+ const Parameters& params);
+ template <class T>
+ void AlignMesh(const T* const points,
+ const unsigned int stridePoints,
+ const unsigned int nPoints,
+ const int* const triangles,
+ const unsigned int strideTriangles,
+ const unsigned int nTriangles,
+ const Parameters& params)
+ {
+ if (GetCancel() || !params.m_pca) {
+ return;
+ }
+ m_timer.Tic();
+
+ m_stage = "Align mesh";
+ m_operation = "Voxelization";
+
+ std::ostringstream msg;
+ if (params.m_logger) {
+ msg << "+ " << m_stage << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ Update(0.0, 0.0, params);
+ if (GetCancel()) {
+ return;
+ }
+ m_dim = (size_t)(pow((double)params.m_resolution, 1.0 / 3.0) + 0.5);
+ Volume volume;
+ volume.Voxelize(points, stridePoints, nPoints,
+ triangles, strideTriangles, nTriangles,
+ m_dim, m_barycenter, m_rot);
+ size_t n = volume.GetNPrimitivesOnSurf() + volume.GetNPrimitivesInsideSurf();
+ Update(50.0, 100.0, params);
+
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t dim = " << m_dim << "\t-> " << n << " voxels" << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+ if (GetCancel()) {
+ return;
+ }
+ m_operation = "PCA";
+ Update(50.0, 0.0, params);
+ volume.AlignToPrincipalAxes(m_rot);
+ m_overallProgress = 1.0;
+ Update(100.0, 100.0, params);
+
+ m_timer.Toc();
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t time " << m_timer.GetElapsedTime() / 1000.0 << "s" << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+ }
+ template <class T>
+ void VoxelizeMesh(const T* const points,
+ const unsigned int stridePoints,
+ const unsigned int nPoints,
+ const int* const triangles,
+ const unsigned int strideTriangles,
+ const unsigned int nTriangles,
+ const Parameters& params)
+ {
+ if (GetCancel()) {
+ return;
+ }
+
+ m_timer.Tic();
+ m_stage = "Voxelization";
+
+ std::ostringstream msg;
+ if (params.m_logger) {
+ msg << "+ " << m_stage << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ delete m_volume;
+ m_volume = 0;
+ int iteration = 0;
+ const int maxIteration = 5;
+ double progress = 0.0;
+ while (iteration++ < maxIteration && !m_cancel) {
+ msg.str("");
+ msg << "Iteration " << iteration;
+ m_operation = msg.str();
+
+ progress = iteration * 100.0 / maxIteration;
+ Update(progress, 0.0, params);
+
+ m_volume = new Volume;
+ m_volume->Voxelize(points, stridePoints, nPoints,
+ triangles, strideTriangles, nTriangles,
+ m_dim, m_barycenter, m_rot);
+
+ Update(progress, 100.0, params);
+
+ size_t n = m_volume->GetNPrimitivesOnSurf() + m_volume->GetNPrimitivesInsideSurf();
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t dim = " << m_dim << "\t-> " << n << " voxels" << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ double a = pow((double)(params.m_resolution) / n, 0.33);
+ size_t dim_next = (size_t)(m_dim * a + 0.5);
+ if (n < params.m_resolution && iteration < maxIteration && m_volume->GetNPrimitivesOnSurf() < params.m_resolution / 8 && m_dim != dim_next) {
+ delete m_volume;
+ m_volume = 0;
+ m_dim = dim_next;
+ }
+ else {
+ break;
+ }
+ }
+ m_overallProgress = 10.0;
+ Update(100.0, 100.0, params);
+
+ m_timer.Toc();
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t time " << m_timer.GetElapsedTime() / 1000.0 << "s" << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+ }
+ template <class T>
+ bool ComputeACD(const T* const points,
+ const unsigned int stridePoints,
+ const unsigned int nPoints,
+ const int* const triangles,
+ const unsigned int strideTriangles,
+ const unsigned int nTriangles,
+ const Parameters& params)
+ {
+ Init();
+ if (params.m_oclAcceleration) {
+ // build kernals
+ }
+ AlignMesh(points, stridePoints, nPoints, triangles, strideTriangles, nTriangles, params);
+ VoxelizeMesh(points, stridePoints, nPoints, triangles, strideTriangles, nTriangles, params);
+ ComputePrimitiveSet(params);
+ ComputeACD(params);
+ MergeConvexHulls(params);
+ SimplifyConvexHulls(params);
+ if (params.m_oclAcceleration) {
+ // Release kernals
+ }
+ if (GetCancel()) {
+ Clean();
+ return false;
+ }
+ return true;
+ }
+
+private:
+ SArray<Mesh*> m_convexHulls;
+ std::string m_stage;
+ std::string m_operation;
+ double m_overallProgress;
+ double m_stageProgress;
+ double m_operationProgress;
+ double m_rot[3][3];
+ double m_volumeCH0;
+ Vec3<double> m_barycenter;
+ Timer m_timer;
+ size_t m_dim;
+ Volume* m_volume;
+ PrimitiveSet* m_pset;
+ Mutex m_cancelMutex;
+ bool m_cancel;
+ int m_ompNumProcessors;
+#ifdef CL_VERSION_1_1
+ cl_device_id* m_oclDevice;
+ cl_context m_oclContext;
+ cl_program m_oclProgram;
+ cl_command_queue* m_oclQueue;
+ cl_kernel* m_oclKernelComputePartialVolumes;
+ cl_kernel* m_oclKernelComputeSum;
+ size_t m_oclWorkGroupSize;
+#endif //CL_VERSION_1_1
+};
+}
+#endif // VHACD_VHACD_H
diff --git a/Extras/VHACD/inc/vhacdVector.h b/Extras/VHACD/inc/vhacdVector.h
new file mode 100644
index 0000000..c9edfb1
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdVector.h
@@ -0,0 +1,103 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#pragma once
+#ifndef VHACD_VECTOR_H
+#define VHACD_VECTOR_H
+#include <iostream>
+#include <math.h>
+
+namespace VHACD {
+//! Vector dim 3.
+template <typename T>
+class Vec3 {
+public:
+ T& operator[](size_t i) { return m_data[i]; }
+ const T& operator[](size_t i) const { return m_data[i]; }
+ T& X();
+ T& Y();
+ T& Z();
+ const T& X() const;
+ const T& Y() const;
+ const T& Z() const;
+ void Normalize();
+ T GetNorm() const;
+ void operator=(const Vec3& rhs);
+ void operator+=(const Vec3& rhs);
+ void operator-=(const Vec3& rhs);
+ void operator-=(T a);
+ void operator+=(T a);
+ void operator/=(T a);
+ void operator*=(T a);
+ Vec3 operator^(const Vec3& rhs) const;
+ T operator*(const Vec3& rhs) const;
+ Vec3 operator+(const Vec3& rhs) const;
+ Vec3 operator-(const Vec3& rhs) const;
+ Vec3 operator-() const;
+ Vec3 operator*(T rhs) const;
+ Vec3 operator/(T rhs) const;
+ bool operator<(const Vec3& rhs) const;
+ bool operator>(const Vec3& rhs) const;
+ Vec3();
+ Vec3(T a);
+ Vec3(T x, T y, T z);
+ Vec3(const Vec3& rhs);
+ /*virtual*/ ~Vec3(void);
+
+private:
+ T m_data[3];
+};
+//! Vector dim 2.
+template <typename T>
+class Vec2 {
+public:
+ T& operator[](size_t i) { return m_data[i]; }
+ const T& operator[](size_t i) const { return m_data[i]; }
+ T& X();
+ T& Y();
+ const T& X() const;
+ const T& Y() const;
+ void Normalize();
+ T GetNorm() const;
+ void operator=(const Vec2& rhs);
+ void operator+=(const Vec2& rhs);
+ void operator-=(const Vec2& rhs);
+ void operator-=(T a);
+ void operator+=(T a);
+ void operator/=(T a);
+ void operator*=(T a);
+ T operator^(const Vec2& rhs) const;
+ T operator*(const Vec2& rhs) const;
+ Vec2 operator+(const Vec2& rhs) const;
+ Vec2 operator-(const Vec2& rhs) const;
+ Vec2 operator-() const;
+ Vec2 operator*(T rhs) const;
+ Vec2 operator/(T rhs) const;
+ Vec2();
+ Vec2(T a);
+ Vec2(T x, T y);
+ Vec2(const Vec2& rhs);
+ /*virtual*/ ~Vec2(void);
+
+private:
+ T m_data[2];
+};
+
+template <typename T>
+const bool Colinear(const Vec3<T>& a, const Vec3<T>& b, const Vec3<T>& c);
+template <typename T>
+const T ComputeVolume4(const Vec3<T>& a, const Vec3<T>& b, const Vec3<T>& c, const Vec3<T>& d);
+}
+#include "vhacdVector.inl" // template implementation
+#endif
\ No newline at end of file
diff --git a/Extras/VHACD/inc/vhacdVector.inl b/Extras/VHACD/inc/vhacdVector.inl
new file mode 100644
index 0000000..223c2ef
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdVector.inl
@@ -0,0 +1,362 @@
+#pragma once
+#ifndef VHACD_VECTOR_INL
+#define VHACD_VECTOR_INL
+namespace VHACD
+{
+ template <typename T>
+ inline Vec3<T> operator*(T lhs, const Vec3<T> & rhs)
+ {
+ return Vec3<T>(lhs * rhs.X(), lhs * rhs.Y(), lhs * rhs.Z());
+ }
+ template <typename T>
+ inline T & Vec3<T>::X()
+ {
+ return m_data[0];
+ }
+ template <typename T>
+ inline T & Vec3<T>::Y()
+ {
+ return m_data[1];
+ }
+ template <typename T>
+ inline T & Vec3<T>::Z()
+ {
+ return m_data[2];
+ }
+ template <typename T>
+ inline const T & Vec3<T>::X() const
+ {
+ return m_data[0];
+ }
+ template <typename T>
+ inline const T & Vec3<T>::Y() const
+ {
+ return m_data[1];
+ }
+ template <typename T>
+ inline const T & Vec3<T>::Z() const
+ {
+ return m_data[2];
+ }
+ template <typename T>
+ inline void Vec3<T>::Normalize()
+ {
+ T n = sqrt(m_data[0]*m_data[0]+m_data[1]*m_data[1]+m_data[2]*m_data[2]);
+ if (n != 0.0) (*this) /= n;
+ }
+ template <typename T>
+ inline T Vec3<T>::GetNorm() const
+ {
+ return sqrt(m_data[0]*m_data[0]+m_data[1]*m_data[1]+m_data[2]*m_data[2]);
+ }
+ template <typename T>
+ inline void Vec3<T>::operator= (const Vec3 & rhs)
+ {
+ this->m_data[0] = rhs.m_data[0];
+ this->m_data[1] = rhs.m_data[1];
+ this->m_data[2] = rhs.m_data[2];
+ }
+ template <typename T>
+ inline void Vec3<T>::operator+=(const Vec3 & rhs)
+ {
+ this->m_data[0] += rhs.m_data[0];
+ this->m_data[1] += rhs.m_data[1];
+ this->m_data[2] += rhs.m_data[2];
+ }
+ template <typename T>
+ inline void Vec3<T>::operator-=(const Vec3 & rhs)
+ {
+ this->m_data[0] -= rhs.m_data[0];
+ this->m_data[1] -= rhs.m_data[1];
+ this->m_data[2] -= rhs.m_data[2];
+ }
+ template <typename T>
+ inline void Vec3<T>::operator-=(T a)
+ {
+ this->m_data[0] -= a;
+ this->m_data[1] -= a;
+ this->m_data[2] -= a;
+ }
+ template <typename T>
+ inline void Vec3<T>::operator+=(T a)
+ {
+ this->m_data[0] += a;
+ this->m_data[1] += a;
+ this->m_data[2] += a;
+ }
+ template <typename T>
+ inline void Vec3<T>::operator/=(T a)
+ {
+ this->m_data[0] /= a;
+ this->m_data[1] /= a;
+ this->m_data[2] /= a;
+ }
+ template <typename T>
+ inline void Vec3<T>::operator*=(T a)
+ {
+ this->m_data[0] *= a;
+ this->m_data[1] *= a;
+ this->m_data[2] *= a;
+ }
+ template <typename T>
+ inline Vec3<T> Vec3<T>::operator^ (const Vec3<T> & rhs) const
+ {
+ return Vec3<T>(m_data[1] * rhs.m_data[2] - m_data[2] * rhs.m_data[1],
+ m_data[2] * rhs.m_data[0] - m_data[0] * rhs.m_data[2],
+ m_data[0] * rhs.m_data[1] - m_data[1] * rhs.m_data[0]);
+ }
+ template <typename T>
+ inline T Vec3<T>::operator*(const Vec3<T> & rhs) const
+ {
+ return (m_data[0] * rhs.m_data[0] + m_data[1] * rhs.m_data[1] + m_data[2] * rhs.m_data[2]);
+ }
+ template <typename T>
+ inline Vec3<T> Vec3<T>::operator+(const Vec3<T> & rhs) const
+ {
+ return Vec3<T>(m_data[0] + rhs.m_data[0],m_data[1] + rhs.m_data[1],m_data[2] + rhs.m_data[2]);
+ }
+ template <typename T>
+ inline Vec3<T> Vec3<T>::operator-(const Vec3<T> & rhs) const
+ {
+ return Vec3<T>(m_data[0] - rhs.m_data[0],m_data[1] - rhs.m_data[1],m_data[2] - rhs.m_data[2]) ;
+ }
+ template <typename T>
+ inline Vec3<T> Vec3<T>::operator-() const
+ {
+ return Vec3<T>(-m_data[0],-m_data[1],-m_data[2]) ;
+ }
+
+ template <typename T>
+ inline Vec3<T> Vec3<T>::operator*(T rhs) const
+ {
+ return Vec3<T>(rhs * this->m_data[0], rhs * this->m_data[1], rhs * this->m_data[2]);
+ }
+ template <typename T>
+ inline Vec3<T> Vec3<T>::operator/ (T rhs) const
+ {
+ return Vec3<T>(m_data[0] / rhs, m_data[1] / rhs, m_data[2] / rhs);
+ }
+ template <typename T>
+ inline Vec3<T>::Vec3(T a)
+ {
+ m_data[0] = m_data[1] = m_data[2] = a;
+ }
+ template <typename T>
+ inline Vec3<T>::Vec3(T x, T y, T z)
+ {
+ m_data[0] = x;
+ m_data[1] = y;
+ m_data[2] = z;
+ }
+ template <typename T>
+ inline Vec3<T>::Vec3(const Vec3 & rhs)
+ {
+ m_data[0] = rhs.m_data[0];
+ m_data[1] = rhs.m_data[1];
+ m_data[2] = rhs.m_data[2];
+ }
+ template <typename T>
+ inline Vec3<T>::~Vec3(void){};
+
+ template <typename T>
+ inline Vec3<T>::Vec3() {}
+
+ template<typename T>
+ inline const bool Colinear(const Vec3<T> & a, const Vec3<T> & b, const Vec3<T> & c)
+ {
+ return ((c.Z() - a.Z()) * (b.Y() - a.Y()) - (b.Z() - a.Z()) * (c.Y() - a.Y()) == 0.0 /*EPS*/) &&
+ ((b.Z() - a.Z()) * (c.X() - a.X()) - (b.X() - a.X()) * (c.Z() - a.Z()) == 0.0 /*EPS*/) &&
+ ((b.X() - a.X()) * (c.Y() - a.Y()) - (b.Y() - a.Y()) * (c.X() - a.X()) == 0.0 /*EPS*/);
+ }
+
+ template<typename T>
+ inline const T ComputeVolume4(const Vec3<T> & a, const Vec3<T> & b, const Vec3<T> & c, const Vec3<T> & d)
+ {
+ return (a-d) * ((b-d) ^ (c-d));
+ }
+
+ template <typename T>
+ inline bool Vec3<T>::operator<(const Vec3 & rhs) const
+ {
+ if (X() == rhs[0])
+ {
+ if (Y() == rhs[1])
+ {
+ return (Z()<rhs[2]);
+ }
+ return (Y()<rhs[1]);
+ }
+ return (X()<rhs[0]);
+ }
+ template <typename T>
+ inline bool Vec3<T>::operator>(const Vec3 & rhs) const
+ {
+ if (X() == rhs[0])
+ {
+ if (Y() == rhs[1])
+ {
+ return (Z()>rhs[2]);
+ }
+ return (Y()>rhs[1]);
+ }
+ return (X()>rhs[0]);
+ }
+ template <typename T>
+ inline Vec2<T> operator*(T lhs, const Vec2<T> & rhs)
+ {
+ return Vec2<T>(lhs * rhs.X(), lhs * rhs.Y());
+ }
+ template <typename T>
+ inline T & Vec2<T>::X()
+ {
+ return m_data[0];
+ }
+ template <typename T>
+ inline T & Vec2<T>::Y()
+ {
+ return m_data[1];
+ }
+ template <typename T>
+ inline const T & Vec2<T>::X() const
+ {
+ return m_data[0];
+ }
+ template <typename T>
+ inline const T & Vec2<T>::Y() const
+ {
+ return m_data[1];
+ }
+ template <typename T>
+ inline void Vec2<T>::Normalize()
+ {
+ T n = sqrt(m_data[0]*m_data[0]+m_data[1]*m_data[1]);
+ if (n != 0.0) (*this) /= n;
+ }
+ template <typename T>
+ inline T Vec2<T>::GetNorm() const
+ {
+ return sqrt(m_data[0]*m_data[0]+m_data[1]*m_data[1]);
+ }
+ template <typename T>
+ inline void Vec2<T>::operator= (const Vec2 & rhs)
+ {
+ this->m_data[0] = rhs.m_data[0];
+ this->m_data[1] = rhs.m_data[1];
+ }
+ template <typename T>
+ inline void Vec2<T>::operator+=(const Vec2 & rhs)
+ {
+ this->m_data[0] += rhs.m_data[0];
+ this->m_data[1] += rhs.m_data[1];
+ }
+ template <typename T>
+ inline void Vec2<T>::operator-=(const Vec2 & rhs)
+ {
+ this->m_data[0] -= rhs.m_data[0];
+ this->m_data[1] -= rhs.m_data[1];
+ }
+ template <typename T>
+ inline void Vec2<T>::operator-=(T a)
+ {
+ this->m_data[0] -= a;
+ this->m_data[1] -= a;
+ }
+ template <typename T>
+ inline void Vec2<T>::operator+=(T a)
+ {
+ this->m_data[0] += a;
+ this->m_data[1] += a;
+ }
+ template <typename T>
+ inline void Vec2<T>::operator/=(T a)
+ {
+ this->m_data[0] /= a;
+ this->m_data[1] /= a;
+ }
+ template <typename T>
+ inline void Vec2<T>::operator*=(T a)
+ {
+ this->m_data[0] *= a;
+ this->m_data[1] *= a;
+ }
+ template <typename T>
+ inline T Vec2<T>::operator^ (const Vec2<T> & rhs) const
+ {
+ return m_data[0] * rhs.m_data[1] - m_data[1] * rhs.m_data[0];
+ }
+ template <typename T>
+ inline T Vec2<T>::operator*(const Vec2<T> & rhs) const
+ {
+ return (m_data[0] * rhs.m_data[0] + m_data[1] * rhs.m_data[1]);
+ }
+ template <typename T>
+ inline Vec2<T> Vec2<T>::operator+(const Vec2<T> & rhs) const
+ {
+ return Vec2<T>(m_data[0] + rhs.m_data[0],m_data[1] + rhs.m_data[1]);
+ }
+ template <typename T>
+ inline Vec2<T> Vec2<T>::operator-(const Vec2<T> & rhs) const
+ {
+ return Vec2<T>(m_data[0] - rhs.m_data[0],m_data[1] - rhs.m_data[1]);
+ }
+ template <typename T>
+ inline Vec2<T> Vec2<T>::operator-() const
+ {
+ return Vec2<T>(-m_data[0],-m_data[1]) ;
+ }
+
+ template <typename T>
+ inline Vec2<T> Vec2<T>::operator*(T rhs) const
+ {
+ return Vec2<T>(rhs * this->m_data[0], rhs * this->m_data[1]);
+ }
+ template <typename T>
+ inline Vec2<T> Vec2<T>::operator/ (T rhs) const
+ {
+ return Vec2<T>(m_data[0] / rhs, m_data[1] / rhs);
+ }
+ template <typename T>
+ inline Vec2<T>::Vec2(T a)
+ {
+ m_data[0] = m_data[1] = a;
+ }
+ template <typename T>
+ inline Vec2<T>::Vec2(T x, T y)
+ {
+ m_data[0] = x;
+ m_data[1] = y;
+ }
+ template <typename T>
+ inline Vec2<T>::Vec2(const Vec2 & rhs)
+ {
+ m_data[0] = rhs.m_data[0];
+ m_data[1] = rhs.m_data[1];
+ }
+ template <typename T>
+ inline Vec2<T>::~Vec2(void){};
+
+ template <typename T>
+ inline Vec2<T>::Vec2() {}
+
+ /*
+ InsideTriangle decides if a point P is Inside of the triangle
+ defined by A, B, C.
+ */
+ template<typename T>
+ inline const bool InsideTriangle(const Vec2<T> & a, const Vec2<T> & b, const Vec2<T> & c, const Vec2<T> & p)
+ {
+ T ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
+ T cCROSSap, bCROSScp, aCROSSbp;
+ ax = c.X() - b.X(); ay = c.Y() - b.Y();
+ bx = a.X() - c.X(); by = a.Y() - c.Y();
+ cx = b.X() - a.X(); cy = b.Y() - a.Y();
+ apx= p.X() - a.X(); apy= p.Y() - a.Y();
+ bpx= p.X() - b.X(); bpy= p.Y() - b.Y();
+ cpx= p.X() - c.X(); cpy= p.Y() - c.Y();
+ aCROSSbp = ax*bpy - ay*bpx;
+ cCROSSap = cx*apy - cy*apx;
+ bCROSScp = bx*cpy - by*cpx;
+ return ((aCROSSbp >= 0.0) && (bCROSScp >= 0.0) && (cCROSSap >= 0.0));
+ }
+}
+#endif //VHACD_VECTOR_INL
\ No newline at end of file
diff --git a/Extras/VHACD/inc/vhacdVolume.h b/Extras/VHACD/inc/vhacdVolume.h
new file mode 100644
index 0000000..31377aa
--- /dev/null
+++ b/Extras/VHACD/inc/vhacdVolume.h
@@ -0,0 +1,419 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#pragma once
+#ifndef VHACD_VOLUME_H
+#define VHACD_VOLUME_H
+#include "vhacdMesh.h"
+#include "vhacdVector.h"
+#include <assert.h>
+
+namespace VHACD {
+
+enum VOXEL_VALUE {
+ PRIMITIVE_UNDEFINED = 0,
+ PRIMITIVE_OUTSIDE_SURFACE = 1,
+ PRIMITIVE_INSIDE_SURFACE = 2,
+ PRIMITIVE_ON_SURFACE = 3
+};
+
+struct Voxel {
+public:
+ short m_coord[3];
+ short m_data;
+};
+
+class PrimitiveSet {
+public:
+ virtual ~PrimitiveSet(){};
+ virtual PrimitiveSet* Create() const = 0;
+ virtual const size_t GetNPrimitives() const = 0;
+ virtual const size_t GetNPrimitivesOnSurf() const = 0;
+ virtual const size_t GetNPrimitivesInsideSurf() const = 0;
+ virtual const double GetEigenValue(AXIS axis) const = 0;
+ virtual const double ComputeMaxVolumeError() const = 0;
+ virtual const double ComputeVolume() const = 0;
+ virtual void Clip(const Plane& plane, PrimitiveSet* const positivePart,
+ PrimitiveSet* const negativePart) const = 0;
+ virtual void Intersect(const Plane& plane, SArray<Vec3<double> >* const positivePts,
+ SArray<Vec3<double> >* const negativePts, const size_t sampling) const = 0;
+ virtual void ComputeExteriorPoints(const Plane& plane, const Mesh& mesh,
+ SArray<Vec3<double> >* const exteriorPts) const = 0;
+ virtual void ComputeClippedVolumes(const Plane& plane, double& positiveVolume,
+ double& negativeVolume) const = 0;
+ virtual void SelectOnSurface(PrimitiveSet* const onSurfP) const = 0;
+ virtual void ComputeConvexHull(Mesh& meshCH, const size_t sampling = 1) const = 0;
+ virtual void ComputeBB() = 0;
+ virtual void ComputePrincipalAxes() = 0;
+ virtual void AlignToPrincipalAxes() = 0;
+ virtual void RevertAlignToPrincipalAxes() = 0;
+ virtual void Convert(Mesh& mesh, const VOXEL_VALUE value) const = 0;
+ const Mesh& GetConvexHull() const { return m_convexHull; };
+ Mesh& GetConvexHull() { return m_convexHull; };
+private:
+ Mesh m_convexHull;
+};
+
+//!
+class VoxelSet : public PrimitiveSet {
+ friend class Volume;
+
+public:
+ //! Destructor.
+ ~VoxelSet(void);
+ //! Constructor.
+ VoxelSet();
+
+ const size_t GetNPrimitives() const { return m_voxels.Size(); }
+ const size_t GetNPrimitivesOnSurf() const { return m_numVoxelsOnSurface; }
+ const size_t GetNPrimitivesInsideSurf() const { return m_numVoxelsInsideSurface; }
+ const double GetEigenValue(AXIS axis) const { return m_D[axis][axis]; }
+ const double ComputeVolume() const { return m_unitVolume * m_voxels.Size(); }
+ const double ComputeMaxVolumeError() const { return m_unitVolume * m_numVoxelsOnSurface; }
+ const Vec3<short>& GetMinBBVoxels() const { return m_minBBVoxels; }
+ const Vec3<short>& GetMaxBBVoxels() const { return m_maxBBVoxels; }
+ const Vec3<double>& GetMinBB() const { return m_minBB; }
+ const double& GetScale() const { return m_scale; }
+ const double& GetUnitVolume() const { return m_unitVolume; }
+ Vec3<double> GetPoint(Vec3<short> voxel) const
+ {
+ return Vec3<double>(voxel[0] * m_scale + m_minBB[0],
+ voxel[1] * m_scale + m_minBB[1],
+ voxel[2] * m_scale + m_minBB[2]);
+ }
+ Vec3<double> GetPoint(const Voxel& voxel) const
+ {
+ return Vec3<double>(voxel.m_coord[0] * m_scale + m_minBB[0],
+ voxel.m_coord[1] * m_scale + m_minBB[1],
+ voxel.m_coord[2] * m_scale + m_minBB[2]);
+ }
+ Vec3<double> GetPoint(Vec3<double> voxel) const
+ {
+ return Vec3<double>(voxel[0] * m_scale + m_minBB[0],
+ voxel[1] * m_scale + m_minBB[1],
+ voxel[2] * m_scale + m_minBB[2]);
+ }
+ void GetPoints(const Voxel& voxel, Vec3<double>* const pts) const;
+ void ComputeConvexHull(Mesh& meshCH, const size_t sampling = 1) const;
+ void Clip(const Plane& plane, PrimitiveSet* const positivePart, PrimitiveSet* const negativePart) const;
+ void Intersect(const Plane& plane, SArray<Vec3<double> >* const positivePts,
+ SArray<Vec3<double> >* const negativePts, const size_t sampling) const;
+ void ComputeExteriorPoints(const Plane& plane, const Mesh& mesh,
+ SArray<Vec3<double> >* const exteriorPts) const;
+ void ComputeClippedVolumes(const Plane& plane, double& positiveVolume, double& negativeVolume) const;
+ void SelectOnSurface(PrimitiveSet* const onSurfP) const;
+ void ComputeBB();
+ void Convert(Mesh& mesh, const VOXEL_VALUE value) const;
+ void ComputePrincipalAxes();
+ PrimitiveSet* Create() const
+ {
+ return new VoxelSet();
+ }
+ void AlignToPrincipalAxes(){};
+ void RevertAlignToPrincipalAxes(){};
+ Voxel* const GetVoxels() { return m_voxels.Data(); }
+ const Voxel* const GetVoxels() const { return m_voxels.Data(); }
+
+private:
+ size_t m_numVoxelsOnSurface;
+ size_t m_numVoxelsInsideSurface;
+ Vec3<double> m_minBB;
+ double m_scale;
+ SArray<Voxel, 8> m_voxels;
+ double m_unitVolume;
+ Vec3<double> m_minBBPts;
+ Vec3<double> m_maxBBPts;
+ Vec3<short> m_minBBVoxels;
+ Vec3<short> m_maxBBVoxels;
+ Vec3<short> m_barycenter;
+ double m_Q[3][3];
+ double m_D[3][3];
+ Vec3<double> m_barycenterPCA;
+};
+
+struct Tetrahedron {
+public:
+ Vec3<double> m_pts[4];
+ unsigned char m_data;
+};
+
+//!
+class TetrahedronSet : public PrimitiveSet {
+ friend class Volume;
+
+public:
+ //! Destructor.
+ ~TetrahedronSet(void);
+ //! Constructor.
+ TetrahedronSet();
+
+ const size_t GetNPrimitives() const { return m_tetrahedra.Size(); }
+ const size_t GetNPrimitivesOnSurf() const { return m_numTetrahedraOnSurface; }
+ const size_t GetNPrimitivesInsideSurf() const { return m_numTetrahedraInsideSurface; }
+ const Vec3<double>& GetMinBB() const { return m_minBB; }
+ const Vec3<double>& GetMaxBB() const { return m_maxBB; }
+ const Vec3<double>& GetBarycenter() const { return m_barycenter; }
+ const double GetEigenValue(AXIS axis) const { return m_D[axis][axis]; }
+ const double GetSacle() const { return m_scale; }
+ const double ComputeVolume() const;
+ const double ComputeMaxVolumeError() const;
+ void ComputeConvexHull(Mesh& meshCH, const size_t sampling = 1) const;
+ void ComputePrincipalAxes();
+ void AlignToPrincipalAxes();
+ void RevertAlignToPrincipalAxes();
+ void Clip(const Plane& plane, PrimitiveSet* const positivePart, PrimitiveSet* const negativePart) const;
+ void Intersect(const Plane& plane, SArray<Vec3<double> >* const positivePts,
+ SArray<Vec3<double> >* const negativePts, const size_t sampling) const;
+ void ComputeExteriorPoints(const Plane& plane, const Mesh& mesh,
+ SArray<Vec3<double> >* const exteriorPts) const;
+ void ComputeClippedVolumes(const Plane& plane, double& positiveVolume, double& negativeVolume) const;
+ void SelectOnSurface(PrimitiveSet* const onSurfP) const;
+ void ComputeBB();
+ void Convert(Mesh& mesh, const VOXEL_VALUE value) const;
+ inline bool Add(Tetrahedron& tetrahedron);
+ PrimitiveSet* Create() const
+ {
+ return new TetrahedronSet();
+ }
+ static const double EPS;
+
+private:
+ void AddClippedTetrahedra(const Vec3<double> (&pts)[10], const int nPts);
+
+ size_t m_numTetrahedraOnSurface;
+ size_t m_numTetrahedraInsideSurface;
+ double m_scale;
+ Vec3<double> m_minBB;
+ Vec3<double> m_maxBB;
+ Vec3<double> m_barycenter;
+ SArray<Tetrahedron, 8> m_tetrahedra;
+ double m_Q[3][3];
+ double m_D[3][3];
+};
+
+//!
+class Volume {
+public:
+ //! Destructor.
+ ~Volume(void);
+
+ //! Constructor.
+ Volume();
+
+ //! Voxelize
+ template <class T>
+ void Voxelize(const T* const points, const unsigned int stridePoints, const unsigned int nPoints,
+ const int* const triangles, const unsigned int strideTriangles, const unsigned int nTriangles,
+ const size_t dim, const Vec3<double>& barycenter, const double (&rot)[3][3]);
+ unsigned char& GetVoxel(const size_t i, const size_t j, const size_t k)
+ {
+ assert(i < m_dim[0] || i >= 0);
+ assert(j < m_dim[0] || j >= 0);
+ assert(k < m_dim[0] || k >= 0);
+ return m_data[i + j * m_dim[0] + k * m_dim[0] * m_dim[1]];
+ }
+ const unsigned char& GetVoxel(const size_t i, const size_t j, const size_t k) const
+ {
+ assert(i < m_dim[0] || i >= 0);
+ assert(j < m_dim[0] || j >= 0);
+ assert(k < m_dim[0] || k >= 0);
+ return m_data[i + j * m_dim[0] + k * m_dim[0] * m_dim[1]];
+ }
+ const size_t GetNPrimitivesOnSurf() const { return m_numVoxelsOnSurface; }
+ const size_t GetNPrimitivesInsideSurf() const { return m_numVoxelsInsideSurface; }
+ void Convert(Mesh& mesh, const VOXEL_VALUE value) const;
+ void Convert(VoxelSet& vset) const;
+ void Convert(TetrahedronSet& tset) const;
+ void AlignToPrincipalAxes(double (&rot)[3][3]) const;
+
+private:
+ void FillOutsideSurface(const size_t i0, const size_t j0, const size_t k0, const size_t i1,
+ const size_t j1, const size_t k1);
+ void FillInsideSurface();
+ template <class T>
+ void ComputeBB(const T* const points, const unsigned int stridePoints, const unsigned int nPoints,
+ const Vec3<double>& barycenter, const double (&rot)[3][3]);
+ void Allocate();
+ void Free();
+
+ Vec3<double> m_minBB;
+ Vec3<double> m_maxBB;
+ double m_scale;
+ size_t m_dim[3]; //>! dim
+ size_t m_numVoxelsOnSurface;
+ size_t m_numVoxelsInsideSurface;
+ size_t m_numVoxelsOutsideSurface;
+ unsigned char* m_data;
+};
+int TriBoxOverlap(const Vec3<double>& boxcenter, const Vec3<double>& boxhalfsize, const Vec3<double>& triver0,
+ const Vec3<double>& triver1, const Vec3<double>& triver2);
+template <class T>
+inline void ComputeAlignedPoint(const T* const points, const unsigned int idx, const Vec3<double>& barycenter,
+ const double (&rot)[3][3], Vec3<double>& pt){};
+template <>
+inline void ComputeAlignedPoint<float>(const float* const points, const unsigned int idx, const Vec3<double>& barycenter, const double (&rot)[3][3], Vec3<double>& pt)
+{
+ double x = points[idx + 0] - barycenter[0];
+ double y = points[idx + 1] - barycenter[1];
+ double z = points[idx + 2] - barycenter[2];
+ pt[0] = rot[0][0] * x + rot[1][0] * y + rot[2][0] * z;
+ pt[1] = rot[0][1] * x + rot[1][1] * y + rot[2][1] * z;
+ pt[2] = rot[0][2] * x + rot[1][2] * y + rot[2][2] * z;
+}
+template <>
+inline void ComputeAlignedPoint<double>(const double* const points, const unsigned int idx, const Vec3<double>& barycenter, const double (&rot)[3][3], Vec3<double>& pt)
+{
+ double x = points[idx + 0] - barycenter[0];
+ double y = points[idx + 1] - barycenter[1];
+ double z = points[idx + 2] - barycenter[2];
+ pt[0] = rot[0][0] * x + rot[1][0] * y + rot[2][0] * z;
+ pt[1] = rot[0][1] * x + rot[1][1] * y + rot[2][1] * z;
+ pt[2] = rot[0][2] * x + rot[1][2] * y + rot[2][2] * z;
+}
+template <class T>
+void Volume::ComputeBB(const T* const points, const unsigned int stridePoints, const unsigned int nPoints,
+ const Vec3<double>& barycenter, const double (&rot)[3][3])
+{
+ Vec3<double> pt;
+ ComputeAlignedPoint(points, 0, barycenter, rot, pt);
+ m_maxBB = pt;
+ m_minBB = pt;
+ for (unsigned int v = 1; v < nPoints; ++v) {
+ ComputeAlignedPoint(points, v * stridePoints, barycenter, rot, pt);
+ for (int i = 0; i < 3; ++i) {
+ if (pt[i] < m_minBB[i])
+ m_minBB[i] = pt[i];
+ else if (pt[i] > m_maxBB[i])
+ m_maxBB[i] = pt[i];
+ }
+ }
+}
+template <class T>
+void Volume::Voxelize(const T* const points, const unsigned int stridePoints, const unsigned int nPoints,
+ const int* const triangles, const unsigned int strideTriangles, const unsigned int nTriangles,
+ const size_t dim, const Vec3<double>& barycenter, const double (&rot)[3][3])
+{
+ if (nPoints == 0) {
+ return;
+ }
+ ComputeBB(points, stridePoints, nPoints, barycenter, rot);
+
+ double d[3] = { m_maxBB[0] - m_minBB[0], m_maxBB[1] - m_minBB[1], m_maxBB[2] - m_minBB[2] };
+ double r;
+ if (d[0] > d[1] && d[0] > d[2]) {
+ r = d[0];
+ m_dim[0] = dim;
+ m_dim[1] = 2 + static_cast<size_t>(dim * d[1] / d[0]);
+ m_dim[2] = 2 + static_cast<size_t>(dim * d[2] / d[0]);
+ }
+ else if (d[1] > d[0] && d[1] > d[2]) {
+ r = d[1];
+ m_dim[1] = dim;
+ m_dim[0] = 2 + static_cast<size_t>(dim * d[0] / d[1]);
+ m_dim[2] = 2 + static_cast<size_t>(dim * d[2] / d[1]);
+ }
+ else {
+ r = d[2];
+ m_dim[2] = dim;
+ m_dim[0] = 2 + static_cast<size_t>(dim * d[0] / d[2]);
+ m_dim[1] = 2 + static_cast<size_t>(dim * d[1] / d[2]);
+ }
+
+ m_scale = r / (dim - 1);
+ double invScale = (dim - 1) / r;
+
+ Allocate();
+ m_numVoxelsOnSurface = 0;
+ m_numVoxelsInsideSurface = 0;
+ m_numVoxelsOutsideSurface = 0;
+
+ Vec3<double> p[3];
+ size_t i, j, k;
+ size_t i0, j0, k0;
+ size_t i1, j1, k1;
+ Vec3<double> boxcenter;
+ Vec3<double> pt;
+ const Vec3<double> boxhalfsize(0.5, 0.5, 0.5);
+ for (size_t t = 0, ti = 0; t < nTriangles; ++t, ti += strideTriangles) {
+ Vec3<int> tri(triangles[ti + 0],
+ triangles[ti + 1],
+ triangles[ti + 2]);
+ for (int c = 0; c < 3; ++c) {
+ ComputeAlignedPoint(points, tri[c] * stridePoints, barycenter, rot, pt);
+ p[c][0] = (pt[0] - m_minBB[0]) * invScale;
+ p[c][1] = (pt[1] - m_minBB[1]) * invScale;
+ p[c][2] = (pt[2] - m_minBB[2]) * invScale;
+ i = static_cast<size_t>(p[c][0] + 0.5);
+ j = static_cast<size_t>(p[c][1] + 0.5);
+ k = static_cast<size_t>(p[c][2] + 0.5);
+ assert(i < m_dim[0] && i >= 0 && j < m_dim[1] && j >= 0 && k < m_dim[2] && k >= 0);
+
+ if (c == 0) {
+ i0 = i1 = i;
+ j0 = j1 = j;
+ k0 = k1 = k;
+ }
+ else {
+ if (i < i0)
+ i0 = i;
+ if (j < j0)
+ j0 = j;
+ if (k < k0)
+ k0 = k;
+ if (i > i1)
+ i1 = i;
+ if (j > j1)
+ j1 = j;
+ if (k > k1)
+ k1 = k;
+ }
+ }
+ if (i0 > 0)
+ --i0;
+ if (j0 > 0)
+ --j0;
+ if (k0 > 0)
+ --k0;
+ if (i1 < m_dim[0])
+ ++i1;
+ if (j1 < m_dim[1])
+ ++j1;
+ if (k1 < m_dim[2])
+ ++k1;
+ for (size_t i = i0; i < i1; ++i) {
+ boxcenter[0] = (double)i;
+ for (size_t j = j0; j < j1; ++j) {
+ boxcenter[1] = (double)j;
+ for (size_t k = k0; k < k1; ++k) {
+ boxcenter[2] = (double)k;
+ int res = TriBoxOverlap(boxcenter, boxhalfsize, p[0], p[1], p[2]);
+ unsigned char& value = GetVoxel(i, j, k);
+ if (res == 1 && value == PRIMITIVE_UNDEFINED) {
+ value = PRIMITIVE_ON_SURFACE;
+ ++m_numVoxelsOnSurface;
+ }
+ }
+ }
+ }
+ }
+ FillOutsideSurface(0, 0, 0, m_dim[0], m_dim[1], 1);
+ FillOutsideSurface(0, 0, m_dim[2] - 1, m_dim[0], m_dim[1], m_dim[2]);
+ FillOutsideSurface(0, 0, 0, m_dim[0], 1, m_dim[2]);
+ FillOutsideSurface(0, m_dim[1] - 1, 0, m_dim[0], m_dim[1], m_dim[2]);
+ FillOutsideSurface(0, 0, 0, 1, m_dim[1], m_dim[2]);
+ FillOutsideSurface(m_dim[0] - 1, 0, 0, m_dim[0], m_dim[1], m_dim[2]);
+ FillInsideSurface();
+}
+}
+#endif // VHACD_VOLUME_H
diff --git a/Extras/VHACD/premake4.lua b/Extras/VHACD/premake4.lua
new file mode 100644
index 0000000..f66f160
--- /dev/null
+++ b/Extras/VHACD/premake4.lua
@@ -0,0 +1,4 @@
+
+include "src"
+include "test/src"
+
diff --git a/Extras/VHACD/public/VHACD.h b/Extras/VHACD/public/VHACD.h
new file mode 100644
index 0000000..20f6aac
--- /dev/null
+++ b/Extras/VHACD/public/VHACD.h
@@ -0,0 +1,121 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#pragma once
+#ifndef VHACD_H
+#define VHACD_H
+
+#define VHACD_VERSION_MAJOR 2
+#define VHACD_VERSION_MINOR 2
+
+namespace VHACD {
+class IVHACD {
+public:
+ class IUserCallback {
+ public:
+ virtual ~IUserCallback(){};
+ virtual void Update(const double overallProgress,
+ const double stageProgress,
+ const double operationProgress,
+ const char* const stage,
+ const char* const operation)
+ = 0;
+ };
+
+ class IUserLogger {
+ public:
+ virtual ~IUserLogger(){};
+ virtual void Log(const char* const msg) = 0;
+ };
+
+ class ConvexHull {
+ public:
+ double* m_points;
+ int* m_triangles;
+ unsigned int m_nPoints;
+ unsigned int m_nTriangles;
+ };
+
+ class Parameters {
+ public:
+ Parameters(void) { Init(); }
+ void Init(void)
+ {
+ m_resolution = 1000000;
+ m_depth = 20;
+ m_concavity = 0.001;
+ m_planeDownsampling = 4;
+ m_convexhullDownsampling = 4;
+ m_alpha = 0.05;
+ m_beta = 0.05;
+ m_gamma = 0.0005;
+ m_pca = 0;
+ m_mode = 0; // 0: voxel-based (recommended), 1: tetrahedron-based
+ m_maxNumVerticesPerCH = 64;
+ m_minVolumePerCH = 0.0001;
+ m_callback = 0;
+ m_logger = 0;
+ m_convexhullApproximation = true;
+ m_oclAcceleration = true;
+ }
+ double m_concavity;
+ double m_alpha;
+ double m_beta;
+ double m_gamma;
+ double m_minVolumePerCH;
+ IUserCallback* m_callback;
+ IUserLogger* m_logger;
+ unsigned int m_resolution;
+ unsigned int m_maxNumVerticesPerCH;
+ int m_depth;
+ int m_planeDownsampling;
+ int m_convexhullDownsampling;
+ int m_pca;
+ int m_mode;
+ int m_convexhullApproximation;
+ int m_oclAcceleration;
+ };
+
+ virtual void Cancel() = 0;
+ virtual bool Compute(const float* const points,
+ const unsigned int stridePoints,
+ const unsigned int countPoints,
+ const int* const triangles,
+ const unsigned int strideTriangles,
+ const unsigned int countTriangles,
+ const Parameters& params)
+ = 0;
+ virtual bool Compute(const double* const points,
+ const unsigned int stridePoints,
+ const unsigned int countPoints,
+ const int* const triangles,
+ const unsigned int strideTriangles,
+ const unsigned int countTriangles,
+ const Parameters& params)
+ = 0;
+ virtual unsigned int GetNConvexHulls() const = 0;
+ virtual void GetConvexHull(const unsigned int index, ConvexHull& ch) const = 0;
+ virtual void Clean(void) = 0; // release internally allocated memory
+ virtual void Release(void) = 0; // release IVHACD
+ virtual bool OCLInit(void* const oclDevice,
+ IUserLogger* const logger = 0)
+ = 0;
+ virtual bool OCLRelease(IUserLogger* const logger = 0) = 0;
+
+protected:
+ virtual ~IVHACD(void) {}
+};
+IVHACD* CreateVHACD(void);
+}
+#endif // VHACD_H
\ No newline at end of file
diff --git a/Extras/VHACD/src/VHACD.cpp b/Extras/VHACD/src/VHACD.cpp
new file mode 100644
index 0000000..8f1d309
--- /dev/null
+++ b/Extras/VHACD/src/VHACD.cpp
@@ -0,0 +1,1433 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+
+#define _CRT_SECURE_NO_WARNINGS
+
+#include <algorithm>
+#include <fstream>
+#include <iomanip>
+#include <limits>
+#include <sstream>
+#if _OPENMP
+#include <omp.h>
+#endif // _OPENMP
+
+#include "../public/VHACD.h"
+#include "btConvexHullComputer.h"
+#include "vhacdICHull.h"
+#include "vhacdMesh.h"
+#include "vhacdSArray.h"
+#include "vhacdTimer.h"
+#include "vhacdVHACD.h"
+#include "vhacdVector.h"
+#include "vhacdVolume.h"
+
+#define MAX(a, b) (((a) > (b)) ? (a) : (b))
+#define MIN(a, b) (((a) < (b)) ? (a) : (b))
+#define ABS(a) (((a) < 0) ? -(a) : (a))
+#define ZSGN(a) (((a) < 0) ? -1 : (a) > 0 ? 1 : 0)
+#define MAX_DOUBLE (1.79769e+308)
+
+#ifdef USE_SSE
+#include <immintrin.h>
+
+const int SIMD_WIDTH = 4;
+inline int FindMinimumElement(const float* const d, float* const m, const int n)
+{
+ // Min within vectors
+ __m128 min_i = _mm_set1_ps(-1.0f);
+ __m128 min_v = _mm_set1_ps(std::numeric_limits<float>::max());
+ for (int i = 0; i <= n - SIMD_WIDTH; i += SIMD_WIDTH) {
+ const __m128 data = _mm_load_ps(&d[i]);
+ const __m128 pred = _mm_cmplt_ps(data, min_v);
+
+ min_i = _mm_blendv_ps(min_i, _mm_set1_ps(i), pred);
+ min_v = _mm_min_ps(data, min_v);
+ }
+
+ /* Min within vector */
+ const __m128 min1 = _mm_shuffle_ps(min_v, min_v, _MM_SHUFFLE(1, 0, 3, 2));
+ const __m128 min2 = _mm_min_ps(min_v, min1);
+ const __m128 min3 = _mm_shuffle_ps(min2, min2, _MM_SHUFFLE(0, 1, 0, 1));
+ const __m128 min4 = _mm_min_ps(min2, min3);
+ float min_d = _mm_cvtss_f32(min4);
+
+ // Min index
+ const int min_idx = __builtin_ctz(_mm_movemask_ps(_mm_cmpeq_ps(min_v, min4)));
+ int ret = min_i[min_idx] + min_idx;
+
+ // Trailing elements
+ for (int i = (n & ~(SIMD_WIDTH - 1)); i < n; ++i) {
+ if (d[i] < min_d) {
+ min_d = d[i];
+ ret = i;
+ }
+ }
+
+ *m = min_d;
+ return ret;
+}
+
+inline int FindMinimumElement(const float* const d, float* const m, const int begin, const int end)
+{
+ // Leading elements
+ int min_i = -1;
+ float min_d = std::numeric_limits<float>::max();
+ const int aligned = (begin & ~(SIMD_WIDTH - 1)) + ((begin & (SIMD_WIDTH - 1)) ? SIMD_WIDTH : 0);
+ for (int i = begin; i < std::min(end, aligned); ++i) {
+ if (d[i] < min_d) {
+ min_d = d[i];
+ min_i = i;
+ }
+ }
+
+ // Middle and trailing elements
+ float r_m = std::numeric_limits<float>::max();
+ const int n = end - aligned;
+ const int r_i = (n > 0) ? FindMinimumElement(&d[aligned], &r_m, n) : 0;
+
+ // Pick the lowest
+ if (r_m < min_d) {
+ *m = r_m;
+ return r_i + aligned;
+ }
+ else {
+ *m = min_d;
+ return min_i;
+ }
+}
+#else
+inline int FindMinimumElement(const float* const d, float* const m, const int begin, const int end)
+{
+ int idx = -1;
+ float min = (std::numeric_limits<float>::max)();
+ for (int i = begin; i < end; ++i) {
+ if (d[i] < min) {
+ idx = i;
+ min = d[i];
+ }
+ }
+
+ *m = min;
+ return idx;
+}
+#endif
+
+//#define OCL_SOURCE_FROM_FILE
+#ifndef OCL_SOURCE_FROM_FILE
+const char* oclProgramSource = "\
+__kernel void ComputePartialVolumes(__global short4 * voxels, \
+ const int numVoxels, \
+ const float4 plane, \
+ const float4 minBB, \
+ const float4 scale, \
+ __local uint4 * localPartialVolumes, \
+ __global uint4 * partialVolumes) \
+{ \
+ int localId = get_local_id(0); \
+ int groupSize = get_local_size(0); \
+ int i0 = get_global_id(0) << 2; \
+ float4 voxel; \
+ uint4 v; \
+ voxel = convert_float4(voxels[i0]); \
+ v.s0 = (dot(plane, mad(scale, voxel, minBB)) >= 0.0f) * (i0 < numVoxels);\
+ voxel = convert_float4(voxels[i0 + 1]); \
+ v.s1 = (dot(plane, mad(scale, voxel, minBB)) >= 0.0f) * (i0 + 1 < numVoxels);\
+ voxel = convert_float4(voxels[i0 + 2]); \
+ v.s2 = (dot(plane, mad(scale, voxel, minBB)) >= 0.0f) * (i0 + 2 < numVoxels);\
+ voxel = convert_float4(voxels[i0 + 3]); \
+ v.s3 = (dot(plane, mad(scale, voxel, minBB)) >= 0.0f) * (i0 + 3 < numVoxels);\
+ localPartialVolumes[localId] = v; \
+ barrier(CLK_LOCAL_MEM_FENCE); \
+ for (int i = groupSize >> 1; i > 0; i >>= 1) \
+ { \
+ if (localId < i) \
+ { \
+ localPartialVolumes[localId] += localPartialVolumes[localId + i]; \
+ } \
+ barrier(CLK_LOCAL_MEM_FENCE); \
+ } \
+ if (localId == 0) \
+ { \
+ partialVolumes[get_group_id(0)] = localPartialVolumes[0]; \
+ } \
+} \
+__kernel void ComputePartialSums(__global uint4 * data, \
+ const int dataSize, \
+ __local uint4 * partialSums) \
+{ \
+ int globalId = get_global_id(0); \
+ int localId = get_local_id(0); \
+ int groupSize = get_local_size(0); \
+ int i; \
+ if (globalId < dataSize) \
+ { \
+ partialSums[localId] = data[globalId]; \
+ } \
+ else \
+ { \
+ partialSums[localId] = (0, 0, 0, 0); \
+ } \
+ barrier(CLK_LOCAL_MEM_FENCE); \
+ for (i = groupSize >> 1; i > 0; i >>= 1) \
+ { \
+ if (localId < i) \
+ { \
+ partialSums[localId] += partialSums[localId + i]; \
+ } \
+ barrier(CLK_LOCAL_MEM_FENCE); \
+ } \
+ if (localId == 0) \
+ { \
+ data[get_group_id(0)] = partialSums[0]; \
+ } \
+}";
+#endif //OCL_SOURCE_FROM_FILE
+
+namespace VHACD {
+IVHACD* CreateVHACD(void)
+{
+ return new VHACD();
+}
+bool VHACD::OCLInit(void* const oclDevice, IUserLogger* const logger)
+{
+#ifdef CL_VERSION_1_1
+ m_oclDevice = (cl_device_id*)oclDevice;
+ cl_int error;
+ m_oclContext = clCreateContext(NULL, 1, m_oclDevice, NULL, NULL, &error);
+ if (error != CL_SUCCESS) {
+ if (logger) {
+ logger->Log("Couldn't create context\n");
+ }
+ return false;
+ }
+
+#ifdef OCL_SOURCE_FROM_FILE
+ std::string cl_files = OPENCL_CL_FILES;
+// read kernal from file
+#ifdef _WIN32
+ std::replace(cl_files.begin(), cl_files.end(), '/', '\\');
+#endif // _WIN32
+
+ FILE* program_handle = fopen(cl_files.c_str(), "rb");
+ fseek(program_handle, 0, SEEK_END);
+ size_t program_size = ftell(program_handle);
+ rewind(program_handle);
+ char* program_buffer = new char[program_size + 1];
+ program_buffer[program_size] = '\0';
+ fread(program_buffer, sizeof(char), program_size, program_handle);
+ fclose(program_handle);
+ // create program
+ m_oclProgram = clCreateProgramWithSource(m_oclContext, 1, (const char**)&program_buffer, &program_size, &error);
+ delete[] program_buffer;
+#else
+ size_t program_size = strlen(oclProgramSource);
+ m_oclProgram = clCreateProgramWithSource(m_oclContext, 1, (const char**)&oclProgramSource, &program_size, &error);
+#endif
+ if (error != CL_SUCCESS) {
+ if (logger) {
+ logger->Log("Couldn't create program\n");
+ }
+ return false;
+ }
+
+ /* Build program */
+ error = clBuildProgram(m_oclProgram, 1, m_oclDevice, "-cl-denorms-are-zero", NULL, NULL);
+ if (error != CL_SUCCESS) {
+ size_t log_size;
+ /* Find Size of log and print to std output */
+ clGetProgramBuildInfo(m_oclProgram, *m_oclDevice, CL_PROGRAM_BUILD_LOG, 0, NULL, &log_size);
+ char* program_log = new char[log_size + 2];
+ program_log[log_size] = '\n';
+ program_log[log_size + 1] = '\0';
+ clGetProgramBuildInfo(m_oclProgram, *m_oclDevice, CL_PROGRAM_BUILD_LOG, log_size + 1, program_log, NULL);
+ if (logger) {
+ logger->Log("Couldn't build program\n");
+ logger->Log(program_log);
+ }
+ delete[] program_log;
+ return false;
+ }
+
+ delete[] m_oclQueue;
+ delete[] m_oclKernelComputePartialVolumes;
+ delete[] m_oclKernelComputeSum;
+ m_oclQueue = new cl_command_queue[m_ompNumProcessors];
+ m_oclKernelComputePartialVolumes = new cl_kernel[m_ompNumProcessors];
+ m_oclKernelComputeSum = new cl_kernel[m_ompNumProcessors];
+
+ const char nameKernelComputePartialVolumes[] = "ComputePartialVolumes";
+ const char nameKernelComputeSum[] = "ComputePartialSums";
+ for (int k = 0; k < m_ompNumProcessors; ++k) {
+ m_oclKernelComputePartialVolumes[k] = clCreateKernel(m_oclProgram, nameKernelComputePartialVolumes, &error);
+ if (error != CL_SUCCESS) {
+ if (logger) {
+ logger->Log("Couldn't create kernel\n");
+ }
+ return false;
+ }
+ m_oclKernelComputeSum[k] = clCreateKernel(m_oclProgram, nameKernelComputeSum, &error);
+ if (error != CL_SUCCESS) {
+ if (logger) {
+ logger->Log("Couldn't create kernel\n");
+ }
+ return false;
+ }
+ }
+
+ error = clGetKernelWorkGroupInfo(m_oclKernelComputePartialVolumes[0],
+ *m_oclDevice,
+ CL_KERNEL_WORK_GROUP_SIZE,
+ sizeof(size_t),
+ &m_oclWorkGroupSize,
+ NULL);
+ size_t workGroupSize = 0;
+ error = clGetKernelWorkGroupInfo(m_oclKernelComputeSum[0],
+ *m_oclDevice,
+ CL_KERNEL_WORK_GROUP_SIZE,
+ sizeof(size_t),
+ &workGroupSize,
+ NULL);
+ if (error != CL_SUCCESS) {
+ if (logger) {
+ logger->Log("Couldn't query work group info\n");
+ }
+ return false;
+ }
+
+ if (workGroupSize < m_oclWorkGroupSize) {
+ m_oclWorkGroupSize = workGroupSize;
+ }
+
+ for (int k = 0; k < m_ompNumProcessors; ++k) {
+ m_oclQueue[k] = clCreateCommandQueue(m_oclContext, *m_oclDevice, 0 /*CL_QUEUE_PROFILING_ENABLE*/, &error);
+ if (error != CL_SUCCESS) {
+ if (logger) {
+ logger->Log("Couldn't create queue\n");
+ }
+ return false;
+ }
+ }
+ return true;
+#else //CL_VERSION_1_1
+ return false;
+#endif //CL_VERSION_1_1
+}
+bool VHACD::OCLRelease(IUserLogger* const logger)
+{
+#ifdef CL_VERSION_1_1
+ cl_int error;
+ if (m_oclKernelComputePartialVolumes) {
+ for (int k = 0; k < m_ompNumProcessors; ++k) {
+ error = clReleaseKernel(m_oclKernelComputePartialVolumes[k]);
+ if (error != CL_SUCCESS) {
+ if (logger) {
+ logger->Log("Couldn't release kernal\n");
+ }
+ return false;
+ }
+ }
+ delete[] m_oclKernelComputePartialVolumes;
+ }
+ if (m_oclKernelComputeSum) {
+ for (int k = 0; k < m_ompNumProcessors; ++k) {
+ error = clReleaseKernel(m_oclKernelComputeSum[k]);
+ if (error != CL_SUCCESS) {
+ if (logger) {
+ logger->Log("Couldn't release kernal\n");
+ }
+ return false;
+ }
+ }
+ delete[] m_oclKernelComputeSum;
+ }
+ if (m_oclQueue) {
+ for (int k = 0; k < m_ompNumProcessors; ++k) {
+ error = clReleaseCommandQueue(m_oclQueue[k]);
+ if (error != CL_SUCCESS) {
+ if (logger) {
+ logger->Log("Couldn't release queue\n");
+ }
+ return false;
+ }
+ }
+ delete[] m_oclQueue;
+ }
+ error = clReleaseProgram(m_oclProgram);
+ if (error != CL_SUCCESS) {
+ if (logger) {
+ logger->Log("Couldn't release program\n");
+ }
+ return false;
+ }
+ error = clReleaseContext(m_oclContext);
+ if (error != CL_SUCCESS) {
+ if (logger) {
+ logger->Log("Couldn't release context\n");
+ }
+ return false;
+ }
+
+ return true;
+#else //CL_VERSION_1_1
+ return false;
+#endif //CL_VERSION_1_1
+}
+void VHACD::ComputePrimitiveSet(const Parameters& params)
+{
+ if (GetCancel()) {
+ return;
+ }
+ m_timer.Tic();
+
+ m_stage = "Compute primitive set";
+ m_operation = "Convert volume to pset";
+
+ std::ostringstream msg;
+ if (params.m_logger) {
+ msg << "+ " << m_stage << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ Update(0.0, 0.0, params);
+ if (params.m_mode == 0) {
+ VoxelSet* vset = new VoxelSet;
+ m_volume->Convert(*vset);
+ m_pset = vset;
+ }
+ else {
+ TetrahedronSet* tset = new TetrahedronSet;
+ m_volume->Convert(*tset);
+ m_pset = tset;
+ }
+
+ delete m_volume;
+ m_volume = 0;
+
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t # primitives " << m_pset->GetNPrimitives() << std::endl;
+ msg << "\t # inside surface " << m_pset->GetNPrimitivesInsideSurf() << std::endl;
+ msg << "\t # on surface " << m_pset->GetNPrimitivesOnSurf() << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ m_overallProgress = 15.0;
+ Update(100.0, 100.0, params);
+ m_timer.Toc();
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t time " << m_timer.GetElapsedTime() / 1000.0 << "s" << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+}
+bool VHACD::Compute(const double* const points, const unsigned int stridePoints, const unsigned int nPoints,
+ const int* const triangles, const unsigned int strideTriangles, const unsigned int nTriangles, const Parameters& params)
+{
+ return ComputeACD(points, stridePoints, nPoints, triangles, strideTriangles, nTriangles, params);
+}
+bool VHACD::Compute(const float* const points, const unsigned int stridePoints, const unsigned int nPoints,
+ const int* const triangles, const unsigned int strideTriangles, const unsigned int nTriangles, const Parameters& params)
+{
+ return ComputeACD(points, stridePoints, nPoints, triangles, strideTriangles, nTriangles, params);
+}
+double ComputePreferredCuttingDirection(const PrimitiveSet* const tset, Vec3<double>& dir)
+{
+ double ex = tset->GetEigenValue(AXIS_X);
+ double ey = tset->GetEigenValue(AXIS_Y);
+ double ez = tset->GetEigenValue(AXIS_Z);
+ double vx = (ey - ez) * (ey - ez);
+ double vy = (ex - ez) * (ex - ez);
+ double vz = (ex - ey) * (ex - ey);
+ if (vx < vy && vx < vz) {
+ double e = ey * ey + ez * ez;
+ dir[0] = 1.0;
+ dir[1] = 0.0;
+ dir[2] = 0.0;
+ return (e == 0.0) ? 0.0 : 1.0 - vx / e;
+ }
+ else if (vy < vx && vy < vz) {
+ double e = ex * ex + ez * ez;
+ dir[0] = 0.0;
+ dir[1] = 1.0;
+ dir[2] = 0.0;
+ return (e == 0.0) ? 0.0 : 1.0 - vy / e;
+ }
+ else {
+ double e = ex * ex + ey * ey;
+ dir[0] = 0.0;
+ dir[1] = 0.0;
+ dir[2] = 1.0;
+ return (e == 0.0) ? 0.0 : 1.0 - vz / e;
+ }
+}
+void ComputeAxesAlignedClippingPlanes(const VoxelSet& vset, const short downsampling, SArray<Plane>& planes)
+{
+ const Vec3<short> minV = vset.GetMinBBVoxels();
+ const Vec3<short> maxV = vset.GetMaxBBVoxels();
+ Vec3<double> pt;
+ Plane plane;
+ const short i0 = minV[0];
+ const short i1 = maxV[0];
+ plane.m_a = 1.0;
+ plane.m_b = 0.0;
+ plane.m_c = 0.0;
+ plane.m_axis = AXIS_X;
+ for (short i = i0; i <= i1; i += downsampling) {
+ pt = vset.GetPoint(Vec3<double>(i + 0.5, 0.0, 0.0));
+ plane.m_d = -pt[0];
+ plane.m_index = i;
+ planes.PushBack(plane);
+ }
+ const short j0 = minV[1];
+ const short j1 = maxV[1];
+ plane.m_a = 0.0;
+ plane.m_b = 1.0;
+ plane.m_c = 0.0;
+ plane.m_axis = AXIS_Y;
+ for (short j = j0; j <= j1; j += downsampling) {
+ pt = vset.GetPoint(Vec3<double>(0.0, j + 0.5, 0.0));
+ plane.m_d = -pt[1];
+ plane.m_index = j;
+ planes.PushBack(plane);
+ }
+ const short k0 = minV[2];
+ const short k1 = maxV[2];
+ plane.m_a = 0.0;
+ plane.m_b = 0.0;
+ plane.m_c = 1.0;
+ plane.m_axis = AXIS_Z;
+ for (short k = k0; k <= k1; k += downsampling) {
+ pt = vset.GetPoint(Vec3<double>(0.0, 0.0, k + 0.5));
+ plane.m_d = -pt[2];
+ plane.m_index = k;
+ planes.PushBack(plane);
+ }
+}
+void ComputeAxesAlignedClippingPlanes(const TetrahedronSet& tset, const short downsampling, SArray<Plane>& planes)
+{
+ const Vec3<double> minV = tset.GetMinBB();
+ const Vec3<double> maxV = tset.GetMaxBB();
+ const double scale = tset.GetSacle();
+ const short i0 = 0;
+ const short j0 = 0;
+ const short k0 = 0;
+ const short i1 = static_cast<short>((maxV[0] - minV[0]) / scale + 0.5);
+ const short j1 = static_cast<short>((maxV[1] - minV[1]) / scale + 0.5);
+ const short k1 = static_cast<short>((maxV[2] - minV[2]) / scale + 0.5);
+
+ Plane plane;
+ plane.m_a = 1.0;
+ plane.m_b = 0.0;
+ plane.m_c = 0.0;
+ plane.m_axis = AXIS_X;
+ for (short i = i0; i <= i1; i += downsampling) {
+ double x = minV[0] + scale * i;
+ plane.m_d = -x;
+ plane.m_index = i;
+ planes.PushBack(plane);
+ }
+ plane.m_a = 0.0;
+ plane.m_b = 1.0;
+ plane.m_c = 0.0;
+ plane.m_axis = AXIS_Y;
+ for (short j = j0; j <= j1; j += downsampling) {
+ double y = minV[1] + scale * j;
+ plane.m_d = -y;
+ plane.m_index = j;
+ planes.PushBack(plane);
+ }
+ plane.m_a = 0.0;
+ plane.m_b = 0.0;
+ plane.m_c = 1.0;
+ plane.m_axis = AXIS_Z;
+ for (short k = k0; k <= k1; k += downsampling) {
+ double z = minV[2] + scale * k;
+ plane.m_d = -z;
+ plane.m_index = k;
+ planes.PushBack(plane);
+ }
+}
+void RefineAxesAlignedClippingPlanes(const VoxelSet& vset, const Plane& bestPlane, const short downsampling,
+ SArray<Plane>& planes)
+{
+ const Vec3<short> minV = vset.GetMinBBVoxels();
+ const Vec3<short> maxV = vset.GetMaxBBVoxels();
+ Vec3<double> pt;
+ Plane plane;
+
+ if (bestPlane.m_axis == AXIS_X) {
+ const short i0 = MAX(minV[0], bestPlane.m_index - downsampling);
+ const short i1 = MIN(maxV[0], bestPlane.m_index + downsampling);
+ plane.m_a = 1.0;
+ plane.m_b = 0.0;
+ plane.m_c = 0.0;
+ plane.m_axis = AXIS_X;
+ for (short i = i0; i <= i1; ++i) {
+ pt = vset.GetPoint(Vec3<double>(i + 0.5, 0.0, 0.0));
+ plane.m_d = -pt[0];
+ plane.m_index = i;
+ planes.PushBack(plane);
+ }
+ }
+ else if (bestPlane.m_axis == AXIS_Y) {
+ const short j0 = MAX(minV[1], bestPlane.m_index - downsampling);
+ const short j1 = MIN(maxV[1], bestPlane.m_index + downsampling);
+ plane.m_a = 0.0;
+ plane.m_b = 1.0;
+ plane.m_c = 0.0;
+ plane.m_axis = AXIS_Y;
+ for (short j = j0; j <= j1; ++j) {
+ pt = vset.GetPoint(Vec3<double>(0.0, j + 0.5, 0.0));
+ plane.m_d = -pt[1];
+ plane.m_index = j;
+ planes.PushBack(plane);
+ }
+ }
+ else {
+ const short k0 = MAX(minV[2], bestPlane.m_index - downsampling);
+ const short k1 = MIN(maxV[2], bestPlane.m_index + downsampling);
+ plane.m_a = 0.0;
+ plane.m_b = 0.0;
+ plane.m_c = 1.0;
+ plane.m_axis = AXIS_Z;
+ for (short k = k0; k <= k1; ++k) {
+ pt = vset.GetPoint(Vec3<double>(0.0, 0.0, k + 0.5));
+ plane.m_d = -pt[2];
+ plane.m_index = k;
+ planes.PushBack(plane);
+ }
+ }
+}
+void RefineAxesAlignedClippingPlanes(const TetrahedronSet& tset, const Plane& bestPlane, const short downsampling,
+ SArray<Plane>& planes)
+{
+ const Vec3<double> minV = tset.GetMinBB();
+ const Vec3<double> maxV = tset.GetMaxBB();
+ const double scale = tset.GetSacle();
+ Plane plane;
+
+ if (bestPlane.m_axis == AXIS_X) {
+ const short i0 = MAX(0, bestPlane.m_index - downsampling);
+ const short i1 = static_cast<short>(MIN((maxV[0] - minV[0]) / scale + 0.5, bestPlane.m_index + downsampling));
+ plane.m_a = 1.0;
+ plane.m_b = 0.0;
+ plane.m_c = 0.0;
+ plane.m_axis = AXIS_X;
+ for (short i = i0; i <= i1; ++i) {
+ double x = minV[0] + scale * i;
+ plane.m_d = -x;
+ plane.m_index = i;
+ planes.PushBack(plane);
+ }
+ }
+ else if (bestPlane.m_axis == AXIS_Y) {
+ const short j0 = MAX(0, bestPlane.m_index - downsampling);
+ const short j1 = static_cast<short>(MIN((maxV[1] - minV[1]) / scale + 0.5, bestPlane.m_index + downsampling));
+ plane.m_a = 0.0;
+ plane.m_b = 1.0;
+ plane.m_c = 0.0;
+ plane.m_axis = AXIS_Y;
+ for (short j = j0; j <= j1; ++j) {
+ double y = minV[1] + scale * j;
+ plane.m_d = -y;
+ plane.m_index = j;
+ planes.PushBack(plane);
+ }
+ }
+ else {
+ const short k0 = MAX(0, bestPlane.m_index - downsampling);
+ const short k1 = static_cast<short>(MIN((maxV[2] - minV[2]) / scale + 0.5, bestPlane.m_index + downsampling));
+ plane.m_a = 0.0;
+ plane.m_b = 0.0;
+ plane.m_c = 1.0;
+ plane.m_axis = AXIS_Z;
+ for (short k = k0; k <= k1; ++k) {
+ double z = minV[2] + scale * k;
+ plane.m_d = -z;
+ plane.m_index = k;
+ planes.PushBack(plane);
+ }
+ }
+}
+inline double ComputeLocalConcavity(const double volume, const double volumeCH)
+{
+ return fabs(volumeCH - volume) / volumeCH;
+}
+inline double ComputeConcavity(const double volume, const double volumeCH, const double volume0)
+{
+ return fabs(volumeCH - volume) / volume0;
+}
+
+//#define DEBUG_TEMP
+void VHACD::ComputeBestClippingPlane(const PrimitiveSet* inputPSet, const double volume, const SArray<Plane>& planes,
+ const Vec3<double>& preferredCuttingDirection, const double w, const double alpha, const double beta,
+ const int convexhullDownsampling, const double progress0, const double progress1, Plane& bestPlane,
+ double& minConcavity, const Parameters& params)
+{
+ if (GetCancel()) {
+ return;
+ }
+ char msg[256];
+ size_t nPrimitives = inputPSet->GetNPrimitives();
+ bool oclAcceleration = (nPrimitives > OCL_MIN_NUM_PRIMITIVES && params.m_oclAcceleration && params.m_mode == 0) ? true : false;
+ int iBest = -1;
+ int nPlanes = static_cast<int>(planes.Size());
+ bool cancel = false;
+ int done = 0;
+ double minTotal = MAX_DOUBLE;
+ double minBalance = MAX_DOUBLE;
+ double minSymmetry = MAX_DOUBLE;
+ minConcavity = MAX_DOUBLE;
+
+ SArray<Vec3<double> >* chPts = new SArray<Vec3<double> >[2 * m_ompNumProcessors];
+ Mesh* chs = new Mesh[2 * m_ompNumProcessors];
+ PrimitiveSet* onSurfacePSet = inputPSet->Create();
+ inputPSet->SelectOnSurface(onSurfacePSet);
+
+ PrimitiveSet** psets = 0;
+ if (!params.m_convexhullApproximation) {
+ psets = new PrimitiveSet*[2 * m_ompNumProcessors];
+ for (int i = 0; i < 2 * m_ompNumProcessors; ++i) {
+ psets[i] = inputPSet->Create();
+ }
+ }
+
+#ifdef CL_VERSION_1_1
+ // allocate OpenCL data structures
+ cl_mem voxels;
+ cl_mem* partialVolumes = 0;
+ size_t globalSize = 0;
+ size_t nWorkGroups = 0;
+ double unitVolume = 0.0;
+ if (oclAcceleration) {
+ VoxelSet* vset = (VoxelSet*)inputPSet;
+ const Vec3<double> minBB = vset->GetMinBB();
+ const float fMinBB[4] = { (float)minBB[0], (float)minBB[1], (float)minBB[2], 1.0f };
+ const float fSclae[4] = { (float)vset->GetScale(), (float)vset->GetScale(), (float)vset->GetScale(), 0.0f };
+ const int nVoxels = (int)nPrimitives;
+ unitVolume = vset->GetUnitVolume();
+ nWorkGroups = (nPrimitives + 4 * m_oclWorkGroupSize - 1) / (4 * m_oclWorkGroupSize);
+ globalSize = nWorkGroups * m_oclWorkGroupSize;
+ cl_int error;
+ voxels = clCreateBuffer(m_oclContext,
+ CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
+ sizeof(Voxel) * nPrimitives,
+ vset->GetVoxels(),
+ &error);
+ if (error != CL_SUCCESS) {
+ if (params.m_logger) {
+ params.m_logger->Log("Couldn't create buffer\n");
+ }
+ SetCancel(true);
+ }
+
+ partialVolumes = new cl_mem[m_ompNumProcessors];
+ for (int i = 0; i < m_ompNumProcessors; ++i) {
+ partialVolumes[i] = clCreateBuffer(m_oclContext,
+ CL_MEM_WRITE_ONLY,
+ sizeof(unsigned int) * 4 * nWorkGroups,
+ NULL,
+ &error);
+ if (error != CL_SUCCESS) {
+ if (params.m_logger) {
+ params.m_logger->Log("Couldn't create buffer\n");
+ }
+ SetCancel(true);
+ break;
+ }
+ error = clSetKernelArg(m_oclKernelComputePartialVolumes[i], 0, sizeof(cl_mem), &voxels);
+ error |= clSetKernelArg(m_oclKernelComputePartialVolumes[i], 1, sizeof(unsigned int), &nVoxels);
+ error |= clSetKernelArg(m_oclKernelComputePartialVolumes[i], 3, sizeof(float) * 4, fMinBB);
+ error |= clSetKernelArg(m_oclKernelComputePartialVolumes[i], 4, sizeof(float) * 4, &fSclae);
+ error |= clSetKernelArg(m_oclKernelComputePartialVolumes[i], 5, sizeof(unsigned int) * 4 * m_oclWorkGroupSize, NULL);
+ error |= clSetKernelArg(m_oclKernelComputePartialVolumes[i], 6, sizeof(cl_mem), &(partialVolumes[i]));
+ error |= clSetKernelArg(m_oclKernelComputeSum[i], 0, sizeof(cl_mem), &(partialVolumes[i]));
+ error |= clSetKernelArg(m_oclKernelComputeSum[i], 2, sizeof(unsigned int) * 4 * m_oclWorkGroupSize, NULL);
+ if (error != CL_SUCCESS) {
+ if (params.m_logger) {
+ params.m_logger->Log("Couldn't kernel atguments \n");
+ }
+ SetCancel(true);
+ }
+ }
+ }
+#else // CL_VERSION_1_1
+ oclAcceleration = false;
+#endif // CL_VERSION_1_1
+
+#ifdef DEBUG_TEMP
+ Timer timerComputeCost;
+ timerComputeCost.Tic();
+#endif // DEBUG_TEMP
+
+#if USE_THREAD == 1 && _OPENMP
+#pragma omp parallel for
+#endif
+ for (int x = 0; x < nPlanes; ++x) {
+ int threadID = 0;
+#if USE_THREAD == 1 && _OPENMP
+ threadID = omp_get_thread_num();
+#pragma omp flush(cancel)
+#endif
+ if (!cancel) {
+ //Update progress
+ if (GetCancel()) {
+ cancel = true;
+#if USE_THREAD == 1 && _OPENMP
+#pragma omp flush(cancel)
+#endif
+ }
+ Plane plane = planes[x];
+
+ if (oclAcceleration) {
+#ifdef CL_VERSION_1_1
+ const float fPlane[4] = { (float)plane.m_a, (float)plane.m_b, (float)plane.m_c, (float)plane.m_d };
+ cl_int error = clSetKernelArg(m_oclKernelComputePartialVolumes[threadID], 2, sizeof(float) * 4, fPlane);
+ if (error != CL_SUCCESS) {
+ if (params.m_logger) {
+ params.m_logger->Log("Couldn't kernel atguments \n");
+ }
+ SetCancel(true);
+ }
+
+ error = clEnqueueNDRangeKernel(m_oclQueue[threadID], m_oclKernelComputePartialVolumes[threadID],
+ 1, NULL, &globalSize, &m_oclWorkGroupSize, 0, NULL, NULL);
+ if (error != CL_SUCCESS) {
+ if (params.m_logger) {
+ params.m_logger->Log("Couldn't run kernel \n");
+ }
+ SetCancel(true);
+ }
+ int nValues = (int)nWorkGroups;
+ while (nValues > 1) {
+ error = clSetKernelArg(m_oclKernelComputeSum[threadID], 1, sizeof(int), &nValues);
+ if (error != CL_SUCCESS) {
+ if (params.m_logger) {
+ params.m_logger->Log("Couldn't kernel atguments \n");
+ }
+ SetCancel(true);
+ }
+ size_t nWorkGroups = (nValues + m_oclWorkGroupSize - 1) / m_oclWorkGroupSize;
+ size_t globalSize = nWorkGroups * m_oclWorkGroupSize;
+ error = clEnqueueNDRangeKernel(m_oclQueue[threadID], m_oclKernelComputeSum[threadID],
+ 1, NULL, &globalSize, &m_oclWorkGroupSize, 0, NULL, NULL);
+ if (error != CL_SUCCESS) {
+ if (params.m_logger) {
+ params.m_logger->Log("Couldn't run kernel \n");
+ }
+ SetCancel(true);
+ }
+ nValues = (int)nWorkGroups;
+ }
+#endif // CL_VERSION_1_1
+ }
+
+ Mesh& leftCH = chs[threadID];
+ Mesh& rightCH = chs[threadID + m_ompNumProcessors];
+ rightCH.ResizePoints(0);
+ leftCH.ResizePoints(0);
+ rightCH.ResizeTriangles(0);
+ leftCH.ResizeTriangles(0);
+
+// compute convex-hulls
+#ifdef TEST_APPROX_CH
+ double volumeLeftCH1;
+ double volumeRightCH1;
+#endif //TEST_APPROX_CH
+ if (params.m_convexhullApproximation) {
+ SArray<Vec3<double> >& leftCHPts = chPts[threadID];
+ SArray<Vec3<double> >& rightCHPts = chPts[threadID + m_ompNumProcessors];
+ rightCHPts.Resize(0);
+ leftCHPts.Resize(0);
+ onSurfacePSet->Intersect(plane, &rightCHPts, &leftCHPts, convexhullDownsampling * 32);
+ inputPSet->GetConvexHull().Clip(plane, rightCHPts, leftCHPts);
+ rightCH.ComputeConvexHull((double*)rightCHPts.Data(), rightCHPts.Size());
+ leftCH.ComputeConvexHull((double*)leftCHPts.Data(), leftCHPts.Size());
+#ifdef TEST_APPROX_CH
+ Mesh leftCH1;
+ Mesh rightCH1;
+ VoxelSet right;
+ VoxelSet left;
+ onSurfacePSet->Clip(plane, &right, &left);
+ right.ComputeConvexHull(rightCH1, convexhullDownsampling);
+ left.ComputeConvexHull(leftCH1, convexhullDownsampling);
+
+ volumeLeftCH1 = leftCH1.ComputeVolume();
+ volumeRightCH1 = rightCH1.ComputeVolume();
+#endif //TEST_APPROX_CH
+ }
+ else {
+ PrimitiveSet* const right = psets[threadID];
+ PrimitiveSet* const left = psets[threadID + m_ompNumProcessors];
+ onSurfacePSet->Clip(plane, right, left);
+ right->ComputeConvexHull(rightCH, convexhullDownsampling);
+ left->ComputeConvexHull(leftCH, convexhullDownsampling);
+ }
+ double volumeLeftCH = leftCH.ComputeVolume();
+ double volumeRightCH = rightCH.ComputeVolume();
+
+ // compute clipped volumes
+ double volumeLeft = 0.0;
+ double volumeRight = 0.0;
+ if (oclAcceleration) {
+#ifdef CL_VERSION_1_1
+ unsigned int volumes[4];
+ cl_int error = clEnqueueReadBuffer(m_oclQueue[threadID], partialVolumes[threadID], CL_TRUE,
+ 0, sizeof(unsigned int) * 4, volumes, 0, NULL, NULL);
+ size_t nPrimitivesRight = volumes[0] + volumes[1] + volumes[2] + volumes[3];
+ size_t nPrimitivesLeft = nPrimitives - nPrimitivesRight;
+ volumeRight = nPrimitivesRight * unitVolume;
+ volumeLeft = nPrimitivesLeft * unitVolume;
+ if (error != CL_SUCCESS) {
+ if (params.m_logger) {
+ params.m_logger->Log("Couldn't read buffer \n");
+ }
+ SetCancel(true);
+ }
+#endif // CL_VERSION_1_1
+ }
+ else {
+ inputPSet->ComputeClippedVolumes(plane, volumeRight, volumeLeft);
+ }
+ double concavityLeft = ComputeConcavity(volumeLeft, volumeLeftCH, m_volumeCH0);
+ double concavityRight = ComputeConcavity(volumeRight, volumeRightCH, m_volumeCH0);
+ double concavity = (concavityLeft + concavityRight);
+
+ // compute cost
+ double balance = alpha * fabs(volumeLeft - volumeRight) / m_volumeCH0;
+ double d = w * (preferredCuttingDirection[0] * plane.m_a + preferredCuttingDirection[1] * plane.m_b + preferredCuttingDirection[2] * plane.m_c);
+ double symmetry = beta * d;
+ double total = concavity + balance + symmetry;
+
+#if USE_THREAD == 1 && _OPENMP
+#pragma omp critical
+#endif
+ {
+ if (total < minTotal || (total == minTotal && x < iBest)) {
+ minConcavity = concavity;
+ minBalance = balance;
+ minSymmetry = symmetry;
+ bestPlane = plane;
+ minTotal = total;
+ iBest = x;
+ }
+ ++done;
+ if (!(done & 127)) // reduce update frequency
+ {
+ double progress = done * (progress1 - progress0) / nPlanes + progress0;
+ Update(m_stageProgress, progress, params);
+ }
+ }
+ }
+ }
+
+#ifdef DEBUG_TEMP
+ timerComputeCost.Toc();
+ printf_s("Cost[%i] = %f\n", nPlanes, timerComputeCost.GetElapsedTime());
+#endif // DEBUG_TEMP
+
+#ifdef CL_VERSION_1_1
+ if (oclAcceleration) {
+ clReleaseMemObject(voxels);
+ for (int i = 0; i < m_ompNumProcessors; ++i) {
+ clReleaseMemObject(partialVolumes[i]);
+ }
+ delete[] partialVolumes;
+ }
+#endif // CL_VERSION_1_1
+
+ if (psets) {
+ for (int i = 0; i < 2 * m_ompNumProcessors; ++i) {
+ delete psets[i];
+ }
+ delete[] psets;
+ }
+ delete onSurfacePSet;
+ delete[] chPts;
+ delete[] chs;
+ if (params.m_logger) {
+ sprintf(msg, "\n\t\t\t Best %04i T=%2.6f C=%2.6f B=%2.6f S=%2.6f (%1.1f, %1.1f, %1.1f, %3.3f)\n\n", iBest, minTotal, minConcavity, minBalance, minSymmetry, bestPlane.m_a, bestPlane.m_b, bestPlane.m_c, bestPlane.m_d);
+ params.m_logger->Log(msg);
+ }
+}
+void VHACD::ComputeACD(const Parameters& params)
+{
+ if (GetCancel()) {
+ return;
+ }
+ m_timer.Tic();
+
+ m_stage = "Approximate Convex Decomposition";
+ m_stageProgress = 0.0;
+ std::ostringstream msg;
+ if (params.m_logger) {
+ msg << "+ " << m_stage << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ SArray<PrimitiveSet*> parts;
+ SArray<PrimitiveSet*> inputParts;
+ SArray<PrimitiveSet*> temp;
+ inputParts.PushBack(m_pset);
+ m_pset = 0;
+ SArray<Plane> planes;
+ SArray<Plane> planesRef;
+ int sub = 0;
+ bool firstIteration = true;
+ m_volumeCH0 = 1.0;
+ while (sub++ < params.m_depth && inputParts.Size() > 0 && !m_cancel) {
+ msg.str("");
+ msg << "Subdivision level " << sub;
+ m_operation = msg.str();
+
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t Subdivision level " << sub << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ double maxConcavity = 0.0;
+ const size_t nInputParts = inputParts.Size();
+ Update(m_stageProgress, 0.0, params);
+ for (size_t p = 0; p < nInputParts && !m_cancel; ++p) {
+ const double progress0 = p * 100.0 / nInputParts;
+ const double progress1 = (p + 0.75) * 100.0 / nInputParts;
+ const double progress2 = (p + 1.00) * 100.0 / nInputParts;
+
+ Update(m_stageProgress, progress0, params);
+
+ PrimitiveSet* pset = inputParts[p];
+ inputParts[p] = 0;
+ double volume = pset->ComputeVolume();
+ pset->ComputeBB();
+ pset->ComputePrincipalAxes();
+ if (params.m_pca) {
+ pset->AlignToPrincipalAxes();
+ }
+
+ pset->ComputeConvexHull(pset->GetConvexHull());
+ double volumeCH = fabs(pset->GetConvexHull().ComputeVolume());
+ if (firstIteration) {
+ m_volumeCH0 = volumeCH;
+ }
+
+ double concavity = ComputeConcavity(volume, volumeCH, m_volumeCH0);
+ double error = 1.01 * pset->ComputeMaxVolumeError() / m_volumeCH0;
+
+ if (firstIteration) {
+ firstIteration = false;
+ }
+
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t -> Part[" << p
+ << "] C = " << concavity
+ << ", E = " << error
+ << ", VS = " << pset->GetNPrimitivesOnSurf()
+ << ", VI = " << pset->GetNPrimitivesInsideSurf()
+ << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ if (concavity > params.m_concavity && concavity > error) {
+ Vec3<double> preferredCuttingDirection;
+ double w = ComputePreferredCuttingDirection(pset, preferredCuttingDirection);
+ planes.Resize(0);
+ if (params.m_mode == 0) {
+ VoxelSet* vset = (VoxelSet*)pset;
+ ComputeAxesAlignedClippingPlanes(*vset, params.m_planeDownsampling, planes);
+ }
+ else {
+ TetrahedronSet* tset = (TetrahedronSet*)pset;
+ ComputeAxesAlignedClippingPlanes(*tset, params.m_planeDownsampling, planes);
+ }
+
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t\t [Regular sampling] Number of clipping planes " << planes.Size() << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ Plane bestPlane;
+ double minConcavity = MAX_DOUBLE;
+ ComputeBestClippingPlane(pset,
+ volume,
+ planes,
+ preferredCuttingDirection,
+ w,
+ concavity * params.m_alpha,
+ concavity * params.m_beta,
+ params.m_convexhullDownsampling,
+ progress0,
+ progress1,
+ bestPlane,
+ minConcavity,
+ params);
+ if (!m_cancel && (params.m_planeDownsampling > 1 || params.m_convexhullDownsampling > 1)) {
+ planesRef.Resize(0);
+
+ if (params.m_mode == 0) {
+ VoxelSet* vset = (VoxelSet*)pset;
+ RefineAxesAlignedClippingPlanes(*vset, bestPlane, params.m_planeDownsampling, planesRef);
+ }
+ else {
+ TetrahedronSet* tset = (TetrahedronSet*)pset;
+ RefineAxesAlignedClippingPlanes(*tset, bestPlane, params.m_planeDownsampling, planesRef);
+ }
+
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t\t [Refining] Number of clipping planes " << planesRef.Size() << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+ ComputeBestClippingPlane(pset,
+ volume,
+ planesRef,
+ preferredCuttingDirection,
+ w,
+ concavity * params.m_alpha,
+ concavity * params.m_beta,
+ 1, // convexhullDownsampling = 1
+ progress1,
+ progress2,
+ bestPlane,
+ minConcavity,
+ params);
+ }
+ if (GetCancel()) {
+ delete pset; // clean up
+ break;
+ }
+ else {
+ if (maxConcavity < minConcavity) {
+ maxConcavity = minConcavity;
+ }
+ PrimitiveSet* bestLeft = pset->Create();
+ PrimitiveSet* bestRight = pset->Create();
+ temp.PushBack(bestLeft);
+ temp.PushBack(bestRight);
+ pset->Clip(bestPlane, bestRight, bestLeft);
+ if (params.m_pca) {
+ bestRight->RevertAlignToPrincipalAxes();
+ bestLeft->RevertAlignToPrincipalAxes();
+ }
+ delete pset;
+ }
+ }
+ else {
+ if (params.m_pca) {
+ pset->RevertAlignToPrincipalAxes();
+ }
+ parts.PushBack(pset);
+ }
+ }
+
+ Update(95.0 * (1.0 - maxConcavity) / (1.0 - params.m_concavity), 100.0, params);
+ if (GetCancel()) {
+ const size_t nTempParts = temp.Size();
+ for (size_t p = 0; p < nTempParts; ++p) {
+ delete temp[p];
+ }
+ temp.Resize(0);
+ }
+ else {
+ inputParts = temp;
+ temp.Resize(0);
+ }
+ }
+ const size_t nInputParts = inputParts.Size();
+ for (size_t p = 0; p < nInputParts; ++p) {
+ parts.PushBack(inputParts[p]);
+ }
+
+ if (GetCancel()) {
+ const size_t nParts = parts.Size();
+ for (size_t p = 0; p < nParts; ++p) {
+ delete parts[p];
+ }
+ return;
+ }
+
+ m_overallProgress = 90.0;
+ Update(m_stageProgress, 100.0, params);
+
+ msg.str("");
+ msg << "Generate convex-hulls";
+ m_operation = msg.str();
+ size_t nConvexHulls = parts.Size();
+ if (params.m_logger) {
+ msg.str("");
+ msg << "+ Generate " << nConvexHulls << " convex-hulls " << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ Update(m_stageProgress, 0.0, params);
+ m_convexHulls.Resize(0);
+ for (size_t p = 0; p < nConvexHulls && !m_cancel; ++p) {
+ Update(m_stageProgress, p * 100.0 / nConvexHulls, params);
+ m_convexHulls.PushBack(new Mesh);
+ parts[p]->ComputeConvexHull(*m_convexHulls[p]);
+ size_t nv = m_convexHulls[p]->GetNPoints();
+ double x, y, z;
+ for (size_t i = 0; i < nv; ++i) {
+ Vec3<double>& pt = m_convexHulls[p]->GetPoint(i);
+ x = pt[0];
+ y = pt[1];
+ z = pt[2];
+ pt[0] = m_rot[0][0] * x + m_rot[0][1] * y + m_rot[0][2] * z + m_barycenter[0];
+ pt[1] = m_rot[1][0] * x + m_rot[1][1] * y + m_rot[1][2] * z + m_barycenter[1];
+ pt[2] = m_rot[2][0] * x + m_rot[2][1] * y + m_rot[2][2] * z + m_barycenter[2];
+ }
+ }
+
+ const size_t nParts = parts.Size();
+ for (size_t p = 0; p < nParts; ++p) {
+ delete parts[p];
+ parts[p] = 0;
+ }
+ parts.Resize(0);
+
+ if (GetCancel()) {
+ const size_t nConvexHulls = m_convexHulls.Size();
+ for (size_t p = 0; p < nConvexHulls; ++p) {
+ delete m_convexHulls[p];
+ }
+ m_convexHulls.Clear();
+ return;
+ }
+
+ m_overallProgress = 95.0;
+ Update(100.0, 100.0, params);
+ m_timer.Toc();
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t time " << m_timer.GetElapsedTime() / 1000.0 << "s" << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+}
+void AddPoints(const Mesh* const mesh, SArray<Vec3<double> >& pts)
+{
+ const int n = (int)mesh->GetNPoints();
+ for (int i = 0; i < n; ++i) {
+ pts.PushBack(mesh->GetPoint(i));
+ }
+}
+void ComputeConvexHull(const Mesh* const ch1, const Mesh* const ch2, SArray<Vec3<double> >& pts, Mesh* const combinedCH)
+{
+ pts.Resize(0);
+ AddPoints(ch1, pts);
+ AddPoints(ch2, pts);
+
+ btConvexHullComputer ch;
+ ch.compute((double*)pts.Data(), 3 * sizeof(double), (int)pts.Size(), -1.0, -1.0);
+ combinedCH->ResizePoints(0);
+ combinedCH->ResizeTriangles(0);
+ for (int v = 0; v < ch.vertices.size(); v++) {
+ combinedCH->AddPoint(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ()));
+ }
+ const int nt = ch.faces.size();
+ for (int t = 0; t < nt; ++t) {
+ const btConvexHullComputer::Edge* sourceEdge = &(ch.edges[ch.faces[t]]);
+ int a = sourceEdge->getSourceVertex();
+ int b = sourceEdge->getTargetVertex();
+ const btConvexHullComputer::Edge* edge = sourceEdge->getNextEdgeOfFace();
+ int c = edge->getTargetVertex();
+ while (c != a) {
+ combinedCH->AddTriangle(Vec3<int>(a, b, c));
+ edge = edge->getNextEdgeOfFace();
+ b = c;
+ c = edge->getTargetVertex();
+ }
+ }
+}
+void VHACD::MergeConvexHulls(const Parameters& params)
+{
+ if (GetCancel()) {
+ return;
+ }
+ m_timer.Tic();
+
+ m_stage = "Merge Convex Hulls";
+
+ std::ostringstream msg;
+ if (params.m_logger) {
+ msg << "+ " << m_stage << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ size_t nConvexHulls = m_convexHulls.Size();
+ int iteration = 0;
+ if (nConvexHulls > 1 && !m_cancel) {
+ const double threshold = params.m_gamma;
+ SArray<Vec3<double> > pts;
+ Mesh combinedCH;
+
+ // Populate the cost matrix
+ size_t idx = 0;
+ SArray<float> costMatrix;
+ costMatrix.Resize(((nConvexHulls * nConvexHulls) - nConvexHulls) >> 1);
+ for (size_t p1 = 1; p1 < nConvexHulls; ++p1) {
+ const float volume1 = m_convexHulls[p1]->ComputeVolume();
+ for (size_t p2 = 0; p2 < p1; ++p2) {
+ ComputeConvexHull(m_convexHulls[p1], m_convexHulls[p2], pts, &combinedCH);
+ costMatrix[idx++] = ComputeConcavity(volume1 + m_convexHulls[p2]->ComputeVolume(), combinedCH.ComputeVolume(), m_volumeCH0);
+ }
+ }
+
+ // Until we cant merge below the maximum cost
+ size_t costSize = m_convexHulls.Size();
+ while (!m_cancel) {
+ msg.str("");
+ msg << "Iteration " << iteration++;
+ m_operation = msg.str();
+
+ // Search for lowest cost
+ float bestCost = (std::numeric_limits<float>::max)();
+ const size_t addr = FindMinimumElement(costMatrix.Data(), &bestCost, 0, costMatrix.Size());
+
+ // Check if we should merge these hulls
+ if (bestCost >= threshold) {
+ break;
+ }
+ double nr = 1 + (8 * addr);
+ const size_t addrI = (static_cast<int>(sqrt(nr)) - 1) >> 1;
+ const size_t p1 = addrI + 1;
+ const size_t p2 = addr - ((addrI * (addrI + 1)) >> 1);
+ assert(p1 >= 0);
+ assert(p2 >= 0);
+ assert(p1 < costSize);
+ assert(p2 < costSize);
+
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t\t Merging (" << p1 << ", " << p2 << ") " << bestCost << std::endl
+ << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ // Make the lowest cost row and column into a new hull
+ Mesh* cch = new Mesh;
+ ComputeConvexHull(m_convexHulls[p1], m_convexHulls[p2], pts, cch);
+ delete m_convexHulls[p2];
+ m_convexHulls[p2] = cch;
+
+ delete m_convexHulls[p1];
+ std::swap(m_convexHulls[p1], m_convexHulls[m_convexHulls.Size() - 1]);
+ m_convexHulls.PopBack();
+
+ costSize = costSize - 1;
+
+ // Calculate costs versus the new hull
+ size_t rowIdx = ((p2 - 1) * p2) >> 1;
+ const float volume1 = m_convexHulls[p2]->ComputeVolume();
+ for (size_t i = 0; (i < p2) && (!m_cancel); ++i) {
+ ComputeConvexHull(m_convexHulls[p2], m_convexHulls[i], pts, &combinedCH);
+ costMatrix[rowIdx++] = ComputeConcavity(volume1 + m_convexHulls[i]->ComputeVolume(), combinedCH.ComputeVolume(), m_volumeCH0);
+ }
+
+ rowIdx += p2;
+ for (size_t i = p2 + 1; (i < costSize) && (!m_cancel); ++i) {
+ ComputeConvexHull(m_convexHulls[p2], m_convexHulls[i], pts, &combinedCH);
+ costMatrix[rowIdx] = ComputeConcavity(volume1 + m_convexHulls[i]->ComputeVolume(), combinedCH.ComputeVolume(), m_volumeCH0);
+ rowIdx += i;
+ assert(rowIdx >= 0);
+ }
+
+ // Move the top column in to replace its space
+ const size_t erase_idx = ((costSize - 1) * costSize) >> 1;
+ if (p1 < costSize) {
+ rowIdx = (addrI * p1) >> 1;
+ size_t top_row = erase_idx;
+ for (size_t i = 0; i < p1; ++i) {
+ if (i != p2) {
+ costMatrix[rowIdx] = costMatrix[top_row];
+ }
+ ++rowIdx;
+ ++top_row;
+ }
+
+ ++top_row;
+ rowIdx += p1;
+ for (size_t i = p1 + 1; i < (costSize + 1); ++i) {
+ costMatrix[rowIdx] = costMatrix[top_row++];
+ rowIdx += i;
+ assert(rowIdx >= 0);
+ }
+ }
+ costMatrix.Resize(erase_idx);
+ }
+ }
+ m_overallProgress = 99.0;
+ Update(100.0, 100.0, params);
+ m_timer.Toc();
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t time " << m_timer.GetElapsedTime() / 1000.0 << "s" << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+}
+void SimplifyConvexHull(Mesh* const ch, const size_t nvertices, const double minVolume)
+{
+ if (nvertices <= 4) {
+ return;
+ }
+ ICHull icHull;
+ icHull.AddPoints(ch->GetPointsBuffer(), ch->GetNPoints());
+ icHull.Process((unsigned int)nvertices, minVolume);
+ TMMesh& mesh = icHull.GetMesh();
+ const size_t nT = mesh.GetNTriangles();
+ const size_t nV = mesh.GetNVertices();
+ ch->ResizePoints(nV);
+ ch->ResizeTriangles(nT);
+ mesh.GetIFS(ch->GetPointsBuffer(), ch->GetTrianglesBuffer());
+}
+void VHACD::SimplifyConvexHulls(const Parameters& params)
+{
+ if (m_cancel || params.m_maxNumVerticesPerCH < 4) {
+ return;
+ }
+ m_timer.Tic();
+
+ m_stage = "Simplify convex-hulls";
+ m_operation = "Simplify convex-hulls";
+
+ std::ostringstream msg;
+ const size_t nConvexHulls = m_convexHulls.Size();
+ if (params.m_logger) {
+ msg << "+ Simplify " << nConvexHulls << " convex-hulls " << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+
+ Update(0.0, 0.0, params);
+ for (size_t i = 0; i < nConvexHulls && !m_cancel; ++i) {
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t\t Simplify CH[" << std::setfill('0') << std::setw(5) << i << "] " << m_convexHulls[i]->GetNPoints() << " V, " << m_convexHulls[i]->GetNTriangles() << " T" << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+ SimplifyConvexHull(m_convexHulls[i], params.m_maxNumVerticesPerCH, m_volumeCH0 * params.m_minVolumePerCH);
+ }
+
+ m_overallProgress = 100.0;
+ Update(100.0, 100.0, params);
+ m_timer.Toc();
+ if (params.m_logger) {
+ msg.str("");
+ msg << "\t time " << m_timer.GetElapsedTime() / 1000.0 << "s" << std::endl;
+ params.m_logger->Log(msg.str().c_str());
+ }
+}
+}
diff --git a/Extras/VHACD/src/btAlignedAllocator.cpp b/Extras/VHACD/src/btAlignedAllocator.cpp
new file mode 100644
index 0000000..e69442f
--- /dev/null
+++ b/Extras/VHACD/src/btAlignedAllocator.cpp
@@ -0,0 +1,176 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btAlignedAllocator.h"
+
+int gNumAlignedAllocs = 0;
+int gNumAlignedFree = 0;
+int gTotalBytesAlignedAllocs = 0; //detect memory leaks
+
+static void* btAllocDefault(size_t size)
+{
+ return malloc(size);
+}
+
+static void btFreeDefault(void* ptr)
+{
+ free(ptr);
+}
+
+static btAllocFunc* sAllocFunc = btAllocDefault;
+static btFreeFunc* sFreeFunc = btFreeDefault;
+
+#if defined(BT_HAS_ALIGNED_ALLOCATOR)
+#include <malloc.h>
+static void* btAlignedAllocDefault(size_t size, int alignment)
+{
+ return _aligned_malloc(size, (size_t)alignment);
+}
+
+static void btAlignedFreeDefault(void* ptr)
+{
+ _aligned_free(ptr);
+}
+#elif defined(__CELLOS_LV2__)
+#include <stdlib.h>
+
+static inline void* btAlignedAllocDefault(size_t size, int alignment)
+{
+ return memalign(alignment, size);
+}
+
+static inline void btAlignedFreeDefault(void* ptr)
+{
+ free(ptr);
+}
+#else
+static inline void* btAlignedAllocDefault(size_t size, int alignment)
+{
+ void* ret;
+ char* real;
+ unsigned long offset;
+
+ real = (char*)sAllocFunc(size + sizeof(void*) + (alignment - 1));
+ if (real) {
+ offset = (alignment - (unsigned long)(real + sizeof(void*))) & (alignment - 1);
+ ret = (void*)((real + sizeof(void*)) + offset);
+ *((void**)(ret)-1) = (void*)(real);
+ }
+ else {
+ ret = (void*)(real);
+ }
+ return (ret);
+}
+
+static inline void btAlignedFreeDefault(void* ptr)
+{
+ void* real;
+
+ if (ptr) {
+ real = *((void**)(ptr)-1);
+ sFreeFunc(real);
+ }
+}
+#endif
+
+static btAlignedAllocFunc* sAlignedAllocFunc = btAlignedAllocDefault;
+static btAlignedFreeFunc* sAlignedFreeFunc = btAlignedFreeDefault;
+
+void btAlignedAllocSetCustomAligned(btAlignedAllocFunc* allocFunc, btAlignedFreeFunc* freeFunc)
+{
+ sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault;
+ sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault;
+}
+
+void btAlignedAllocSetCustom(btAllocFunc* allocFunc, btFreeFunc* freeFunc)
+{
+ sAllocFunc = allocFunc ? allocFunc : btAllocDefault;
+ sFreeFunc = freeFunc ? freeFunc : btFreeDefault;
+}
+
+#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
+//this generic allocator provides the total allocated number of bytes
+#include <stdio.h>
+
+void* btAlignedAllocInternal(size_t size, int alignment, int line, char* filename)
+{
+ void* ret;
+ char* real;
+ unsigned long offset;
+
+ gTotalBytesAlignedAllocs += size;
+ gNumAlignedAllocs++;
+
+ real = (char*)sAllocFunc(size + 2 * sizeof(void*) + (alignment - 1));
+ if (real) {
+ offset = (alignment - (unsigned long)(real + 2 * sizeof(void*))) & (alignment - 1);
+ ret = (void*)((real + 2 * sizeof(void*)) + offset);
+ *((void**)(ret)-1) = (void*)(real);
+ *((int*)(ret)-2) = size;
+ }
+ else {
+ ret = (void*)(real); //??
+ }
+
+ printf("allocation#%d at address %x, from %s,line %d, size %d\n", gNumAlignedAllocs, real, filename, line, size);
+
+ int* ptr = (int*)ret;
+ *ptr = 12;
+ return (ret);
+}
+
+void btAlignedFreeInternal(void* ptr, int line, char* filename)
+{
+
+ void* real;
+ gNumAlignedFree++;
+
+ if (ptr) {
+ real = *((void**)(ptr)-1);
+ int size = *((int*)(ptr)-2);
+ gTotalBytesAlignedAllocs -= size;
+
+ printf("free #%d at address %x, from %s,line %d, size %d\n", gNumAlignedFree, real, filename, line, size);
+
+ sFreeFunc(real);
+ }
+ else {
+ printf("NULL ptr\n");
+ }
+}
+
+#else //BT_DEBUG_MEMORY_ALLOCATIONS
+
+void* btAlignedAllocInternal(size_t size, int alignment)
+{
+ gNumAlignedAllocs++;
+ void* ptr;
+ ptr = sAlignedAllocFunc(size, alignment);
+ // printf("btAlignedAllocInternal %d, %x\n",size,ptr);
+ return ptr;
+}
+
+void btAlignedFreeInternal(void* ptr)
+{
+ if (!ptr) {
+ return;
+ }
+
+ gNumAlignedFree++;
+ // printf("btAlignedFreeInternal %x\n",ptr);
+ sAlignedFreeFunc(ptr);
+}
+
+#endif //BT_DEBUG_MEMORY_ALLOCATIONS
diff --git a/Extras/VHACD/src/btConvexHullComputer.cpp b/Extras/VHACD/src/btConvexHullComputer.cpp
new file mode 100644
index 0000000..a84f7c0
--- /dev/null
+++ b/Extras/VHACD/src/btConvexHullComputer.cpp
@@ -0,0 +1,2475 @@
+/*
+Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include <string.h>
+
+#include "btAlignedObjectArray.h"
+#include "btConvexHullComputer.h"
+#include "btMinMax.h"
+#include "btVector3.h"
+
+#ifdef __GNUC__
+#include <stdint.h>
+#elif defined(_MSC_VER)
+typedef __int32 int32_t;
+typedef __int64 int64_t;
+typedef unsigned __int32 uint32_t;
+typedef unsigned __int64 uint64_t;
+#else
+typedef int int32_t;
+typedef long long int int64_t;
+typedef unsigned int uint32_t;
+typedef unsigned long long int uint64_t;
+#endif
+
+//The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines
+//#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL)) // || (defined(__ICL) && defined(_M_X64)) bug in Intel compiler, disable inline assembly
+// #define USE_X86_64_ASM
+//#endif
+
+//#define DEBUG_CONVEX_HULL
+//#define SHOW_ITERATIONS
+
+#if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS)
+#include <stdio.h>
+#endif
+
+// Convex hull implementation based on Preparata and Hong
+// Ole Kniemeyer, MAXON Computer GmbH
+class btConvexHullInternal {
+public:
+ class Point64 {
+ public:
+ int64_t x;
+ int64_t y;
+ int64_t z;
+
+ Point64(int64_t x, int64_t y, int64_t z)
+ : x(x)
+ , y(y)
+ , z(z)
+ {
+ }
+
+ bool isZero()
+ {
+ return (x == 0) && (y == 0) && (z == 0);
+ }
+
+ int64_t dot(const Point64& b) const
+ {
+ return x * b.x + y * b.y + z * b.z;
+ }
+ };
+
+ class Point32 {
+ public:
+ int32_t x;
+ int32_t y;
+ int32_t z;
+ int index;
+
+ Point32()
+ {
+ }
+
+ Point32(int32_t x, int32_t y, int32_t z)
+ : x(x)
+ , y(y)
+ , z(z)
+ , index(-1)
+ {
+ }
+
+ bool operator==(const Point32& b) const
+ {
+ return (x == b.x) && (y == b.y) && (z == b.z);
+ }
+
+ bool operator!=(const Point32& b) const
+ {
+ return (x != b.x) || (y != b.y) || (z != b.z);
+ }
+
+ bool isZero()
+ {
+ return (x == 0) && (y == 0) && (z == 0);
+ }
+
+ Point64 cross(const Point32& b) const
+ {
+ return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
+ }
+
+ Point64 cross(const Point64& b) const
+ {
+ return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);
+ }
+
+ int64_t dot(const Point32& b) const
+ {
+ return x * b.x + y * b.y + z * b.z;
+ }
+
+ int64_t dot(const Point64& b) const
+ {
+ return x * b.x + y * b.y + z * b.z;
+ }
+
+ Point32 operator+(const Point32& b) const
+ {
+ return Point32(x + b.x, y + b.y, z + b.z);
+ }
+
+ Point32 operator-(const Point32& b) const
+ {
+ return Point32(x - b.x, y - b.y, z - b.z);
+ }
+ };
+
+ class Int128 {
+ public:
+ uint64_t low;
+ uint64_t high;
+
+ Int128()
+ {
+ }
+
+ Int128(uint64_t low, uint64_t high)
+ : low(low)
+ , high(high)
+ {
+ }
+
+ Int128(uint64_t low)
+ : low(low)
+ , high(0)
+ {
+ }
+
+ Int128(int64_t value)
+ : low(value)
+ , high((value >= 0) ? 0 : (uint64_t)-1LL)
+ {
+ }
+
+ static Int128 mul(int64_t a, int64_t b);
+
+ static Int128 mul(uint64_t a, uint64_t b);
+
+ Int128 operator-() const
+ {
+ return Int128((uint64_t) - (int64_t)low, ~high + (low == 0));
+ }
+
+ Int128 operator+(const Int128& b) const
+ {
+#ifdef USE_X86_64_ASM
+ Int128 result;
+ __asm__("addq %[bl], %[rl]\n\t"
+ "adcq %[bh], %[rh]\n\t"
+ : [rl] "=r"(result.low), [rh] "=r"(result.high)
+ : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
+ : "cc");
+ return result;
+#else
+ uint64_t lo = low + b.low;
+ return Int128(lo, high + b.high + (lo < low));
+#endif
+ }
+
+ Int128 operator-(const Int128& b) const
+ {
+#ifdef USE_X86_64_ASM
+ Int128 result;
+ __asm__("subq %[bl], %[rl]\n\t"
+ "sbbq %[bh], %[rh]\n\t"
+ : [rl] "=r"(result.low), [rh] "=r"(result.high)
+ : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
+ : "cc");
+ return result;
+#else
+ return *this + -b;
+#endif
+ }
+
+ Int128& operator+=(const Int128& b)
+ {
+#ifdef USE_X86_64_ASM
+ __asm__("addq %[bl], %[rl]\n\t"
+ "adcq %[bh], %[rh]\n\t"
+ : [rl] "=r"(low), [rh] "=r"(high)
+ : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
+ : "cc");
+#else
+ uint64_t lo = low + b.low;
+ if (lo < low) {
+ ++high;
+ }
+ low = lo;
+ high += b.high;
+#endif
+ return *this;
+ }
+
+ Int128& operator++()
+ {
+ if (++low == 0) {
+ ++high;
+ }
+ return *this;
+ }
+
+ Int128 operator*(int64_t b) const;
+
+ btScalar toScalar() const
+ {
+ return ((int64_t)high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low)
+ : -(-*this).toScalar();
+ }
+
+ int getSign() const
+ {
+ return ((int64_t)high < 0) ? -1 : (high || low) ? 1 : 0;
+ }
+
+ bool operator<(const Int128& b) const
+ {
+ return (high < b.high) || ((high == b.high) && (low < b.low));
+ }
+
+ int ucmp(const Int128& b) const
+ {
+ if (high < b.high) {
+ return -1;
+ }
+ if (high > b.high) {
+ return 1;
+ }
+ if (low < b.low) {
+ return -1;
+ }
+ if (low > b.low) {
+ return 1;
+ }
+ return 0;
+ }
+ };
+
+ class Rational64 {
+ private:
+ uint64_t m_numerator;
+ uint64_t m_denominator;
+ int sign;
+
+ public:
+ Rational64(int64_t numerator, int64_t denominator)
+ {
+ if (numerator > 0) {
+ sign = 1;
+ m_numerator = (uint64_t)numerator;
+ }
+ else if (numerator < 0) {
+ sign = -1;
+ m_numerator = (uint64_t)-numerator;
+ }
+ else {
+ sign = 0;
+ m_numerator = 0;
+ }
+ if (denominator > 0) {
+ m_denominator = (uint64_t)denominator;
+ }
+ else if (denominator < 0) {
+ sign = -sign;
+ m_denominator = (uint64_t)-denominator;
+ }
+ else {
+ m_denominator = 0;
+ }
+ }
+
+ bool isNegativeInfinity() const
+ {
+ return (sign < 0) && (m_denominator == 0);
+ }
+
+ bool isNaN() const
+ {
+ return (sign == 0) && (m_denominator == 0);
+ }
+
+ int compare(const Rational64& b) const;
+
+ btScalar toScalar() const
+ {
+ return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar)m_numerator / m_denominator);
+ }
+ };
+
+ class Rational128 {
+ private:
+ Int128 numerator;
+ Int128 denominator;
+ int sign;
+ bool isInt64;
+
+ public:
+ Rational128(int64_t value)
+ {
+ if (value > 0) {
+ sign = 1;
+ this->numerator = value;
+ }
+ else if (value < 0) {
+ sign = -1;
+ this->numerator = -value;
+ }
+ else {
+ sign = 0;
+ this->numerator = (uint64_t)0;
+ }
+ this->denominator = (uint64_t)1;
+ isInt64 = true;
+ }
+
+ Rational128(const Int128& numerator, const Int128& denominator)
+ {
+ sign = numerator.getSign();
+ if (sign >= 0) {
+ this->numerator = numerator;
+ }
+ else {
+ this->numerator = -numerator;
+ }
+ int dsign = denominator.getSign();
+ if (dsign >= 0) {
+ this->denominator = denominator;
+ }
+ else {
+ sign = -sign;
+ this->denominator = -denominator;
+ }
+ isInt64 = false;
+ }
+
+ int compare(const Rational128& b) const;
+
+ int compare(int64_t b) const;
+
+ btScalar toScalar() const
+ {
+ return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar());
+ }
+ };
+
+ class PointR128 {
+ public:
+ Int128 x;
+ Int128 y;
+ Int128 z;
+ Int128 denominator;
+
+ PointR128()
+ {
+ }
+
+ PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator)
+ : x(x)
+ , y(y)
+ , z(z)
+ , denominator(denominator)
+ {
+ }
+
+ btScalar xvalue() const
+ {
+ return x.toScalar() / denominator.toScalar();
+ }
+
+ btScalar yvalue() const
+ {
+ return y.toScalar() / denominator.toScalar();
+ }
+
+ btScalar zvalue() const
+ {
+ return z.toScalar() / denominator.toScalar();
+ }
+ };
+
+ class Edge;
+ class Face;
+
+ class Vertex {
+ public:
+ Vertex* next;
+ Vertex* prev;
+ Edge* edges;
+ Face* firstNearbyFace;
+ Face* lastNearbyFace;
+ PointR128 point128;
+ Point32 point;
+ int copy;
+
+ Vertex()
+ : next(NULL)
+ , prev(NULL)
+ , edges(NULL)
+ , firstNearbyFace(NULL)
+ , lastNearbyFace(NULL)
+ , copy(-1)
+ {
+ }
+
+#ifdef DEBUG_CONVEX_HULL
+ void print()
+ {
+ printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z);
+ }
+
+ void printGraph();
+#endif
+
+ Point32 operator-(const Vertex& b) const
+ {
+ return point - b.point;
+ }
+
+ Rational128 dot(const Point64& b) const
+ {
+ return (point.index >= 0) ? Rational128(point.dot(b))
+ : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator);
+ }
+
+ btScalar xvalue() const
+ {
+ return (point.index >= 0) ? btScalar(point.x) : point128.xvalue();
+ }
+
+ btScalar yvalue() const
+ {
+ return (point.index >= 0) ? btScalar(point.y) : point128.yvalue();
+ }
+
+ btScalar zvalue() const
+ {
+ return (point.index >= 0) ? btScalar(point.z) : point128.zvalue();
+ }
+
+ void receiveNearbyFaces(Vertex* src)
+ {
+ if (lastNearbyFace) {
+ lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace;
+ }
+ else {
+ firstNearbyFace = src->firstNearbyFace;
+ }
+ if (src->lastNearbyFace) {
+ lastNearbyFace = src->lastNearbyFace;
+ }
+ for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex) {
+ btAssert(f->nearbyVertex == src);
+ f->nearbyVertex = this;
+ }
+ src->firstNearbyFace = NULL;
+ src->lastNearbyFace = NULL;
+ }
+ };
+
+ class Edge {
+ public:
+ Edge* next;
+ Edge* prev;
+ Edge* reverse;
+ Vertex* target;
+ Face* face;
+ int copy;
+
+ ~Edge()
+ {
+ next = NULL;
+ prev = NULL;
+ reverse = NULL;
+ target = NULL;
+ face = NULL;
+ }
+
+ void link(Edge* n)
+ {
+ btAssert(reverse->target == n->reverse->target);
+ next = n;
+ n->prev = this;
+ }
+
+#ifdef DEBUG_CONVEX_HULL
+ void print()
+ {
+ printf("E%p : %d -> %d, n=%p p=%p (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev,
+ reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z);
+ }
+#endif
+ };
+
+ class Face {
+ public:
+ Face* next;
+ Vertex* nearbyVertex;
+ Face* nextWithSameNearbyVertex;
+ Point32 origin;
+ Point32 dir0;
+ Point32 dir1;
+
+ Face()
+ : next(NULL)
+ , nearbyVertex(NULL)
+ , nextWithSameNearbyVertex(NULL)
+ {
+ }
+
+ void init(Vertex* a, Vertex* b, Vertex* c)
+ {
+ nearbyVertex = a;
+ origin = a->point;
+ dir0 = *b - *a;
+ dir1 = *c - *a;
+ if (a->lastNearbyFace) {
+ a->lastNearbyFace->nextWithSameNearbyVertex = this;
+ }
+ else {
+ a->firstNearbyFace = this;
+ }
+ a->lastNearbyFace = this;
+ }
+
+ Point64 getNormal()
+ {
+ return dir0.cross(dir1);
+ }
+ };
+
+ template <typename UWord, typename UHWord>
+ class DMul {
+ private:
+ static uint32_t high(uint64_t value)
+ {
+ return (uint32_t)(value >> 32);
+ }
+
+ static uint32_t low(uint64_t value)
+ {
+ return (uint32_t)value;
+ }
+
+ static uint64_t mul(uint32_t a, uint32_t b)
+ {
+ return (uint64_t)a * (uint64_t)b;
+ }
+
+ static void shlHalf(uint64_t& value)
+ {
+ value <<= 32;
+ }
+
+ static uint64_t high(Int128 value)
+ {
+ return value.high;
+ }
+
+ static uint64_t low(Int128 value)
+ {
+ return value.low;
+ }
+
+ static Int128 mul(uint64_t a, uint64_t b)
+ {
+ return Int128::mul(a, b);
+ }
+
+ static void shlHalf(Int128& value)
+ {
+ value.high = value.low;
+ value.low = 0;
+ }
+
+ public:
+ static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
+ {
+ UWord p00 = mul(low(a), low(b));
+ UWord p01 = mul(low(a), high(b));
+ UWord p10 = mul(high(a), low(b));
+ UWord p11 = mul(high(a), high(b));
+ UWord p0110 = UWord(low(p01)) + UWord(low(p10));
+ p11 += high(p01);
+ p11 += high(p10);
+ p11 += high(p0110);
+ shlHalf(p0110);
+ p00 += p0110;
+ if (p00 < p0110) {
+ ++p11;
+ }
+ resLow = p00;
+ resHigh = p11;
+ }
+ };
+
+private:
+ class IntermediateHull {
+ public:
+ Vertex* minXy;
+ Vertex* maxXy;
+ Vertex* minYx;
+ Vertex* maxYx;
+
+ IntermediateHull()
+ : minXy(NULL)
+ , maxXy(NULL)
+ , minYx(NULL)
+ , maxYx(NULL)
+ {
+ }
+
+ void print();
+ };
+
+ enum Orientation { NONE,
+ CLOCKWISE,
+ COUNTER_CLOCKWISE };
+
+ template <typename T>
+ class PoolArray {
+ private:
+ T* array;
+ int size;
+
+ public:
+ PoolArray<T>* next;
+
+ PoolArray(int size)
+ : size(size)
+ , next(NULL)
+ {
+ array = (T*)btAlignedAlloc(sizeof(T) * size, 16);
+ }
+
+ ~PoolArray()
+ {
+ btAlignedFree(array);
+ }
+
+ T* init()
+ {
+ T* o = array;
+ for (int i = 0; i < size; i++, o++) {
+ o->next = (i + 1 < size) ? o + 1 : NULL;
+ }
+ return array;
+ }
+ };
+
+ template <typename T>
+ class Pool {
+ private:
+ PoolArray<T>* arrays;
+ PoolArray<T>* nextArray;
+ T* freeObjects;
+ int arraySize;
+
+ public:
+ Pool()
+ : arrays(NULL)
+ , nextArray(NULL)
+ , freeObjects(NULL)
+ , arraySize(256)
+ {
+ }
+
+ ~Pool()
+ {
+ while (arrays) {
+ PoolArray<T>* p = arrays;
+ arrays = p->next;
+ p->~PoolArray<T>();
+ btAlignedFree(p);
+ }
+ }
+
+ void reset()
+ {
+ nextArray = arrays;
+ freeObjects = NULL;
+ }
+
+ void setArraySize(int arraySize)
+ {
+ this->arraySize = arraySize;
+ }
+
+ T* newObject()
+ {
+ T* o = freeObjects;
+ if (!o) {
+ PoolArray<T>* p = nextArray;
+ if (p) {
+ nextArray = p->next;
+ }
+ else {
+ p = new (btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
+ p->next = arrays;
+ arrays = p;
+ }
+ o = p->init();
+ }
+ freeObjects = o->next;
+ return new (o) T();
+ };
+
+ void freeObject(T* object)
+ {
+ object->~T();
+ object->next = freeObjects;
+ freeObjects = object;
+ }
+ };
+
+ btVector3 scaling;
+ btVector3 center;
+ Pool<Vertex> vertexPool;
+ Pool<Edge> edgePool;
+ Pool<Face> facePool;
+ btAlignedObjectArray<Vertex*> originalVertices;
+ int mergeStamp;
+ int minAxis;
+ int medAxis;
+ int maxAxis;
+ int usedEdgePairs;
+ int maxUsedEdgePairs;
+
+ static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t);
+ Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot);
+ void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1);
+
+ Edge* newEdgePair(Vertex* from, Vertex* to);
+
+ void removeEdgePair(Edge* edge)
+ {
+ Edge* n = edge->next;
+ Edge* r = edge->reverse;
+
+ btAssert(edge->target && r->target);
+
+ if (n != edge) {
+ n->prev = edge->prev;
+ edge->prev->next = n;
+ r->target->edges = n;
+ }
+ else {
+ r->target->edges = NULL;
+ }
+
+ n = r->next;
+
+ if (n != r) {
+ n->prev = r->prev;
+ r->prev->next = n;
+ edge->target->edges = n;
+ }
+ else {
+ edge->target->edges = NULL;
+ }
+
+ edgePool.freeObject(edge);
+ edgePool.freeObject(r);
+ usedEdgePairs--;
+ }
+
+ void computeInternal(int start, int end, IntermediateHull& result);
+
+ bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1);
+
+ void merge(IntermediateHull& h0, IntermediateHull& h1);
+
+ btVector3 toBtVector(const Point32& v);
+
+ btVector3 getBtNormal(Face* face);
+
+ bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack);
+
+public:
+ Vertex* vertexList;
+
+ void compute(const void* coords, bool doubleCoords, int stride, int count);
+
+ btVector3 getCoordinates(const Vertex* v);
+
+ btScalar shrink(btScalar amount, btScalar clampAmount);
+};
+
+btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const
+{
+ bool negative = (int64_t)high < 0;
+ Int128 a = negative ? -*this : *this;
+ if (b < 0) {
+ negative = !negative;
+ b = -b;
+ }
+ Int128 result = mul(a.low, (uint64_t)b);
+ result.high += a.high * (uint64_t)b;
+ return negative ? -result : result;
+}
+
+btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b)
+{
+ Int128 result;
+
+#ifdef USE_X86_64_ASM
+ __asm__("imulq %[b]"
+ : "=a"(result.low), "=d"(result.high)
+ : "0"(a), [b] "r"(b)
+ : "cc");
+ return result;
+
+#else
+ bool negative = a < 0;
+ if (negative) {
+ a = -a;
+ }
+ if (b < 0) {
+ negative = !negative;
+ b = -b;
+ }
+ DMul<uint64_t, uint32_t>::mul((uint64_t)a, (uint64_t)b, result.low, result.high);
+ return negative ? -result : result;
+#endif
+}
+
+btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b)
+{
+ Int128 result;
+
+#ifdef USE_X86_64_ASM
+ __asm__("mulq %[b]"
+ : "=a"(result.low), "=d"(result.high)
+ : "0"(a), [b] "r"(b)
+ : "cc");
+
+#else
+ DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high);
+#endif
+
+ return result;
+}
+
+int btConvexHullInternal::Rational64::compare(const Rational64& b) const
+{
+ if (sign != b.sign) {
+ return sign - b.sign;
+ }
+ else if (sign == 0) {
+ return 0;
+ }
+
+// return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0;
+
+#ifdef USE_X86_64_ASM
+
+ int result;
+ int64_t tmp;
+ int64_t dummy;
+ __asm__("mulq %[bn]\n\t"
+ "movq %%rax, %[tmp]\n\t"
+ "movq %%rdx, %%rbx\n\t"
+ "movq %[tn], %%rax\n\t"
+ "mulq %[bd]\n\t"
+ "subq %[tmp], %%rax\n\t"
+ "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator"
+ "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise
+ "orq %%rdx, %%rax\n\t"
+ "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero
+ "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference)
+ "shll $16, %%ebx\n\t" // ebx has same sign as difference
+ : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
+ : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
+ : "%rdx", "cc");
+ return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
+ // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)
+ : 0;
+
+#else
+
+ return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator));
+
+#endif
+}
+
+int btConvexHullInternal::Rational128::compare(const Rational128& b) const
+{
+ if (sign != b.sign) {
+ return sign - b.sign;
+ }
+ else if (sign == 0) {
+ return 0;
+ }
+ if (isInt64) {
+ return -b.compare(sign * (int64_t)numerator.low);
+ }
+
+ Int128 nbdLow, nbdHigh, dbnLow, dbnHigh;
+ DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh);
+ DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh);
+
+ int cmp = nbdHigh.ucmp(dbnHigh);
+ if (cmp) {
+ return cmp * sign;
+ }
+ return nbdLow.ucmp(dbnLow) * sign;
+}
+
+int btConvexHullInternal::Rational128::compare(int64_t b) const
+{
+ if (isInt64) {
+ int64_t a = sign * (int64_t)numerator.low;
+ return (a > b) ? 1 : (a < b) ? -1 : 0;
+ }
+ if (b > 0) {
+ if (sign <= 0) {
+ return -1;
+ }
+ }
+ else if (b < 0) {
+ if (sign >= 0) {
+ return 1;
+ }
+ b = -b;
+ }
+ else {
+ return sign;
+ }
+
+ return numerator.ucmp(denominator * b) * sign;
+}
+
+btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
+{
+ btAssert(from && to);
+ Edge* e = edgePool.newObject();
+ Edge* r = edgePool.newObject();
+ e->reverse = r;
+ r->reverse = e;
+ e->copy = mergeStamp;
+ r->copy = mergeStamp;
+ e->target = to;
+ r->target = from;
+ e->face = NULL;
+ r->face = NULL;
+ usedEdgePairs++;
+ if (usedEdgePairs > maxUsedEdgePairs) {
+ maxUsedEdgePairs = usedEdgePairs;
+ }
+ return e;
+}
+
+bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1)
+{
+ Vertex* v0 = h0.maxYx;
+ Vertex* v1 = h1.minYx;
+ if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y)) {
+ btAssert(v0->point.z < v1->point.z);
+ Vertex* v1p = v1->prev;
+ if (v1p == v1) {
+ c0 = v0;
+ if (v1->edges) {
+ btAssert(v1->edges->next == v1->edges);
+ v1 = v1->edges->target;
+ btAssert(v1->edges->next == v1->edges);
+ }
+ c1 = v1;
+ return false;
+ }
+ Vertex* v1n = v1->next;
+ v1p->next = v1n;
+ v1n->prev = v1p;
+ if (v1 == h1.minXy) {
+ if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y))) {
+ h1.minXy = v1n;
+ }
+ else {
+ h1.minXy = v1p;
+ }
+ }
+ if (v1 == h1.maxXy) {
+ if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y))) {
+ h1.maxXy = v1n;
+ }
+ else {
+ h1.maxXy = v1p;
+ }
+ }
+ }
+
+ v0 = h0.maxXy;
+ v1 = h1.maxXy;
+ Vertex* v00 = NULL;
+ Vertex* v10 = NULL;
+ int32_t sign = 1;
+
+ for (int side = 0; side <= 1; side++) {
+ int32_t dx = (v1->point.x - v0->point.x) * sign;
+ if (dx > 0) {
+ while (true) {
+ int32_t dy = v1->point.y - v0->point.y;
+
+ Vertex* w0 = side ? v0->next : v0->prev;
+ if (w0 != v0) {
+ int32_t dx0 = (w0->point.x - v0->point.x) * sign;
+ int32_t dy0 = w0->point.y - v0->point.y;
+ if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0)))) {
+ v0 = w0;
+ dx = (v1->point.x - v0->point.x) * sign;
+ continue;
+ }
+ }
+
+ Vertex* w1 = side ? v1->next : v1->prev;
+ if (w1 != v1) {
+ int32_t dx1 = (w1->point.x - v1->point.x) * sign;
+ int32_t dy1 = w1->point.y - v1->point.y;
+ int32_t dxn = (w1->point.x - v0->point.x) * sign;
+ if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1)))) {
+ v1 = w1;
+ dx = dxn;
+ continue;
+ }
+ }
+
+ break;
+ }
+ }
+ else if (dx < 0) {
+ while (true) {
+ int32_t dy = v1->point.y - v0->point.y;
+
+ Vertex* w1 = side ? v1->prev : v1->next;
+ if (w1 != v1) {
+ int32_t dx1 = (w1->point.x - v1->point.x) * sign;
+ int32_t dy1 = w1->point.y - v1->point.y;
+ if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) {
+ v1 = w1;
+ dx = (v1->point.x - v0->point.x) * sign;
+ continue;
+ }
+ }
+
+ Vertex* w0 = side ? v0->prev : v0->next;
+ if (w0 != v0) {
+ int32_t dx0 = (w0->point.x - v0->point.x) * sign;
+ int32_t dy0 = w0->point.y - v0->point.y;
+ int32_t dxn = (v1->point.x - w0->point.x) * sign;
+ if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) {
+ v0 = w0;
+ dx = dxn;
+ continue;
+ }
+ }
+
+ break;
+ }
+ }
+ else {
+ int32_t x = v0->point.x;
+ int32_t y0 = v0->point.y;
+ Vertex* w0 = v0;
+ Vertex* t;
+ while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0)) {
+ w0 = t;
+ y0 = t->point.y;
+ }
+ v0 = w0;
+
+ int32_t y1 = v1->point.y;
+ Vertex* w1 = v1;
+ while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) {
+ w1 = t;
+ y1 = t->point.y;
+ }
+ v1 = w1;
+ }
+
+ if (side == 0) {
+ v00 = v0;
+ v10 = v1;
+
+ v0 = h0.minXy;
+ v1 = h1.minXy;
+ sign = -1;
+ }
+ }
+
+ v0->prev = v1;
+ v1->next = v0;
+
+ v00->next = v10;
+ v10->prev = v00;
+
+ if (h1.minXy->point.x < h0.minXy->point.x) {
+ h0.minXy = h1.minXy;
+ }
+ if (h1.maxXy->point.x >= h0.maxXy->point.x) {
+ h0.maxXy = h1.maxXy;
+ }
+
+ h0.maxYx = h1.maxYx;
+
+ c0 = v00;
+ c1 = v10;
+
+ return true;
+}
+
+void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result)
+{
+ int n = end - start;
+ switch (n) {
+ case 0:
+ result.minXy = NULL;
+ result.maxXy = NULL;
+ result.minYx = NULL;
+ result.maxYx = NULL;
+ return;
+ case 2: {
+ Vertex* v = originalVertices[start];
+ Vertex* w = v + 1;
+ if (v->point != w->point) {
+ int32_t dx = v->point.x - w->point.x;
+ int32_t dy = v->point.y - w->point.y;
+
+ if ((dx == 0) && (dy == 0)) {
+ if (v->point.z > w->point.z) {
+ Vertex* t = w;
+ w = v;
+ v = t;
+ }
+ btAssert(v->point.z < w->point.z);
+ v->next = v;
+ v->prev = v;
+ result.minXy = v;
+ result.maxXy = v;
+ result.minYx = v;
+ result.maxYx = v;
+ }
+ else {
+ v->next = w;
+ v->prev = w;
+ w->next = v;
+ w->prev = v;
+
+ if ((dx < 0) || ((dx == 0) && (dy < 0))) {
+ result.minXy = v;
+ result.maxXy = w;
+ }
+ else {
+ result.minXy = w;
+ result.maxXy = v;
+ }
+
+ if ((dy < 0) || ((dy == 0) && (dx < 0))) {
+ result.minYx = v;
+ result.maxYx = w;
+ }
+ else {
+ result.minYx = w;
+ result.maxYx = v;
+ }
+ }
+
+ Edge* e = newEdgePair(v, w);
+ e->link(e);
+ v->edges = e;
+
+ e = e->reverse;
+ e->link(e);
+ w->edges = e;
+
+ return;
+ }
+ }
+ // lint -fallthrough
+ case 1: {
+ Vertex* v = originalVertices[start];
+ v->edges = NULL;
+ v->next = v;
+ v->prev = v;
+
+ result.minXy = v;
+ result.maxXy = v;
+ result.minYx = v;
+ result.maxYx = v;
+
+ return;
+ }
+ }
+
+ int split0 = start + n / 2;
+ Point32 p = originalVertices[split0 - 1]->point;
+ int split1 = split0;
+ while ((split1 < end) && (originalVertices[split1]->point == p)) {
+ split1++;
+ }
+ computeInternal(start, split0, result);
+ IntermediateHull hull1;
+ computeInternal(split1, end, hull1);
+#ifdef DEBUG_CONVEX_HULL
+ printf("\n\nMerge\n");
+ result.print();
+ hull1.print();
+#endif
+ merge(result, hull1);
+#ifdef DEBUG_CONVEX_HULL
+ printf("\n Result\n");
+ result.print();
+#endif
+}
+
+#ifdef DEBUG_CONVEX_HULL
+void btConvexHullInternal::IntermediateHull::print()
+{
+ printf(" Hull\n");
+ for (Vertex* v = minXy; v;) {
+ printf(" ");
+ v->print();
+ if (v == maxXy) {
+ printf(" maxXy");
+ }
+ if (v == minYx) {
+ printf(" minYx");
+ }
+ if (v == maxYx) {
+ printf(" maxYx");
+ }
+ if (v->next->prev != v) {
+ printf(" Inconsistency");
+ }
+ printf("\n");
+ v = v->next;
+ if (v == minXy) {
+ break;
+ }
+ }
+ if (minXy) {
+ minXy->copy = (minXy->copy == -1) ? -2 : -1;
+ minXy->printGraph();
+ }
+}
+
+void btConvexHullInternal::Vertex::printGraph()
+{
+ print();
+ printf("\nEdges\n");
+ Edge* e = edges;
+ if (e) {
+ do {
+ e->print();
+ printf("\n");
+ e = e->next;
+ } while (e != edges);
+ do {
+ Vertex* v = e->target;
+ if (v->copy != copy) {
+ v->copy = copy;
+ v->printGraph();
+ }
+ e = e->next;
+ } while (e != edges);
+ }
+}
+#endif
+
+btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t)
+{
+ btAssert(prev->reverse->target == next->reverse->target);
+ if (prev->next == next) {
+ if (prev->prev == next) {
+ Point64 n = t.cross(s);
+ Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
+ btAssert(!m.isZero());
+ int64_t dot = n.dot(m);
+ btAssert(dot != 0);
+ return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
+ }
+ return COUNTER_CLOCKWISE;
+ }
+ else if (prev->prev == next) {
+ return CLOCKWISE;
+ }
+ else {
+ return NONE;
+ }
+}
+
+btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
+{
+ Edge* minEdge = NULL;
+
+#ifdef DEBUG_CONVEX_HULL
+ printf("find max edge for %d\n", start->point.index);
+#endif
+ Edge* e = start->edges;
+ if (e) {
+ do {
+ if (e->copy > mergeStamp) {
+ Point32 t = *e->target - *start;
+ Rational64 cot(t.dot(sxrxs), t.dot(rxs));
+#ifdef DEBUG_CONVEX_HULL
+ printf(" Angle is %f (%d) for ", (float)btAtan(cot.toScalar()), (int)cot.isNaN());
+ e->print();
+#endif
+ if (cot.isNaN()) {
+ btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
+ }
+ else {
+ int cmp;
+ if (minEdge == NULL) {
+ minCot = cot;
+ minEdge = e;
+ }
+ else if ((cmp = cot.compare(minCot)) < 0) {
+ minCot = cot;
+ minEdge = e;
+ }
+ else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE))) {
+ minEdge = e;
+ }
+ }
+#ifdef DEBUG_CONVEX_HULL
+ printf("\n");
+#endif
+ }
+ e = e->next;
+ } while (e != start->edges);
+ }
+ return minEdge;
+}
+
+void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1)
+{
+ Edge* start0 = e0;
+ Edge* start1 = e1;
+ Point32 et0 = start0 ? start0->target->point : c0->point;
+ Point32 et1 = start1 ? start1->target->point : c1->point;
+ Point32 s = c1->point - c0->point;
+ Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s);
+ int64_t dist = c0->point.dot(normal);
+ btAssert(!start1 || (start1->target->point.dot(normal) == dist));
+ Point64 perp = s.cross(normal);
+ btAssert(!perp.isZero());
+
+#ifdef DEBUG_CONVEX_HULL
+ printf(" Advancing %d %d (%p %p, %d %d)\n", c0->point.index, c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1);
+#endif
+
+ int64_t maxDot0 = et0.dot(perp);
+ if (e0) {
+ while (e0->target != stop0) {
+ Edge* e = e0->reverse->prev;
+ if (e->target->point.dot(normal) < dist) {
+ break;
+ }
+ btAssert(e->target->point.dot(normal) == dist);
+ if (e->copy == mergeStamp) {
+ break;
+ }
+ int64_t dot = e->target->point.dot(perp);
+ if (dot <= maxDot0) {
+ break;
+ }
+ maxDot0 = dot;
+ e0 = e;
+ et0 = e->target->point;
+ }
+ }
+
+ int64_t maxDot1 = et1.dot(perp);
+ if (e1) {
+ while (e1->target != stop1) {
+ Edge* e = e1->reverse->next;
+ if (e->target->point.dot(normal) < dist) {
+ break;
+ }
+ btAssert(e->target->point.dot(normal) == dist);
+ if (e->copy == mergeStamp) {
+ break;
+ }
+ int64_t dot = e->target->point.dot(perp);
+ if (dot <= maxDot1) {
+ break;
+ }
+ maxDot1 = dot;
+ e1 = e;
+ et1 = e->target->point;
+ }
+ }
+
+#ifdef DEBUG_CONVEX_HULL
+ printf(" Starting at %d %d\n", et0.index, et1.index);
+#endif
+
+ int64_t dx = maxDot1 - maxDot0;
+ if (dx > 0) {
+ while (true) {
+ int64_t dy = (et1 - et0).dot(s);
+
+ if (e0 && (e0->target != stop0)) {
+ Edge* f0 = e0->next->reverse;
+ if (f0->copy > mergeStamp) {
+ int64_t dx0 = (f0->target->point - et0).dot(perp);
+ int64_t dy0 = (f0->target->point - et0).dot(s);
+ if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) {
+ et0 = f0->target->point;
+ dx = (et1 - et0).dot(perp);
+ e0 = (e0 == start0) ? NULL : f0;
+ continue;
+ }
+ }
+ }
+
+ if (e1 && (e1->target != stop1)) {
+ Edge* f1 = e1->reverse->next;
+ if (f1->copy > mergeStamp) {
+ Point32 d1 = f1->target->point - et1;
+ if (d1.dot(normal) == 0) {
+ int64_t dx1 = d1.dot(perp);
+ int64_t dy1 = d1.dot(s);
+ int64_t dxn = (f1->target->point - et0).dot(perp);
+ if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0)))) {
+ e1 = f1;
+ et1 = e1->target->point;
+ dx = dxn;
+ continue;
+ }
+ }
+ else {
+ btAssert((e1 == start1) && (d1.dot(normal) < 0));
+ }
+ }
+ }
+
+ break;
+ }
+ }
+ else if (dx < 0) {
+ while (true) {
+ int64_t dy = (et1 - et0).dot(s);
+
+ if (e1 && (e1->target != stop1)) {
+ Edge* f1 = e1->prev->reverse;
+ if (f1->copy > mergeStamp) {
+ int64_t dx1 = (f1->target->point - et1).dot(perp);
+ int64_t dy1 = (f1->target->point - et1).dot(s);
+ if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) {
+ et1 = f1->target->point;
+ dx = (et1 - et0).dot(perp);
+ e1 = (e1 == start1) ? NULL : f1;
+ continue;
+ }
+ }
+ }
+
+ if (e0 && (e0->target != stop0)) {
+ Edge* f0 = e0->reverse->prev;
+ if (f0->copy > mergeStamp) {
+ Point32 d0 = f0->target->point - et0;
+ if (d0.dot(normal) == 0) {
+ int64_t dx0 = d0.dot(perp);
+ int64_t dy0 = d0.dot(s);
+ int64_t dxn = (et1 - f0->target->point).dot(perp);
+ if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0)))) {
+ e0 = f0;
+ et0 = e0->target->point;
+ dx = dxn;
+ continue;
+ }
+ }
+ else {
+ btAssert((e0 == start0) && (d0.dot(normal) < 0));
+ }
+ }
+ }
+
+ break;
+ }
+ }
+#ifdef DEBUG_CONVEX_HULL
+ printf(" Advanced edges to %d %d\n", et0.index, et1.index);
+#endif
+}
+
+void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
+{
+ if (!h1.maxXy) {
+ return;
+ }
+ if (!h0.maxXy) {
+ h0 = h1;
+ return;
+ }
+
+ mergeStamp--;
+
+ Vertex* c0 = NULL;
+ Edge* toPrev0 = NULL;
+ Edge* firstNew0 = NULL;
+ Edge* pendingHead0 = NULL;
+ Edge* pendingTail0 = NULL;
+ Vertex* c1 = NULL;
+ Edge* toPrev1 = NULL;
+ Edge* firstNew1 = NULL;
+ Edge* pendingHead1 = NULL;
+ Edge* pendingTail1 = NULL;
+ Point32 prevPoint;
+
+ if (mergeProjection(h0, h1, c0, c1)) {
+ Point32 s = *c1 - *c0;
+ Point64 normal = Point32(0, 0, -1).cross(s);
+ Point64 t = s.cross(normal);
+ btAssert(!t.isZero());
+
+ Edge* e = c0->edges;
+ Edge* start0 = NULL;
+ if (e) {
+ do {
+ int64_t dot = (*e->target - *c0).dot(normal);
+ btAssert(dot <= 0);
+ if ((dot == 0) && ((*e->target - *c0).dot(t) > 0)) {
+ if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE)) {
+ start0 = e;
+ }
+ }
+ e = e->next;
+ } while (e != c0->edges);
+ }
+
+ e = c1->edges;
+ Edge* start1 = NULL;
+ if (e) {
+ do {
+ int64_t dot = (*e->target - *c1).dot(normal);
+ btAssert(dot <= 0);
+ if ((dot == 0) && ((*e->target - *c1).dot(t) > 0)) {
+ if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE)) {
+ start1 = e;
+ }
+ }
+ e = e->next;
+ } while (e != c1->edges);
+ }
+
+ if (start0 || start1) {
+ findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL);
+ if (start0) {
+ c0 = start0->target;
+ }
+ if (start1) {
+ c1 = start1->target;
+ }
+ }
+
+ prevPoint = c1->point;
+ prevPoint.z++;
+ }
+ else {
+ prevPoint = c1->point;
+ prevPoint.x++;
+ }
+
+ Vertex* first0 = c0;
+ Vertex* first1 = c1;
+ bool firstRun = true;
+
+ while (true) {
+ Point32 s = *c1 - *c0;
+ Point32 r = prevPoint - c0->point;
+ Point64 rxs = r.cross(s);
+ Point64 sxrxs = s.cross(rxs);
+
+#ifdef DEBUG_CONVEX_HULL
+ printf("\n Checking %d %d\n", c0->point.index, c1->point.index);
+#endif
+ Rational64 minCot0(0, 0);
+ Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0);
+ Rational64 minCot1(0, 0);
+ Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1);
+ if (!min0 && !min1) {
+ Edge* e = newEdgePair(c0, c1);
+ e->link(e);
+ c0->edges = e;
+
+ e = e->reverse;
+ e->link(e);
+ c1->edges = e;
+ return;
+ }
+ else {
+ int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1);
+#ifdef DEBUG_CONVEX_HULL
+ printf(" -> Result %d\n", cmp);
+#endif
+ if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity())) {
+ Edge* e = newEdgePair(c0, c1);
+ if (pendingTail0) {
+ pendingTail0->prev = e;
+ }
+ else {
+ pendingHead0 = e;
+ }
+ e->next = pendingTail0;
+ pendingTail0 = e;
+
+ e = e->reverse;
+ if (pendingTail1) {
+ pendingTail1->next = e;
+ }
+ else {
+ pendingHead1 = e;
+ }
+ e->prev = pendingTail1;
+ pendingTail1 = e;
+ }
+
+ Edge* e0 = min0;
+ Edge* e1 = min1;
+
+#ifdef DEBUG_CONVEX_HULL
+ printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1);
+#endif
+
+ if (cmp == 0) {
+ findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL);
+ }
+
+ if ((cmp >= 0) && e1) {
+ if (toPrev1) {
+ for (Edge *e = toPrev1->next, *n = NULL; e != min1; e = n) {
+ n = e->next;
+ removeEdgePair(e);
+ }
+ }
+
+ if (pendingTail1) {
+ if (toPrev1) {
+ toPrev1->link(pendingHead1);
+ }
+ else {
+ min1->prev->link(pendingHead1);
+ firstNew1 = pendingHead1;
+ }
+ pendingTail1->link(min1);
+ pendingHead1 = NULL;
+ pendingTail1 = NULL;
+ }
+ else if (!toPrev1) {
+ firstNew1 = min1;
+ }
+
+ prevPoint = c1->point;
+ c1 = e1->target;
+ toPrev1 = e1->reverse;
+ }
+
+ if ((cmp <= 0) && e0) {
+ if (toPrev0) {
+ for (Edge *e = toPrev0->prev, *n = NULL; e != min0; e = n) {
+ n = e->prev;
+ removeEdgePair(e);
+ }
+ }
+
+ if (pendingTail0) {
+ if (toPrev0) {
+ pendingHead0->link(toPrev0);
+ }
+ else {
+ pendingHead0->link(min0->next);
+ firstNew0 = pendingHead0;
+ }
+ min0->link(pendingTail0);
+ pendingHead0 = NULL;
+ pendingTail0 = NULL;
+ }
+ else if (!toPrev0) {
+ firstNew0 = min0;
+ }
+
+ prevPoint = c0->point;
+ c0 = e0->target;
+ toPrev0 = e0->reverse;
+ }
+ }
+
+ if ((c0 == first0) && (c1 == first1)) {
+ if (toPrev0 == NULL) {
+ pendingHead0->link(pendingTail0);
+ c0->edges = pendingTail0;
+ }
+ else {
+ for (Edge *e = toPrev0->prev, *n = NULL; e != firstNew0; e = n) {
+ n = e->prev;
+ removeEdgePair(e);
+ }
+ if (pendingTail0) {
+ pendingHead0->link(toPrev0);
+ firstNew0->link(pendingTail0);
+ }
+ }
+
+ if (toPrev1 == NULL) {
+ pendingTail1->link(pendingHead1);
+ c1->edges = pendingTail1;
+ }
+ else {
+ for (Edge *e = toPrev1->next, *n = NULL; e != firstNew1; e = n) {
+ n = e->next;
+ removeEdgePair(e);
+ }
+ if (pendingTail1) {
+ toPrev1->link(pendingHead1);
+ pendingTail1->link(firstNew1);
+ }
+ }
+
+ return;
+ }
+
+ firstRun = false;
+ }
+}
+
+static bool pointCmp(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q)
+{
+ return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
+}
+
+void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
+{
+ btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30));
+ const char* ptr = (const char*)coords;
+ if (doubleCoords) {
+ for (int i = 0; i < count; i++) {
+ const double* v = (const double*)ptr;
+ btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]);
+ ptr += stride;
+ min.setMin(p);
+ max.setMax(p);
+ }
+ }
+ else {
+ for (int i = 0; i < count; i++) {
+ const float* v = (const float*)ptr;
+ btVector3 p(v[0], v[1], v[2]);
+ ptr += stride;
+ min.setMin(p);
+ max.setMax(p);
+ }
+ }
+
+ btVector3 s = max - min;
+ maxAxis = s.maxAxis();
+ minAxis = s.minAxis();
+ if (minAxis == maxAxis) {
+ minAxis = (maxAxis + 1) % 3;
+ }
+ medAxis = 3 - maxAxis - minAxis;
+
+ s /= btScalar(10216);
+ if (((medAxis + 1) % 3) != maxAxis) {
+ s *= -1;
+ }
+ scaling = s;
+
+ if (s[0] != 0) {
+ s[0] = btScalar(1) / s[0];
+ }
+ if (s[1] != 0) {
+ s[1] = btScalar(1) / s[1];
+ }
+ if (s[2] != 0) {
+ s[2] = btScalar(1) / s[2];
+ }
+
+ center = (min + max) * btScalar(0.5);
+
+ btAlignedObjectArray<Point32> points;
+ points.resize(count);
+ ptr = (const char*)coords;
+ if (doubleCoords) {
+ for (int i = 0; i < count; i++) {
+ const double* v = (const double*)ptr;
+ btVector3 p((btScalar)v[0], (btScalar)v[1], (btScalar)v[2]);
+ ptr += stride;
+ p = (p - center) * s;
+ points[i].x = (int32_t)p[medAxis];
+ points[i].y = (int32_t)p[maxAxis];
+ points[i].z = (int32_t)p[minAxis];
+ points[i].index = i;
+ }
+ }
+ else {
+ for (int i = 0; i < count; i++) {
+ const float* v = (const float*)ptr;
+ btVector3 p(v[0], v[1], v[2]);
+ ptr += stride;
+ p = (p - center) * s;
+ points[i].x = (int32_t)p[medAxis];
+ points[i].y = (int32_t)p[maxAxis];
+ points[i].z = (int32_t)p[minAxis];
+ points[i].index = i;
+ }
+ }
+ points.quickSort(pointCmp);
+
+ vertexPool.reset();
+ vertexPool.setArraySize(count);
+ originalVertices.resize(count);
+ for (int i = 0; i < count; i++) {
+ Vertex* v = vertexPool.newObject();
+ v->edges = NULL;
+ v->point = points[i];
+ v->copy = -1;
+ originalVertices[i] = v;
+ }
+
+ points.clear();
+
+ edgePool.reset();
+ edgePool.setArraySize(6 * count);
+
+ usedEdgePairs = 0;
+ maxUsedEdgePairs = 0;
+
+ mergeStamp = -3;
+
+ IntermediateHull hull;
+ computeInternal(0, count, hull);
+ vertexList = hull.minXy;
+#ifdef DEBUG_CONVEX_HULL
+ printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count);
+#endif
+}
+
+btVector3 btConvexHullInternal::toBtVector(const Point32& v)
+{
+ btVector3 p;
+ p[medAxis] = btScalar(v.x);
+ p[maxAxis] = btScalar(v.y);
+ p[minAxis] = btScalar(v.z);
+ return p * scaling;
+}
+
+btVector3 btConvexHullInternal::getBtNormal(Face* face)
+{
+ return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
+}
+
+btVector3 btConvexHullInternal::getCoordinates(const Vertex* v)
+{
+ btVector3 p;
+ p[medAxis] = v->xvalue();
+ p[maxAxis] = v->yvalue();
+ p[minAxis] = v->zvalue();
+ return p * scaling + center;
+}
+
+btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount)
+{
+ if (!vertexList) {
+ return 0;
+ }
+ int stamp = --mergeStamp;
+ btAlignedObjectArray<Vertex*> stack;
+ vertexList->copy = stamp;
+ stack.push_back(vertexList);
+ btAlignedObjectArray<Face*> faces;
+
+ Point32 ref = vertexList->point;
+ Int128 hullCenterX(0, 0);
+ Int128 hullCenterY(0, 0);
+ Int128 hullCenterZ(0, 0);
+ Int128 volume(0, 0);
+
+ while (stack.size() > 0) {
+ Vertex* v = stack[stack.size() - 1];
+ stack.pop_back();
+ Edge* e = v->edges;
+ if (e) {
+ do {
+ if (e->target->copy != stamp) {
+ e->target->copy = stamp;
+ stack.push_back(e->target);
+ }
+ if (e->copy != stamp) {
+ Face* face = facePool.newObject();
+ face->init(e->target, e->reverse->prev->target, v);
+ faces.push_back(face);
+ Edge* f = e;
+
+ Vertex* a = NULL;
+ Vertex* b = NULL;
+ do {
+ if (a && b) {
+ int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
+ btAssert(vol >= 0);
+ Point32 c = v->point + a->point + b->point + ref;
+ hullCenterX += vol * c.x;
+ hullCenterY += vol * c.y;
+ hullCenterZ += vol * c.z;
+ volume += vol;
+ }
+
+ btAssert(f->copy != stamp);
+ f->copy = stamp;
+ f->face = face;
+
+ a = b;
+ b = f->target;
+
+ f = f->reverse->prev;
+ } while (f != e);
+ }
+ e = e->next;
+ } while (e != v->edges);
+ }
+ }
+
+ if (volume.getSign() <= 0) {
+ return 0;
+ }
+
+ btVector3 hullCenter;
+ hullCenter[medAxis] = hullCenterX.toScalar();
+ hullCenter[maxAxis] = hullCenterY.toScalar();
+ hullCenter[minAxis] = hullCenterZ.toScalar();
+ hullCenter /= 4 * volume.toScalar();
+ hullCenter *= scaling;
+
+ int faceCount = faces.size();
+
+ if (clampAmount > 0) {
+ btScalar minDist = SIMD_INFINITY;
+ for (int i = 0; i < faceCount; i++) {
+ btVector3 normal = getBtNormal(faces[i]);
+ btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter);
+ if (dist < minDist) {
+ minDist = dist;
+ }
+ }
+
+ if (minDist <= 0) {
+ return 0;
+ }
+
+ amount = btMin(amount, minDist * clampAmount);
+ }
+
+ unsigned int seed = 243703;
+ for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223) {
+ btSwap(faces[i], faces[seed % faceCount]);
+ }
+
+ for (int i = 0; i < faceCount; i++) {
+ if (!shiftFace(faces[i], amount, stack)) {
+ return -amount;
+ }
+ }
+
+ return amount;
+}
+
+bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack)
+{
+ btVector3 origShift = getBtNormal(face) * -amount;
+ if (scaling[0] != 0) {
+ origShift[0] /= scaling[0];
+ }
+ if (scaling[1] != 0) {
+ origShift[1] /= scaling[1];
+ }
+ if (scaling[2] != 0) {
+ origShift[2] /= scaling[2];
+ }
+ Point32 shift((int32_t)origShift[medAxis], (int32_t)origShift[maxAxis], (int32_t)origShift[minAxis]);
+ if (shift.isZero()) {
+ return true;
+ }
+ Point64 normal = face->getNormal();
+#ifdef DEBUG_CONVEX_HULL
+ printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n",
+ face->origin.x, face->origin.y, face->origin.z, face->dir0.x, face->dir0.y, face->dir0.z, face->dir1.x, face->dir1.y, face->dir1.z, shift.x, shift.y, shift.z);
+#endif
+ int64_t origDot = face->origin.dot(normal);
+ Point32 shiftedOrigin = face->origin + shift;
+ int64_t shiftedDot = shiftedOrigin.dot(normal);
+ btAssert(shiftedDot <= origDot);
+ if (shiftedDot >= origDot) {
+ return false;
+ }
+
+ Edge* intersection = NULL;
+
+ Edge* startEdge = face->nearbyVertex->edges;
+#ifdef DEBUG_CONVEX_HULL
+ printf("Start edge is ");
+ startEdge->print();
+ printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot);
+#endif
+ Rational128 optDot = face->nearbyVertex->dot(normal);
+ int cmp = optDot.compare(shiftedDot);
+#ifdef SHOW_ITERATIONS
+ int n = 0;
+#endif
+ if (cmp >= 0) {
+ Edge* e = startEdge;
+ do {
+#ifdef SHOW_ITERATIONS
+ n++;
+#endif
+ Rational128 dot = e->target->dot(normal);
+ btAssert(dot.compare(origDot) <= 0);
+#ifdef DEBUG_CONVEX_HULL
+ printf("Moving downwards, edge is ");
+ e->print();
+ printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
+#endif
+ if (dot.compare(optDot) < 0) {
+ int c = dot.compare(shiftedDot);
+ optDot = dot;
+ e = e->reverse;
+ startEdge = e;
+ if (c < 0) {
+ intersection = e;
+ break;
+ }
+ cmp = c;
+ }
+ e = e->prev;
+ } while (e != startEdge);
+
+ if (!intersection) {
+ return false;
+ }
+ }
+ else {
+ Edge* e = startEdge;
+ do {
+#ifdef SHOW_ITERATIONS
+ n++;
+#endif
+ Rational128 dot = e->target->dot(normal);
+ btAssert(dot.compare(origDot) <= 0);
+#ifdef DEBUG_CONVEX_HULL
+ printf("Moving upwards, edge is ");
+ e->print();
+ printf(", dot is %f (%f %lld)\n", (float)dot.toScalar(), (float)optDot.toScalar(), shiftedDot);
+#endif
+ if (dot.compare(optDot) > 0) {
+ cmp = dot.compare(shiftedDot);
+ if (cmp >= 0) {
+ intersection = e;
+ break;
+ }
+ optDot = dot;
+ e = e->reverse;
+ startEdge = e;
+ }
+ e = e->prev;
+ } while (e != startEdge);
+
+ if (!intersection) {
+ return true;
+ }
+ }
+
+#ifdef SHOW_ITERATIONS
+ printf("Needed %d iterations to find initial intersection\n", n);
+#endif
+
+ if (cmp == 0) {
+ Edge* e = intersection->reverse->next;
+#ifdef SHOW_ITERATIONS
+ n = 0;
+#endif
+ while (e->target->dot(normal).compare(shiftedDot) <= 0) {
+#ifdef SHOW_ITERATIONS
+ n++;
+#endif
+ e = e->next;
+ if (e == intersection->reverse) {
+ return true;
+ }
+#ifdef DEBUG_CONVEX_HULL
+ printf("Checking for outwards edge, current edge is ");
+ e->print();
+ printf("\n");
+#endif
+ }
+#ifdef SHOW_ITERATIONS
+ printf("Needed %d iterations to check for complete containment\n", n);
+#endif
+ }
+
+ Edge* firstIntersection = NULL;
+ Edge* faceEdge = NULL;
+ Edge* firstFaceEdge = NULL;
+
+#ifdef SHOW_ITERATIONS
+ int m = 0;
+#endif
+ while (true) {
+#ifdef SHOW_ITERATIONS
+ m++;
+#endif
+#ifdef DEBUG_CONVEX_HULL
+ printf("Intersecting edge is ");
+ intersection->print();
+ printf("\n");
+#endif
+ if (cmp == 0) {
+ Edge* e = intersection->reverse->next;
+ startEdge = e;
+#ifdef SHOW_ITERATIONS
+ n = 0;
+#endif
+ while (true) {
+#ifdef SHOW_ITERATIONS
+ n++;
+#endif
+ if (e->target->dot(normal).compare(shiftedDot) >= 0) {
+ break;
+ }
+ intersection = e->reverse;
+ e = e->next;
+ if (e == startEdge) {
+ return true;
+ }
+ }
+#ifdef SHOW_ITERATIONS
+ printf("Needed %d iterations to advance intersection\n", n);
+#endif
+ }
+
+#ifdef DEBUG_CONVEX_HULL
+ printf("Advanced intersecting edge to ");
+ intersection->print();
+ printf(", cmp = %d\n", cmp);
+#endif
+
+ if (!firstIntersection) {
+ firstIntersection = intersection;
+ }
+ else if (intersection == firstIntersection) {
+ break;
+ }
+
+ int prevCmp = cmp;
+ Edge* prevIntersection = intersection;
+ Edge* prevFaceEdge = faceEdge;
+
+ Edge* e = intersection->reverse;
+#ifdef SHOW_ITERATIONS
+ n = 0;
+#endif
+ while (true) {
+#ifdef SHOW_ITERATIONS
+ n++;
+#endif
+ e = e->reverse->prev;
+ btAssert(e != intersection->reverse);
+ cmp = e->target->dot(normal).compare(shiftedDot);
+#ifdef DEBUG_CONVEX_HULL
+ printf("Testing edge ");
+ e->print();
+ printf(" -> cmp = %d\n", cmp);
+#endif
+ if (cmp >= 0) {
+ intersection = e;
+ break;
+ }
+ }
+#ifdef SHOW_ITERATIONS
+ printf("Needed %d iterations to find other intersection of face\n", n);
+#endif
+
+ if (cmp > 0) {
+ Vertex* removed = intersection->target;
+ e = intersection->reverse;
+ if (e->prev == e) {
+ removed->edges = NULL;
+ }
+ else {
+ removed->edges = e->prev;
+ e->prev->link(e->next);
+ e->link(e);
+ }
+#ifdef DEBUG_CONVEX_HULL
+ printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
+#endif
+
+ Point64 n0 = intersection->face->getNormal();
+ Point64 n1 = intersection->reverse->face->getNormal();
+ int64_t m00 = face->dir0.dot(n0);
+ int64_t m01 = face->dir1.dot(n0);
+ int64_t m10 = face->dir0.dot(n1);
+ int64_t m11 = face->dir1.dot(n1);
+ int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
+ int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
+ Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
+ btAssert(det.getSign() != 0);
+ Vertex* v = vertexPool.newObject();
+ v->point.index = -1;
+ v->copy = -1;
+ v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
+ + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
+ Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
+ + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
+ Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
+ + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
+ det);
+ v->point.x = (int32_t)v->point128.xvalue();
+ v->point.y = (int32_t)v->point128.yvalue();
+ v->point.z = (int32_t)v->point128.zvalue();
+ intersection->target = v;
+ v->edges = e;
+
+ stack.push_back(v);
+ stack.push_back(removed);
+ stack.push_back(NULL);
+ }
+
+ if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target)) {
+ faceEdge = newEdgePair(prevIntersection->target, intersection->target);
+ if (prevCmp == 0) {
+ faceEdge->link(prevIntersection->reverse->next);
+ }
+ if ((prevCmp == 0) || prevFaceEdge) {
+ prevIntersection->reverse->link(faceEdge);
+ }
+ if (cmp == 0) {
+ intersection->reverse->prev->link(faceEdge->reverse);
+ }
+ faceEdge->reverse->link(intersection->reverse);
+ }
+ else {
+ faceEdge = prevIntersection->reverse->next;
+ }
+
+ if (prevFaceEdge) {
+ if (prevCmp > 0) {
+ faceEdge->link(prevFaceEdge->reverse);
+ }
+ else if (faceEdge != prevFaceEdge->reverse) {
+ stack.push_back(prevFaceEdge->target);
+ while (faceEdge->next != prevFaceEdge->reverse) {
+ Vertex* removed = faceEdge->next->target;
+ removeEdgePair(faceEdge->next);
+ stack.push_back(removed);
+#ifdef DEBUG_CONVEX_HULL
+ printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
+#endif
+ }
+ stack.push_back(NULL);
+ }
+ }
+ faceEdge->face = face;
+ faceEdge->reverse->face = intersection->face;
+
+ if (!firstFaceEdge) {
+ firstFaceEdge = faceEdge;
+ }
+ }
+#ifdef SHOW_ITERATIONS
+ printf("Needed %d iterations to process all intersections\n", m);
+#endif
+
+ if (cmp > 0) {
+ firstFaceEdge->reverse->target = faceEdge->target;
+ firstIntersection->reverse->link(firstFaceEdge);
+ firstFaceEdge->link(faceEdge->reverse);
+ }
+ else if (firstFaceEdge != faceEdge->reverse) {
+ stack.push_back(faceEdge->target);
+ while (firstFaceEdge->next != faceEdge->reverse) {
+ Vertex* removed = firstFaceEdge->next->target;
+ removeEdgePair(firstFaceEdge->next);
+ stack.push_back(removed);
+#ifdef DEBUG_CONVEX_HULL
+ printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z);
+#endif
+ }
+ stack.push_back(NULL);
+ }
+
+ btAssert(stack.size() > 0);
+ vertexList = stack[0];
+
+#ifdef DEBUG_CONVEX_HULL
+ printf("Removing part\n");
+#endif
+#ifdef SHOW_ITERATIONS
+ n = 0;
+#endif
+ int pos = 0;
+ while (pos < stack.size()) {
+ int end = stack.size();
+ while (pos < end) {
+ Vertex* kept = stack[pos++];
+#ifdef DEBUG_CONVEX_HULL
+ kept->print();
+#endif
+ bool deeper = false;
+ Vertex* removed;
+ while ((removed = stack[pos++]) != NULL) {
+#ifdef SHOW_ITERATIONS
+ n++;
+#endif
+ kept->receiveNearbyFaces(removed);
+ while (removed->edges) {
+ if (!deeper) {
+ deeper = true;
+ stack.push_back(kept);
+ }
+ stack.push_back(removed->edges->target);
+ removeEdgePair(removed->edges);
+ }
+ }
+ if (deeper) {
+ stack.push_back(NULL);
+ }
+ }
+ }
+#ifdef SHOW_ITERATIONS
+ printf("Needed %d iterations to remove part\n", n);
+#endif
+
+ stack.resize(0);
+ face->origin = shiftedOrigin;
+
+ return true;
+}
+
+static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
+{
+ int index = vertex->copy;
+ if (index < 0) {
+ index = vertices.size();
+ vertex->copy = index;
+ vertices.push_back(vertex);
+#ifdef DEBUG_CONVEX_HULL
+ printf("Vertex %d gets index *%d\n", vertex->point.index, index);
+#endif
+ }
+ return index;
+}
+
+btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
+{
+ if (count <= 0) {
+ vertices.clear();
+ edges.clear();
+ faces.clear();
+ return 0;
+ }
+
+ btConvexHullInternal hull;
+ hull.compute(coords, doubleCoords, stride, count);
+
+ btScalar shift = 0;
+ if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0)) {
+ vertices.clear();
+ edges.clear();
+ faces.clear();
+ return shift;
+ }
+
+ vertices.resize(0);
+ edges.resize(0);
+ faces.resize(0);
+
+ btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
+ getVertexCopy(hull.vertexList, oldVertices);
+ int copied = 0;
+ while (copied < oldVertices.size()) {
+ btConvexHullInternal::Vertex* v = oldVertices[copied];
+ vertices.push_back(hull.getCoordinates(v));
+ btConvexHullInternal::Edge* firstEdge = v->edges;
+ if (firstEdge) {
+ int firstCopy = -1;
+ int prevCopy = -1;
+ btConvexHullInternal::Edge* e = firstEdge;
+ do {
+ if (e->copy < 0) {
+ int s = edges.size();
+ edges.push_back(Edge());
+ edges.push_back(Edge());
+ Edge* c = &edges[s];
+ Edge* r = &edges[s + 1];
+ e->copy = s;
+ e->reverse->copy = s + 1;
+ c->reverse = 1;
+ r->reverse = -1;
+ c->targetVertex = getVertexCopy(e->target, oldVertices);
+ r->targetVertex = copied;
+#ifdef DEBUG_CONVEX_HULL
+ printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex());
+#endif
+ }
+ if (prevCopy >= 0) {
+ edges[e->copy].next = prevCopy - e->copy;
+ }
+ else {
+ firstCopy = e->copy;
+ }
+ prevCopy = e->copy;
+ e = e->next;
+ } while (e != firstEdge);
+ edges[firstCopy].next = prevCopy - firstCopy;
+ }
+ copied++;
+ }
+
+ for (int i = 0; i < copied; i++) {
+ btConvexHullInternal::Vertex* v = oldVertices[i];
+ btConvexHullInternal::Edge* firstEdge = v->edges;
+ if (firstEdge) {
+ btConvexHullInternal::Edge* e = firstEdge;
+ do {
+ if (e->copy >= 0) {
+#ifdef DEBUG_CONVEX_HULL
+ printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
+#endif
+ faces.push_back(e->copy);
+ btConvexHullInternal::Edge* f = e;
+ do {
+#ifdef DEBUG_CONVEX_HULL
+ printf(" Face *%d\n", edges[f->copy].getTargetVertex());
+#endif
+ f->copy = -1;
+ f = f->reverse->prev;
+ } while (f != e);
+ }
+ e = e->next;
+ } while (e != firstEdge);
+ }
+ }
+
+ return shift;
+}
diff --git a/Extras/VHACD/src/premake4.lua b/Extras/VHACD/src/premake4.lua
new file mode 100644
index 0000000..3566943
--- /dev/null
+++ b/Extras/VHACD/src/premake4.lua
@@ -0,0 +1,10 @@
+ project "vhacd"
+
+ kind "StaticLib"
+ includedirs {
+ "../inc","../public",
+ }
+ files {
+ "*.cpp",
+ "*.h"
+ }
diff --git a/Extras/VHACD/src/vhacdICHull.cpp b/Extras/VHACD/src/vhacdICHull.cpp
new file mode 100644
index 0000000..4216f05
--- /dev/null
+++ b/Extras/VHACD/src/vhacdICHull.cpp
@@ -0,0 +1,725 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#include "vhacdICHull.h"
+#include <limits>
+namespace VHACD {
+const double ICHull::sc_eps = 1.0e-15;
+const int ICHull::sc_dummyIndex = std::numeric_limits<int>::max();
+ICHull::ICHull()
+{
+ m_isFlat = false;
+}
+bool ICHull::AddPoints(const Vec3<double>* points, size_t nPoints)
+{
+ if (!points) {
+ return false;
+ }
+ CircularListElement<TMMVertex>* vertex = NULL;
+ for (size_t i = 0; i < nPoints; i++) {
+ vertex = m_mesh.AddVertex();
+ vertex->GetData().m_pos.X() = points[i].X();
+ vertex->GetData().m_pos.Y() = points[i].Y();
+ vertex->GetData().m_pos.Z() = points[i].Z();
+ vertex->GetData().m_name = static_cast<int>(i);
+ }
+ return true;
+}
+bool ICHull::AddPoint(const Vec3<double>& point, int id)
+{
+ if (AddPoints(&point, 1)) {
+ m_mesh.m_vertices.GetData().m_name = id;
+ return true;
+ }
+ return false;
+}
+
+ICHullError ICHull::Process()
+{
+ unsigned int addedPoints = 0;
+ if (m_mesh.GetNVertices() < 3) {
+ return ICHullErrorNotEnoughPoints;
+ }
+ if (m_mesh.GetNVertices() == 3) {
+ m_isFlat = true;
+ CircularListElement<TMMTriangle>* t1 = m_mesh.AddTriangle();
+ CircularListElement<TMMTriangle>* t2 = m_mesh.AddTriangle();
+ CircularListElement<TMMVertex>* v0 = m_mesh.m_vertices.GetHead();
+ CircularListElement<TMMVertex>* v1 = v0->GetNext();
+ CircularListElement<TMMVertex>* v2 = v1->GetNext();
+ // Compute the normal to the plane
+ Vec3<double> p0 = v0->GetData().m_pos;
+ Vec3<double> p1 = v1->GetData().m_pos;
+ Vec3<double> p2 = v2->GetData().m_pos;
+ m_normal = (p1 - p0) ^ (p2 - p0);
+ m_normal.Normalize();
+ t1->GetData().m_vertices[0] = v0;
+ t1->GetData().m_vertices[1] = v1;
+ t1->GetData().m_vertices[2] = v2;
+ t2->GetData().m_vertices[0] = v1;
+ t2->GetData().m_vertices[1] = v2;
+ t2->GetData().m_vertices[2] = v2;
+ return ICHullErrorOK;
+ }
+ if (m_isFlat) {
+ m_mesh.m_edges.Clear();
+ m_mesh.m_triangles.Clear();
+ m_isFlat = false;
+ }
+ if (m_mesh.GetNTriangles() == 0) // we have to create the first polyhedron
+ {
+ ICHullError res = DoubleTriangle();
+ if (res != ICHullErrorOK) {
+ return res;
+ }
+ else {
+ addedPoints += 3;
+ }
+ }
+ CircularList<TMMVertex>& vertices = m_mesh.GetVertices();
+ // go to the first added and not processed vertex
+ while (!(vertices.GetHead()->GetPrev()->GetData().m_tag)) {
+ vertices.Prev();
+ }
+ while (!vertices.GetData().m_tag) // not processed
+ {
+ vertices.GetData().m_tag = true;
+ if (ProcessPoint()) {
+ addedPoints++;
+ CleanUp(addedPoints);
+ vertices.Next();
+ if (!GetMesh().CheckConsistancy()) {
+ size_t nV = m_mesh.GetNVertices();
+ CircularList<TMMVertex>& vertices = m_mesh.GetVertices();
+ for (size_t v = 0; v < nV; ++v) {
+ if (vertices.GetData().m_name == sc_dummyIndex) {
+ vertices.Delete();
+ break;
+ }
+ vertices.Next();
+ }
+ return ICHullErrorInconsistent;
+ }
+ }
+ }
+ if (m_isFlat) {
+ SArray<CircularListElement<TMMTriangle>*> trianglesToDuplicate;
+ size_t nT = m_mesh.GetNTriangles();
+ for (size_t f = 0; f < nT; f++) {
+ TMMTriangle& currentTriangle = m_mesh.m_triangles.GetHead()->GetData();
+ if (currentTriangle.m_vertices[0]->GetData().m_name == sc_dummyIndex || currentTriangle.m_vertices[1]->GetData().m_name == sc_dummyIndex || currentTriangle.m_vertices[2]->GetData().m_name == sc_dummyIndex) {
+ m_trianglesToDelete.PushBack(m_mesh.m_triangles.GetHead());
+ for (int k = 0; k < 3; k++) {
+ for (int h = 0; h < 2; h++) {
+ if (currentTriangle.m_edges[k]->GetData().m_triangles[h] == m_mesh.m_triangles.GetHead()) {
+ currentTriangle.m_edges[k]->GetData().m_triangles[h] = 0;
+ break;
+ }
+ }
+ }
+ }
+ else {
+ trianglesToDuplicate.PushBack(m_mesh.m_triangles.GetHead());
+ }
+ m_mesh.m_triangles.Next();
+ }
+ size_t nE = m_mesh.GetNEdges();
+ for (size_t e = 0; e < nE; e++) {
+ TMMEdge& currentEdge = m_mesh.m_edges.GetHead()->GetData();
+ if (currentEdge.m_triangles[0] == 0 && currentEdge.m_triangles[1] == 0) {
+ m_edgesToDelete.PushBack(m_mesh.m_edges.GetHead());
+ }
+ m_mesh.m_edges.Next();
+ }
+ size_t nV = m_mesh.GetNVertices();
+ CircularList<TMMVertex>& vertices = m_mesh.GetVertices();
+ for (size_t v = 0; v < nV; ++v) {
+ if (vertices.GetData().m_name == sc_dummyIndex) {
+ vertices.Delete();
+ }
+ else {
+ vertices.GetData().m_tag = false;
+ vertices.Next();
+ }
+ }
+ CleanEdges();
+ CleanTriangles();
+ CircularListElement<TMMTriangle>* newTriangle;
+ for (size_t t = 0; t < trianglesToDuplicate.Size(); t++) {
+ newTriangle = m_mesh.AddTriangle();
+ newTriangle->GetData().m_vertices[0] = trianglesToDuplicate[t]->GetData().m_vertices[1];
+ newTriangle->GetData().m_vertices[1] = trianglesToDuplicate[t]->GetData().m_vertices[0];
+ newTriangle->GetData().m_vertices[2] = trianglesToDuplicate[t]->GetData().m_vertices[2];
+ }
+ }
+ return ICHullErrorOK;
+}
+ICHullError ICHull::Process(const unsigned int nPointsCH,
+ const double minVolume)
+{
+ unsigned int addedPoints = 0;
+ if (nPointsCH < 3 || m_mesh.GetNVertices() < 3) {
+ return ICHullErrorNotEnoughPoints;
+ }
+ if (m_mesh.GetNVertices() == 3) {
+ m_isFlat = true;
+ CircularListElement<TMMTriangle>* t1 = m_mesh.AddTriangle();
+ CircularListElement<TMMTriangle>* t2 = m_mesh.AddTriangle();
+ CircularListElement<TMMVertex>* v0 = m_mesh.m_vertices.GetHead();
+ CircularListElement<TMMVertex>* v1 = v0->GetNext();
+ CircularListElement<TMMVertex>* v2 = v1->GetNext();
+ // Compute the normal to the plane
+ Vec3<double> p0 = v0->GetData().m_pos;
+ Vec3<double> p1 = v1->GetData().m_pos;
+ Vec3<double> p2 = v2->GetData().m_pos;
+ m_normal = (p1 - p0) ^ (p2 - p0);
+ m_normal.Normalize();
+ t1->GetData().m_vertices[0] = v0;
+ t1->GetData().m_vertices[1] = v1;
+ t1->GetData().m_vertices[2] = v2;
+ t2->GetData().m_vertices[0] = v1;
+ t2->GetData().m_vertices[1] = v0;
+ t2->GetData().m_vertices[2] = v2;
+ return ICHullErrorOK;
+ }
+
+ if (m_isFlat) {
+ m_mesh.m_triangles.Clear();
+ m_mesh.m_edges.Clear();
+ m_isFlat = false;
+ }
+
+ if (m_mesh.GetNTriangles() == 0) // we have to create the first polyhedron
+ {
+ ICHullError res = DoubleTriangle();
+ if (res != ICHullErrorOK) {
+ return res;
+ }
+ else {
+ addedPoints += 3;
+ }
+ }
+ CircularList<TMMVertex>& vertices = m_mesh.GetVertices();
+ while (!vertices.GetData().m_tag && addedPoints < nPointsCH) // not processed
+ {
+ if (!FindMaxVolumePoint((addedPoints > 4) ? minVolume : 0.0)) {
+ break;
+ }
+ vertices.GetData().m_tag = true;
+ if (ProcessPoint()) {
+ addedPoints++;
+ CleanUp(addedPoints);
+ if (!GetMesh().CheckConsistancy()) {
+ size_t nV = m_mesh.GetNVertices();
+ CircularList<TMMVertex>& vertices = m_mesh.GetVertices();
+ for (size_t v = 0; v < nV; ++v) {
+ if (vertices.GetData().m_name == sc_dummyIndex) {
+ vertices.Delete();
+ break;
+ }
+ vertices.Next();
+ }
+ return ICHullErrorInconsistent;
+ }
+ vertices.Next();
+ }
+ }
+ // delete remaining points
+ while (!vertices.GetData().m_tag) {
+ vertices.Delete();
+ }
+ if (m_isFlat) {
+ SArray<CircularListElement<TMMTriangle>*> trianglesToDuplicate;
+ size_t nT = m_mesh.GetNTriangles();
+ for (size_t f = 0; f < nT; f++) {
+ TMMTriangle& currentTriangle = m_mesh.m_triangles.GetHead()->GetData();
+ if (currentTriangle.m_vertices[0]->GetData().m_name == sc_dummyIndex || currentTriangle.m_vertices[1]->GetData().m_name == sc_dummyIndex || currentTriangle.m_vertices[2]->GetData().m_name == sc_dummyIndex) {
+ m_trianglesToDelete.PushBack(m_mesh.m_triangles.GetHead());
+ for (int k = 0; k < 3; k++) {
+ for (int h = 0; h < 2; h++) {
+ if (currentTriangle.m_edges[k]->GetData().m_triangles[h] == m_mesh.m_triangles.GetHead()) {
+ currentTriangle.m_edges[k]->GetData().m_triangles[h] = 0;
+ break;
+ }
+ }
+ }
+ }
+ else {
+ trianglesToDuplicate.PushBack(m_mesh.m_triangles.GetHead());
+ }
+ m_mesh.m_triangles.Next();
+ }
+ size_t nE = m_mesh.GetNEdges();
+ for (size_t e = 0; e < nE; e++) {
+ TMMEdge& currentEdge = m_mesh.m_edges.GetHead()->GetData();
+ if (currentEdge.m_triangles[0] == 0 && currentEdge.m_triangles[1] == 0) {
+ m_edgesToDelete.PushBack(m_mesh.m_edges.GetHead());
+ }
+ m_mesh.m_edges.Next();
+ }
+ size_t nV = m_mesh.GetNVertices();
+ CircularList<TMMVertex>& vertices = m_mesh.GetVertices();
+ for (size_t v = 0; v < nV; ++v) {
+ if (vertices.GetData().m_name == sc_dummyIndex) {
+ vertices.Delete();
+ }
+ else {
+ vertices.GetData().m_tag = false;
+ vertices.Next();
+ }
+ }
+ CleanEdges();
+ CleanTriangles();
+ CircularListElement<TMMTriangle>* newTriangle;
+ for (size_t t = 0; t < trianglesToDuplicate.Size(); t++) {
+ newTriangle = m_mesh.AddTriangle();
+ newTriangle->GetData().m_vertices[0] = trianglesToDuplicate[t]->GetData().m_vertices[1];
+ newTriangle->GetData().m_vertices[1] = trianglesToDuplicate[t]->GetData().m_vertices[0];
+ newTriangle->GetData().m_vertices[2] = trianglesToDuplicate[t]->GetData().m_vertices[2];
+ }
+ }
+ return ICHullErrorOK;
+}
+bool ICHull::FindMaxVolumePoint(const double minVolume)
+{
+ CircularList<TMMVertex>& vertices = m_mesh.GetVertices();
+ CircularListElement<TMMVertex>* vMaxVolume = 0;
+ CircularListElement<TMMVertex>* vHeadPrev = vertices.GetHead()->GetPrev();
+
+ double maxVolume = minVolume;
+ double volume = 0.0;
+ while (!vertices.GetData().m_tag) // not processed
+ {
+ if (ComputePointVolume(volume, false)) {
+ if (maxVolume < volume) {
+ maxVolume = volume;
+ vMaxVolume = vertices.GetHead();
+ }
+ vertices.Next();
+ }
+ }
+ CircularListElement<TMMVertex>* vHead = vHeadPrev->GetNext();
+ vertices.GetHead() = vHead;
+ if (!vMaxVolume) {
+ return false;
+ }
+ if (vMaxVolume != vHead) {
+ Vec3<double> pos = vHead->GetData().m_pos;
+ int id = vHead->GetData().m_name;
+ vHead->GetData().m_pos = vMaxVolume->GetData().m_pos;
+ vHead->GetData().m_name = vMaxVolume->GetData().m_name;
+ vMaxVolume->GetData().m_pos = pos;
+ vHead->GetData().m_name = id;
+ }
+ return true;
+}
+ICHullError ICHull::DoubleTriangle()
+{
+ // find three non colinear points
+ m_isFlat = false;
+ CircularList<TMMVertex>& vertices = m_mesh.GetVertices();
+ CircularListElement<TMMVertex>* v0 = vertices.GetHead();
+ while (Colinear(v0->GetData().m_pos,
+ v0->GetNext()->GetData().m_pos,
+ v0->GetNext()->GetNext()->GetData().m_pos)) {
+ if ((v0 = v0->GetNext()) == vertices.GetHead()) {
+ return ICHullErrorCoplanarPoints;
+ }
+ }
+ CircularListElement<TMMVertex>* v1 = v0->GetNext();
+ CircularListElement<TMMVertex>* v2 = v1->GetNext();
+ // mark points as processed
+ v0->GetData().m_tag = v1->GetData().m_tag = v2->GetData().m_tag = true;
+
+ // create two triangles
+ CircularListElement<TMMTriangle>* f0 = MakeFace(v0, v1, v2, 0);
+ MakeFace(v2, v1, v0, f0);
+
+ // find a fourth non-coplanar point to form tetrahedron
+ CircularListElement<TMMVertex>* v3 = v2->GetNext();
+ vertices.GetHead() = v3;
+
+ double vol = ComputeVolume4(v0->GetData().m_pos, v1->GetData().m_pos, v2->GetData().m_pos, v3->GetData().m_pos);
+ while (fabs(vol) < sc_eps && !v3->GetNext()->GetData().m_tag) {
+ v3 = v3->GetNext();
+ vol = ComputeVolume4(v0->GetData().m_pos, v1->GetData().m_pos, v2->GetData().m_pos, v3->GetData().m_pos);
+ }
+ if (fabs(vol) < sc_eps) {
+ // compute the barycenter
+ Vec3<double> bary(0.0, 0.0, 0.0);
+ CircularListElement<TMMVertex>* vBary = v0;
+ do {
+ bary += vBary->GetData().m_pos;
+ } while ((vBary = vBary->GetNext()) != v0);
+ bary /= static_cast<double>(vertices.GetSize());
+
+ // Compute the normal to the plane
+ Vec3<double> p0 = v0->GetData().m_pos;
+ Vec3<double> p1 = v1->GetData().m_pos;
+ Vec3<double> p2 = v2->GetData().m_pos;
+ m_normal = (p1 - p0) ^ (p2 - p0);
+ m_normal.Normalize();
+ // add dummy vertex placed at (bary + normal)
+ vertices.GetHead() = v2;
+ Vec3<double> newPt = bary + m_normal;
+ AddPoint(newPt, sc_dummyIndex);
+ m_isFlat = true;
+ return ICHullErrorOK;
+ }
+ else if (v3 != vertices.GetHead()) {
+ TMMVertex temp;
+ temp.m_name = v3->GetData().m_name;
+ temp.m_pos = v3->GetData().m_pos;
+ v3->GetData().m_name = vertices.GetHead()->GetData().m_name;
+ v3->GetData().m_pos = vertices.GetHead()->GetData().m_pos;
+ vertices.GetHead()->GetData().m_name = temp.m_name;
+ vertices.GetHead()->GetData().m_pos = temp.m_pos;
+ }
+ return ICHullErrorOK;
+}
+CircularListElement<TMMTriangle>* ICHull::MakeFace(CircularListElement<TMMVertex>* v0,
+ CircularListElement<TMMVertex>* v1,
+ CircularListElement<TMMVertex>* v2,
+ CircularListElement<TMMTriangle>* fold)
+{
+ CircularListElement<TMMEdge>* e0;
+ CircularListElement<TMMEdge>* e1;
+ CircularListElement<TMMEdge>* e2;
+ int index = 0;
+ if (!fold) // if first face to be created
+ {
+ e0 = m_mesh.AddEdge(); // create the three edges
+ e1 = m_mesh.AddEdge();
+ e2 = m_mesh.AddEdge();
+ }
+ else // otherwise re-use existing edges (in reverse order)
+ {
+ e0 = fold->GetData().m_edges[2];
+ e1 = fold->GetData().m_edges[1];
+ e2 = fold->GetData().m_edges[0];
+ index = 1;
+ }
+ e0->GetData().m_vertices[0] = v0;
+ e0->GetData().m_vertices[1] = v1;
+ e1->GetData().m_vertices[0] = v1;
+ e1->GetData().m_vertices[1] = v2;
+ e2->GetData().m_vertices[0] = v2;
+ e2->GetData().m_vertices[1] = v0;
+ // create the new face
+ CircularListElement<TMMTriangle>* f = m_mesh.AddTriangle();
+ f->GetData().m_edges[0] = e0;
+ f->GetData().m_edges[1] = e1;
+ f->GetData().m_edges[2] = e2;
+ f->GetData().m_vertices[0] = v0;
+ f->GetData().m_vertices[1] = v1;
+ f->GetData().m_vertices[2] = v2;
+ // link edges to face f
+ e0->GetData().m_triangles[index] = e1->GetData().m_triangles[index] = e2->GetData().m_triangles[index] = f;
+ return f;
+}
+CircularListElement<TMMTriangle>* ICHull::MakeConeFace(CircularListElement<TMMEdge>* e, CircularListElement<TMMVertex>* p)
+{
+ // create two new edges if they don't already exist
+ CircularListElement<TMMEdge>* newEdges[2];
+ for (int i = 0; i < 2; ++i) {
+ if (!(newEdges[i] = e->GetData().m_vertices[i]->GetData().m_duplicate)) { // if the edge doesn't exits add it and mark the vertex as duplicated
+ newEdges[i] = m_mesh.AddEdge();
+ newEdges[i]->GetData().m_vertices[0] = e->GetData().m_vertices[i];
+ newEdges[i]->GetData().m_vertices[1] = p;
+ e->GetData().m_vertices[i]->GetData().m_duplicate = newEdges[i];
+ }
+ }
+ // make the new face
+ CircularListElement<TMMTriangle>* newFace = m_mesh.AddTriangle();
+ newFace->GetData().m_edges[0] = e;
+ newFace->GetData().m_edges[1] = newEdges[0];
+ newFace->GetData().m_edges[2] = newEdges[1];
+ MakeCCW(newFace, e, p);
+ for (int i = 0; i < 2; ++i) {
+ for (int j = 0; j < 2; ++j) {
+ if (!newEdges[i]->GetData().m_triangles[j]) {
+ newEdges[i]->GetData().m_triangles[j] = newFace;
+ break;
+ }
+ }
+ }
+ return newFace;
+}
+bool ICHull::ComputePointVolume(double& totalVolume, bool markVisibleFaces)
+{
+ // mark visible faces
+ CircularListElement<TMMTriangle>* fHead = m_mesh.GetTriangles().GetHead();
+ CircularListElement<TMMTriangle>* f = fHead;
+ CircularList<TMMVertex>& vertices = m_mesh.GetVertices();
+ CircularListElement<TMMVertex>* vertex0 = vertices.GetHead();
+ bool visible = false;
+ Vec3<double> pos0 = Vec3<double>(vertex0->GetData().m_pos.X(),
+ vertex0->GetData().m_pos.Y(),
+ vertex0->GetData().m_pos.Z());
+ double vol = 0.0;
+ totalVolume = 0.0;
+ Vec3<double> ver0, ver1, ver2;
+ do {
+ ver0.X() = f->GetData().m_vertices[0]->GetData().m_pos.X();
+ ver0.Y() = f->GetData().m_vertices[0]->GetData().m_pos.Y();
+ ver0.Z() = f->GetData().m_vertices[0]->GetData().m_pos.Z();
+ ver1.X() = f->GetData().m_vertices[1]->GetData().m_pos.X();
+ ver1.Y() = f->GetData().m_vertices[1]->GetData().m_pos.Y();
+ ver1.Z() = f->GetData().m_vertices[1]->GetData().m_pos.Z();
+ ver2.X() = f->GetData().m_vertices[2]->GetData().m_pos.X();
+ ver2.Y() = f->GetData().m_vertices[2]->GetData().m_pos.Y();
+ ver2.Z() = f->GetData().m_vertices[2]->GetData().m_pos.Z();
+ vol = ComputeVolume4(ver0, ver1, ver2, pos0);
+ if (vol < -sc_eps) {
+ vol = fabs(vol);
+ totalVolume += vol;
+ if (markVisibleFaces) {
+ f->GetData().m_visible = true;
+ m_trianglesToDelete.PushBack(f);
+ }
+ visible = true;
+ }
+ f = f->GetNext();
+ } while (f != fHead);
+
+ if (m_trianglesToDelete.Size() == m_mesh.m_triangles.GetSize()) {
+ for (size_t i = 0; i < m_trianglesToDelete.Size(); i++) {
+ m_trianglesToDelete[i]->GetData().m_visible = false;
+ }
+ visible = false;
+ }
+ // if no faces visible from p then p is inside the hull
+ if (!visible && markVisibleFaces) {
+ vertices.Delete();
+ m_trianglesToDelete.Resize(0);
+ return false;
+ }
+ return true;
+}
+bool ICHull::ProcessPoint()
+{
+ double totalVolume = 0.0;
+ if (!ComputePointVolume(totalVolume, true)) {
+ return false;
+ }
+ // Mark edges in interior of visible region for deletion.
+ // Create a new face based on each border edge
+ CircularListElement<TMMVertex>* v0 = m_mesh.GetVertices().GetHead();
+ CircularListElement<TMMEdge>* eHead = m_mesh.GetEdges().GetHead();
+ CircularListElement<TMMEdge>* e = eHead;
+ CircularListElement<TMMEdge>* tmp = 0;
+ int nvisible = 0;
+ m_edgesToDelete.Resize(0);
+ m_edgesToUpdate.Resize(0);
+ do {
+ tmp = e->GetNext();
+ nvisible = 0;
+ for (int k = 0; k < 2; k++) {
+ if (e->GetData().m_triangles[k]->GetData().m_visible) {
+ nvisible++;
+ }
+ }
+ if (nvisible == 2) {
+ m_edgesToDelete.PushBack(e);
+ }
+ else if (nvisible == 1) {
+ e->GetData().m_newFace = MakeConeFace(e, v0);
+ m_edgesToUpdate.PushBack(e);
+ }
+ e = tmp;
+ } while (e != eHead);
+ return true;
+}
+bool ICHull::MakeCCW(CircularListElement<TMMTriangle>* f,
+ CircularListElement<TMMEdge>* e,
+ CircularListElement<TMMVertex>* v)
+{
+ // the visible face adjacent to e
+ CircularListElement<TMMTriangle>* fv;
+ if (e->GetData().m_triangles[0]->GetData().m_visible) {
+ fv = e->GetData().m_triangles[0];
+ }
+ else {
+ fv = e->GetData().m_triangles[1];
+ }
+
+ // set vertex[0] and vertex[1] to have the same orientation as the corresponding vertices of fv.
+ int i; // index of e->m_vertices[0] in fv
+ CircularListElement<TMMVertex>* v0 = e->GetData().m_vertices[0];
+ CircularListElement<TMMVertex>* v1 = e->GetData().m_vertices[1];
+ for (i = 0; fv->GetData().m_vertices[i] != v0; i++)
+ ;
+
+ if (fv->GetData().m_vertices[(i + 1) % 3] != e->GetData().m_vertices[1]) {
+ f->GetData().m_vertices[0] = v1;
+ f->GetData().m_vertices[1] = v0;
+ }
+ else {
+ f->GetData().m_vertices[0] = v0;
+ f->GetData().m_vertices[1] = v1;
+ // swap edges
+ CircularListElement<TMMEdge>* tmp = f->GetData().m_edges[0];
+ f->GetData().m_edges[0] = f->GetData().m_edges[1];
+ f->GetData().m_edges[1] = tmp;
+ }
+ f->GetData().m_vertices[2] = v;
+ return true;
+}
+bool ICHull::CleanUp(unsigned int& addedPoints)
+{
+ bool r0 = CleanEdges();
+ bool r1 = CleanTriangles();
+ bool r2 = CleanVertices(addedPoints);
+ return r0 && r1 && r2;
+}
+bool ICHull::CleanEdges()
+{
+ // integrate the new faces into the data structure
+ CircularListElement<TMMEdge>* e;
+ const size_t ne_update = m_edgesToUpdate.Size();
+ for (size_t i = 0; i < ne_update; ++i) {
+ e = m_edgesToUpdate[i];
+ if (e->GetData().m_newFace) {
+ if (e->GetData().m_triangles[0]->GetData().m_visible) {
+ e->GetData().m_triangles[0] = e->GetData().m_newFace;
+ }
+ else {
+ e->GetData().m_triangles[1] = e->GetData().m_newFace;
+ }
+ e->GetData().m_newFace = 0;
+ }
+ }
+ // delete edges maked for deletion
+ CircularList<TMMEdge>& edges = m_mesh.GetEdges();
+ const size_t ne_delete = m_edgesToDelete.Size();
+ for (size_t i = 0; i < ne_delete; ++i) {
+ edges.Delete(m_edgesToDelete[i]);
+ }
+ m_edgesToDelete.Resize(0);
+ m_edgesToUpdate.Resize(0);
+ return true;
+}
+bool ICHull::CleanTriangles()
+{
+ CircularList<TMMTriangle>& triangles = m_mesh.GetTriangles();
+ const size_t nt_delete = m_trianglesToDelete.Size();
+ for (size_t i = 0; i < nt_delete; ++i) {
+ triangles.Delete(m_trianglesToDelete[i]);
+ }
+ m_trianglesToDelete.Resize(0);
+ return true;
+}
+bool ICHull::CleanVertices(unsigned int& addedPoints)
+{
+ // mark all vertices incident to some undeleted edge as on the hull
+ CircularList<TMMEdge>& edges = m_mesh.GetEdges();
+ CircularListElement<TMMEdge>* e = edges.GetHead();
+ size_t nE = edges.GetSize();
+ for (size_t i = 0; i < nE; i++) {
+ e->GetData().m_vertices[0]->GetData().m_onHull = true;
+ e->GetData().m_vertices[1]->GetData().m_onHull = true;
+ e = e->GetNext();
+ }
+ // delete all the vertices that have been processed but are not on the hull
+ CircularList<TMMVertex>& vertices = m_mesh.GetVertices();
+ CircularListElement<TMMVertex>* vHead = vertices.GetHead();
+ CircularListElement<TMMVertex>* v = vHead;
+ v = v->GetPrev();
+ do {
+ if (v->GetData().m_tag && !v->GetData().m_onHull) {
+ CircularListElement<TMMVertex>* tmp = v->GetPrev();
+ vertices.Delete(v);
+ v = tmp;
+ addedPoints--;
+ }
+ else {
+ v->GetData().m_duplicate = 0;
+ v->GetData().m_onHull = false;
+ v = v->GetPrev();
+ }
+ } while (v->GetData().m_tag && v != vHead);
+ return true;
+}
+void ICHull::Clear()
+{
+ m_mesh.Clear();
+ m_edgesToDelete.Resize(0);
+ m_edgesToUpdate.Resize(0);
+ m_trianglesToDelete.Resize(0);
+ m_isFlat = false;
+}
+const ICHull& ICHull::operator=(ICHull& rhs)
+{
+ if (&rhs != this) {
+ m_mesh.Copy(rhs.m_mesh);
+ m_edgesToDelete = rhs.m_edgesToDelete;
+ m_edgesToUpdate = rhs.m_edgesToUpdate;
+ m_trianglesToDelete = rhs.m_trianglesToDelete;
+ m_isFlat = rhs.m_isFlat;
+ }
+ return (*this);
+}
+bool ICHull::IsInside(const Vec3<double>& pt0, const double eps)
+{
+ const Vec3<double> pt(pt0.X(), pt0.Y(), pt0.Z());
+ if (m_isFlat) {
+ size_t nT = m_mesh.m_triangles.GetSize();
+ Vec3<double> ver0, ver1, ver2, a, b, c;
+ double u, v;
+ for (size_t t = 0; t < nT; t++) {
+ ver0.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.X();
+ ver0.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Y();
+ ver0.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Z();
+ ver1.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.X();
+ ver1.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Y();
+ ver1.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Z();
+ ver2.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.X();
+ ver2.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Y();
+ ver2.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Z();
+ a = ver1 - ver0;
+ b = ver2 - ver0;
+ c = pt - ver0;
+ u = c * a;
+ v = c * b;
+ if (u >= 0.0 && u <= 1.0 && v >= 0.0 && u + v <= 1.0) {
+ return true;
+ }
+ m_mesh.m_triangles.Next();
+ }
+ return false;
+ }
+ else {
+ size_t nT = m_mesh.m_triangles.GetSize();
+ Vec3<double> ver0, ver1, ver2;
+ double vol;
+ for (size_t t = 0; t < nT; t++) {
+ ver0.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.X();
+ ver0.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Y();
+ ver0.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[0]->GetData().m_pos.Z();
+ ver1.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.X();
+ ver1.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Y();
+ ver1.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[1]->GetData().m_pos.Z();
+ ver2.X() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.X();
+ ver2.Y() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Y();
+ ver2.Z() = m_mesh.m_triangles.GetHead()->GetData().m_vertices[2]->GetData().m_pos.Z();
+ vol = ComputeVolume4(ver0, ver1, ver2, pt);
+ if (vol < eps) {
+ return false;
+ }
+ m_mesh.m_triangles.Next();
+ }
+ return true;
+ }
+}
+}
diff --git a/Extras/VHACD/src/vhacdManifoldMesh.cpp b/Extras/VHACD/src/vhacdManifoldMesh.cpp
new file mode 100644
index 0000000..dffa024
--- /dev/null
+++ b/Extras/VHACD/src/vhacdManifoldMesh.cpp
@@ -0,0 +1,202 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#include "vhacdManifoldMesh.h"
+namespace VHACD {
+TMMVertex::TMMVertex(void)
+{
+ Initialize();
+}
+void TMMVertex::Initialize()
+{
+ m_name = 0;
+ m_id = 0;
+ m_duplicate = 0;
+ m_onHull = false;
+ m_tag = false;
+}
+
+TMMVertex::~TMMVertex(void)
+{
+}
+TMMEdge::TMMEdge(void)
+{
+ Initialize();
+}
+void TMMEdge::Initialize()
+{
+ m_id = 0;
+ m_triangles[0] = m_triangles[1] = m_newFace = 0;
+ m_vertices[0] = m_vertices[1] = 0;
+}
+TMMEdge::~TMMEdge(void)
+{
+}
+void TMMTriangle::Initialize()
+{
+ m_id = 0;
+ for (int i = 0; i < 3; i++) {
+ m_edges[i] = 0;
+ m_vertices[0] = 0;
+ }
+ m_visible = false;
+}
+TMMTriangle::TMMTriangle(void)
+{
+ Initialize();
+}
+TMMTriangle::~TMMTriangle(void)
+{
+}
+TMMesh::TMMesh()
+{
+}
+TMMesh::~TMMesh(void)
+{
+}
+void TMMesh::GetIFS(Vec3<double>* const points, Vec3<int>* const triangles)
+{
+ size_t nV = m_vertices.GetSize();
+ size_t nT = m_triangles.GetSize();
+
+ for (size_t v = 0; v < nV; v++) {
+ points[v] = m_vertices.GetData().m_pos;
+ m_vertices.GetData().m_id = v;
+ m_vertices.Next();
+ }
+ for (size_t f = 0; f < nT; f++) {
+ TMMTriangle& currentTriangle = m_triangles.GetData();
+ triangles[f].X() = static_cast<int>(currentTriangle.m_vertices[0]->GetData().m_id);
+ triangles[f].Y() = static_cast<int>(currentTriangle.m_vertices[1]->GetData().m_id);
+ triangles[f].Z() = static_cast<int>(currentTriangle.m_vertices[2]->GetData().m_id);
+ m_triangles.Next();
+ }
+}
+void TMMesh::Clear()
+{
+ m_vertices.Clear();
+ m_edges.Clear();
+ m_triangles.Clear();
+}
+void TMMesh::Copy(TMMesh& mesh)
+{
+ Clear();
+ // updating the id's
+ size_t nV = mesh.m_vertices.GetSize();
+ size_t nE = mesh.m_edges.GetSize();
+ size_t nT = mesh.m_triangles.GetSize();
+ for (size_t v = 0; v < nV; v++) {
+ mesh.m_vertices.GetData().m_id = v;
+ mesh.m_vertices.Next();
+ }
+ for (size_t e = 0; e < nE; e++) {
+ mesh.m_edges.GetData().m_id = e;
+ mesh.m_edges.Next();
+ }
+ for (size_t f = 0; f < nT; f++) {
+ mesh.m_triangles.GetData().m_id = f;
+ mesh.m_triangles.Next();
+ }
+ // copying data
+ m_vertices = mesh.m_vertices;
+ m_edges = mesh.m_edges;
+ m_triangles = mesh.m_triangles;
+
+ // generate mapping
+ CircularListElement<TMMVertex>** vertexMap = new CircularListElement<TMMVertex>*[nV];
+ CircularListElement<TMMEdge>** edgeMap = new CircularListElement<TMMEdge>*[nE];
+ CircularListElement<TMMTriangle>** triangleMap = new CircularListElement<TMMTriangle>*[nT];
+ for (size_t v = 0; v < nV; v++) {
+ vertexMap[v] = m_vertices.GetHead();
+ m_vertices.Next();
+ }
+ for (size_t e = 0; e < nE; e++) {
+ edgeMap[e] = m_edges.GetHead();
+ m_edges.Next();
+ }
+ for (size_t f = 0; f < nT; f++) {
+ triangleMap[f] = m_triangles.GetHead();
+ m_triangles.Next();
+ }
+
+ // updating pointers
+ for (size_t v = 0; v < nV; v++) {
+ if (vertexMap[v]->GetData().m_duplicate) {
+ vertexMap[v]->GetData().m_duplicate = edgeMap[vertexMap[v]->GetData().m_duplicate->GetData().m_id];
+ }
+ }
+ for (size_t e = 0; e < nE; e++) {
+ if (edgeMap[e]->GetData().m_newFace) {
+ edgeMap[e]->GetData().m_newFace = triangleMap[edgeMap[e]->GetData().m_newFace->GetData().m_id];
+ }
+ if (nT > 0) {
+ for (int f = 0; f < 2; f++) {
+ if (edgeMap[e]->GetData().m_triangles[f]) {
+ edgeMap[e]->GetData().m_triangles[f] = triangleMap[edgeMap[e]->GetData().m_triangles[f]->GetData().m_id];
+ }
+ }
+ }
+ for (int v = 0; v < 2; v++) {
+ if (edgeMap[e]->GetData().m_vertices[v]) {
+ edgeMap[e]->GetData().m_vertices[v] = vertexMap[edgeMap[e]->GetData().m_vertices[v]->GetData().m_id];
+ }
+ }
+ }
+ for (size_t f = 0; f < nT; f++) {
+ if (nE > 0) {
+ for (int e = 0; e < 3; e++) {
+ if (triangleMap[f]->GetData().m_edges[e]) {
+ triangleMap[f]->GetData().m_edges[e] = edgeMap[triangleMap[f]->GetData().m_edges[e]->GetData().m_id];
+ }
+ }
+ }
+ for (int v = 0; v < 3; v++) {
+ if (triangleMap[f]->GetData().m_vertices[v]) {
+ triangleMap[f]->GetData().m_vertices[v] = vertexMap[triangleMap[f]->GetData().m_vertices[v]->GetData().m_id];
+ }
+ }
+ }
+ delete[] vertexMap;
+ delete[] edgeMap;
+ delete[] triangleMap;
+}
+bool TMMesh::CheckConsistancy()
+{
+ size_t nE = m_edges.GetSize();
+ size_t nT = m_triangles.GetSize();
+ for (size_t e = 0; e < nE; e++) {
+ for (int f = 0; f < 2; f++) {
+ if (!m_edges.GetHead()->GetData().m_triangles[f]) {
+ return false;
+ }
+ }
+ m_edges.Next();
+ }
+ for (size_t f = 0; f < nT; f++) {
+ for (int e = 0; e < 3; e++) {
+ int found = 0;
+ for (int k = 0; k < 2; k++) {
+ if (m_triangles.GetHead()->GetData().m_edges[e]->GetData().m_triangles[k] == m_triangles.GetHead()) {
+ found++;
+ }
+ }
+ if (found != 1) {
+ return false;
+ }
+ }
+ m_triangles.Next();
+ }
+ return true;
+}
+}
\ No newline at end of file
diff --git a/Extras/VHACD/src/vhacdMesh.cpp b/Extras/VHACD/src/vhacdMesh.cpp
new file mode 100644
index 0000000..542ce43
--- /dev/null
+++ b/Extras/VHACD/src/vhacdMesh.cpp
@@ -0,0 +1,323 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#define _CRT_SECURE_NO_WARNINGS
+
+#include "btConvexHullComputer.h"
+#include "vhacdMesh.h"
+#include <fstream>
+#include <iosfwd>
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string>
+
+namespace VHACD {
+Mesh::Mesh()
+{
+ m_diag = 1.0;
+}
+Mesh::~Mesh()
+{
+}
+double Mesh::ComputeVolume() const
+{
+ const size_t nV = GetNPoints();
+ const size_t nT = GetNTriangles();
+ if (nV == 0 || nT == 0) {
+ return 0.0;
+ }
+
+ Vec3<double> bary(0.0, 0.0, 0.0);
+ for (size_t v = 0; v < nV; v++) {
+ bary += GetPoint(v);
+ }
+ bary /= static_cast<double>(nV);
+
+ Vec3<double> ver0, ver1, ver2;
+ double totalVolume = 0.0;
+ for (int t = 0; t < nT; t++) {
+ const Vec3<int>& tri = GetTriangle(t);
+ ver0 = GetPoint(tri[0]);
+ ver1 = GetPoint(tri[1]);
+ ver2 = GetPoint(tri[2]);
+ totalVolume += ComputeVolume4(ver0, ver1, ver2, bary);
+ }
+ return totalVolume / 6.0;
+}
+
+void Mesh::ComputeConvexHull(const double* const pts,
+ const size_t nPts)
+{
+ ResizePoints(0);
+ ResizeTriangles(0);
+ btConvexHullComputer ch;
+ ch.compute(pts, 3 * sizeof(double), (int)nPts, -1.0, -1.0);
+ for (int v = 0; v < ch.vertices.size(); v++) {
+ AddPoint(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ()));
+ }
+ const int nt = ch.faces.size();
+ for (int t = 0; t < nt; ++t) {
+ const btConvexHullComputer::Edge* sourceEdge = &(ch.edges[ch.faces[t]]);
+ int a = sourceEdge->getSourceVertex();
+ int b = sourceEdge->getTargetVertex();
+ const btConvexHullComputer::Edge* edge = sourceEdge->getNextEdgeOfFace();
+ int c = edge->getTargetVertex();
+ while (c != a) {
+ AddTriangle(Vec3<int>(a, b, c));
+ edge = edge->getNextEdgeOfFace();
+ b = c;
+ c = edge->getTargetVertex();
+ }
+ }
+}
+void Mesh::Clip(const Plane& plane,
+ SArray<Vec3<double> >& positivePart,
+ SArray<Vec3<double> >& negativePart) const
+{
+ const size_t nV = GetNPoints();
+ if (nV == 0) {
+ return;
+ }
+ double d;
+ for (size_t v = 0; v < nV; v++) {
+ const Vec3<double>& pt = GetPoint(v);
+ d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d;
+ if (d > 0.0) {
+ positivePart.PushBack(pt);
+ }
+ else if (d < 0.0) {
+ negativePart.PushBack(pt);
+ }
+ else {
+ positivePart.PushBack(pt);
+ negativePart.PushBack(pt);
+ }
+ }
+}
+bool Mesh::IsInside(const Vec3<double>& pt) const
+{
+ const size_t nV = GetNPoints();
+ const size_t nT = GetNTriangles();
+ if (nV == 0 || nT == 0) {
+ return false;
+ }
+ Vec3<double> ver0, ver1, ver2;
+ double volume;
+ for (int t = 0; t < nT; t++) {
+ const Vec3<int>& tri = GetTriangle(t);
+ ver0 = GetPoint(tri[0]);
+ ver1 = GetPoint(tri[1]);
+ ver2 = GetPoint(tri[2]);
+ volume = ComputeVolume4(ver0, ver1, ver2, pt);
+ if (volume < 0.0) {
+ return false;
+ }
+ }
+ return true;
+}
+double Mesh::ComputeDiagBB()
+{
+ const size_t nPoints = GetNPoints();
+ if (nPoints == 0)
+ return 0.0;
+ Vec3<double> minBB = m_points[0];
+ Vec3<double> maxBB = m_points[0];
+ double x, y, z;
+ for (size_t v = 1; v < nPoints; v++) {
+ x = m_points[v][0];
+ y = m_points[v][1];
+ z = m_points[v][2];
+ if (x < minBB[0])
+ minBB[0] = x;
+ else if (x > maxBB[0])
+ maxBB[0] = x;
+ if (y < minBB[1])
+ minBB[1] = y;
+ else if (y > maxBB[1])
+ maxBB[1] = y;
+ if (z < minBB[2])
+ minBB[2] = z;
+ else if (z > maxBB[2])
+ maxBB[2] = z;
+ }
+ return (m_diag = (maxBB - minBB).GetNorm());
+}
+
+#ifdef VHACD_DEBUG_MESH
+bool Mesh::SaveVRML2(const std::string& fileName) const
+{
+ std::ofstream fout(fileName.c_str());
+ if (fout.is_open()) {
+ const Material material;
+
+ if (SaveVRML2(fout, material)) {
+ fout.close();
+ return true;
+ }
+ return false;
+ }
+ return false;
+}
+bool Mesh::SaveVRML2(std::ofstream& fout, const Material& material) const
+{
+ if (fout.is_open()) {
+ fout.setf(std::ios::fixed, std::ios::floatfield);
+ fout.setf(std::ios::showpoint);
+ fout.precision(6);
+ size_t nV = m_points.Size();
+ size_t nT = m_triangles.Size();
+ fout << "#VRML V2.0 utf8" << std::endl;
+ fout << "" << std::endl;
+ fout << "# Vertices: " << nV << std::endl;
+ fout << "# Triangles: " << nT << std::endl;
+ fout << "" << std::endl;
+ fout << "Group {" << std::endl;
+ fout << " children [" << std::endl;
+ fout << " Shape {" << std::endl;
+ fout << " appearance Appearance {" << std::endl;
+ fout << " material Material {" << std::endl;
+ fout << " diffuseColor " << material.m_diffuseColor[0] << " "
+ << material.m_diffuseColor[1] << " "
+ << material.m_diffuseColor[2] << std::endl;
+ fout << " ambientIntensity " << material.m_ambientIntensity << std::endl;
+ fout << " specularColor " << material.m_specularColor[0] << " "
+ << material.m_specularColor[1] << " "
+ << material.m_specularColor[2] << std::endl;
+ fout << " emissiveColor " << material.m_emissiveColor[0] << " "
+ << material.m_emissiveColor[1] << " "
+ << material.m_emissiveColor[2] << std::endl;
+ fout << " shininess " << material.m_shininess << std::endl;
+ fout << " transparency " << material.m_transparency << std::endl;
+ fout << " }" << std::endl;
+ fout << " }" << std::endl;
+ fout << " geometry IndexedFaceSet {" << std::endl;
+ fout << " ccw TRUE" << std::endl;
+ fout << " solid TRUE" << std::endl;
+ fout << " convex TRUE" << std::endl;
+ if (nV > 0) {
+ fout << " coord DEF co Coordinate {" << std::endl;
+ fout << " point [" << std::endl;
+ for (size_t v = 0; v < nV; v++) {
+ fout << " " << m_points[v][0] << " "
+ << m_points[v][1] << " "
+ << m_points[v][2] << "," << std::endl;
+ }
+ fout << " ]" << std::endl;
+ fout << " }" << std::endl;
+ }
+ if (nT > 0) {
+ fout << " coordIndex [ " << std::endl;
+ for (size_t f = 0; f < nT; f++) {
+ fout << " " << m_triangles[f][0] << ", "
+ << m_triangles[f][1] << ", "
+ << m_triangles[f][2] << ", -1," << std::endl;
+ }
+ fout << " ]" << std::endl;
+ }
+ fout << " }" << std::endl;
+ fout << " }" << std::endl;
+ fout << " ]" << std::endl;
+ fout << "}" << std::endl;
+ return true;
+ }
+ return false;
+}
+bool Mesh::SaveOFF(const std::string& fileName) const
+{
+ std::ofstream fout(fileName.c_str());
+ if (fout.is_open()) {
+ size_t nV = m_points.Size();
+ size_t nT = m_triangles.Size();
+ fout << "OFF" << std::endl;
+ fout << nV << " " << nT << " " << 0 << std::endl;
+ for (size_t v = 0; v < nV; v++) {
+ fout << m_points[v][0] << " "
+ << m_points[v][1] << " "
+ << m_points[v][2] << std::endl;
+ }
+ for (size_t f = 0; f < nT; f++) {
+ fout << "3 " << m_triangles[f][0] << " "
+ << m_triangles[f][1] << " "
+ << m_triangles[f][2] << std::endl;
+ }
+ fout.close();
+ return true;
+ }
+ return false;
+}
+
+bool Mesh::LoadOFF(const std::string& fileName, bool invert)
+{
+ FILE* fid = fopen(fileName.c_str(), "r");
+ if (fid) {
+ const std::string strOFF("OFF");
+ char temp[1024];
+ fscanf(fid, "%s", temp);
+ if (std::string(temp) != strOFF) {
+ fclose(fid);
+ return false;
+ }
+ else {
+ int nv = 0;
+ int nf = 0;
+ int ne = 0;
+ fscanf(fid, "%i", &nv);
+ fscanf(fid, "%i", &nf);
+ fscanf(fid, "%i", &ne);
+ m_points.Resize(nv);
+ m_triangles.Resize(nf);
+ Vec3<double> coord;
+ float x, y, z;
+ for (int p = 0; p < nv; p++) {
+ fscanf(fid, "%f", &x);
+ fscanf(fid, "%f", &y);
+ fscanf(fid, "%f", &z);
+ m_points[p][0] = x;
+ m_points[p][1] = y;
+ m_points[p][2] = z;
+ }
+ int i, j, k, s;
+ for (int t = 0; t < nf; ++t) {
+ fscanf(fid, "%i", &s);
+ if (s == 3) {
+ fscanf(fid, "%i", &i);
+ fscanf(fid, "%i", &j);
+ fscanf(fid, "%i", &k);
+ m_triangles[t][0] = i;
+ if (invert) {
+ m_triangles[t][1] = k;
+ m_triangles[t][2] = j;
+ }
+ else {
+ m_triangles[t][1] = j;
+ m_triangles[t][2] = k;
+ }
+ }
+ else // Fix me: support only triangular meshes
+ {
+ for (int h = 0; h < s; ++h)
+ fscanf(fid, "%i", &s);
+ }
+ }
+ fclose(fid);
+ }
+ }
+ else {
+ return false;
+ }
+ return true;
+}
+#endif // VHACD_DEBUG_MESH
+}
diff --git a/Extras/VHACD/src/vhacdVolume.cpp b/Extras/VHACD/src/vhacdVolume.cpp
new file mode 100644
index 0000000..a3cf603
--- /dev/null
+++ b/Extras/VHACD/src/vhacdVolume.cpp
@@ -0,0 +1,1617 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+#define _CRT_SECURE_NO_WARNINGS
+#include "btConvexHullComputer.h"
+#include "vhacdVolume.h"
+#include <algorithm>
+#include <float.h>
+#include <math.h>
+#include <queue>
+#include <string.h>
+
+namespace VHACD {
+/********************************************************/
+/* AABB-triangle overlap test code */
+/* by Tomas Akenine-M�ller */
+/* Function: int triBoxOverlap(float boxcenter[3], */
+/* float boxhalfsize[3],float triverts[3][3]); */
+/* History: */
+/* 2001-03-05: released the code in its first version */
+/* 2001-06-18: changed the order of the tests, faster */
+/* */
+/* Acknowledgement: Many thanks to Pierre Terdiman for */
+/* suggestions and discussions on how to optimize code. */
+/* Thanks to David Hunt for finding a ">="-bug! */
+/********************************************************/
+
+#define X 0
+#define Y 1
+#define Z 2
+#define FINDMINMAX(x0, x1, x2, min, max) \
+ min = max = x0; \
+ if (x1 < min) \
+ min = x1; \
+ if (x1 > max) \
+ max = x1; \
+ if (x2 < min) \
+ min = x2; \
+ if (x2 > max) \
+ max = x2;
+
+#define AXISTEST_X01(a, b, fa, fb) \
+ p0 = a * v0[Y] - b * v0[Z]; \
+ p2 = a * v2[Y] - b * v2[Z]; \
+ if (p0 < p2) { \
+ min = p0; \
+ max = p2; \
+ } \
+ else { \
+ min = p2; \
+ max = p0; \
+ } \
+ rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \
+ if (min > rad || max < -rad) \
+ return 0;
+
+#define AXISTEST_X2(a, b, fa, fb) \
+ p0 = a * v0[Y] - b * v0[Z]; \
+ p1 = a * v1[Y] - b * v1[Z]; \
+ if (p0 < p1) { \
+ min = p0; \
+ max = p1; \
+ } \
+ else { \
+ min = p1; \
+ max = p0; \
+ } \
+ rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \
+ if (min > rad || max < -rad) \
+ return 0;
+
+#define AXISTEST_Y02(a, b, fa, fb) \
+ p0 = -a * v0[X] + b * v0[Z]; \
+ p2 = -a * v2[X] + b * v2[Z]; \
+ if (p0 < p2) { \
+ min = p0; \
+ max = p2; \
+ } \
+ else { \
+ min = p2; \
+ max = p0; \
+ } \
+ rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \
+ if (min > rad || max < -rad) \
+ return 0;
+
+#define AXISTEST_Y1(a, b, fa, fb) \
+ p0 = -a * v0[X] + b * v0[Z]; \
+ p1 = -a * v1[X] + b * v1[Z]; \
+ if (p0 < p1) { \
+ min = p0; \
+ max = p1; \
+ } \
+ else { \
+ min = p1; \
+ max = p0; \
+ } \
+ rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \
+ if (min > rad || max < -rad) \
+ return 0;
+
+#define AXISTEST_Z12(a, b, fa, fb) \
+ p1 = a * v1[X] - b * v1[Y]; \
+ p2 = a * v2[X] - b * v2[Y]; \
+ if (p2 < p1) { \
+ min = p2; \
+ max = p1; \
+ } \
+ else { \
+ min = p1; \
+ max = p2; \
+ } \
+ rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \
+ if (min > rad || max < -rad) \
+ return 0;
+
+#define AXISTEST_Z0(a, b, fa, fb) \
+ p0 = a * v0[X] - b * v0[Y]; \
+ p1 = a * v1[X] - b * v1[Y]; \
+ if (p0 < p1) { \
+ min = p0; \
+ max = p1; \
+ } \
+ else { \
+ min = p1; \
+ max = p0; \
+ } \
+ rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \
+ if (min > rad || max < -rad) \
+ return 0;
+
+int PlaneBoxOverlap(const Vec3<double>& normal,
+ const Vec3<double>& vert,
+ const Vec3<double>& maxbox)
+{
+ int q;
+ Vec3<double> vmin, vmax;
+ double v;
+ for (q = X; q <= Z; q++) {
+ v = vert[q];
+ if (normal[q] > 0.0) {
+ vmin[q] = -maxbox[q] - v;
+ vmax[q] = maxbox[q] - v;
+ }
+ else {
+ vmin[q] = maxbox[q] - v;
+ vmax[q] = -maxbox[q] - v;
+ }
+ }
+ if (normal * vmin > 0.0)
+ return 0;
+ if (normal * vmax >= 0.0)
+ return 1;
+ return 0;
+}
+
+int TriBoxOverlap(const Vec3<double>& boxcenter,
+ const Vec3<double>& boxhalfsize,
+ const Vec3<double>& triver0,
+ const Vec3<double>& triver1,
+ const Vec3<double>& triver2)
+{
+ /* use separating axis theorem to test overlap between triangle and box */
+ /* need to test for overlap in these directions: */
+ /* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
+ /* we do not even need to test these) */
+ /* 2) normal of the triangle */
+ /* 3) crossproduct(edge from tri, {x,y,z}-directin) */
+ /* this gives 3x3=9 more tests */
+
+ Vec3<double> v0, v1, v2;
+ double min, max, p0, p1, p2, rad, fex, fey, fez; // -NJMP- "d" local variable removed
+ Vec3<double> normal, e0, e1, e2;
+
+ /* This is the fastest branch on Sun */
+ /* move everything so that the boxcenter is in (0,0,0) */
+
+ v0 = triver0 - boxcenter;
+ v1 = triver1 - boxcenter;
+ v2 = triver2 - boxcenter;
+
+ /* compute triangle edges */
+ e0 = v1 - v0; /* tri edge 0 */
+ e1 = v2 - v1; /* tri edge 1 */
+ e2 = v0 - v2; /* tri edge 2 */
+
+ /* Bullet 3: */
+ /* test the 9 tests first (this was faster) */
+ fex = fabs(e0[X]);
+ fey = fabs(e0[Y]);
+ fez = fabs(e0[Z]);
+
+ AXISTEST_X01(e0[Z], e0[Y], fez, fey);
+ AXISTEST_Y02(e0[Z], e0[X], fez, fex);
+ AXISTEST_Z12(e0[Y], e0[X], fey, fex);
+
+ fex = fabs(e1[X]);
+ fey = fabs(e1[Y]);
+ fez = fabs(e1[Z]);
+
+ AXISTEST_X01(e1[Z], e1[Y], fez, fey);
+ AXISTEST_Y02(e1[Z], e1[X], fez, fex);
+ AXISTEST_Z0(e1[Y], e1[X], fey, fex);
+
+ fex = fabs(e2[X]);
+ fey = fabs(e2[Y]);
+ fez = fabs(e2[Z]);
+
+ AXISTEST_X2(e2[Z], e2[Y], fez, fey);
+ AXISTEST_Y1(e2[Z], e2[X], fez, fex);
+ AXISTEST_Z12(e2[Y], e2[X], fey, fex);
+
+ /* Bullet 1: */
+ /* first test overlap in the {x,y,z}-directions */
+ /* find min, max of the triangle each direction, and test for overlap in */
+ /* that direction -- this is equivalent to testing a minimal AABB around */
+ /* the triangle against the AABB */
+
+ /* test in X-direction */
+ FINDMINMAX(v0[X], v1[X], v2[X], min, max);
+ if (min > boxhalfsize[X] || max < -boxhalfsize[X])
+ return 0;
+
+ /* test in Y-direction */
+ FINDMINMAX(v0[Y], v1[Y], v2[Y], min, max);
+ if (min > boxhalfsize[Y] || max < -boxhalfsize[Y])
+ return 0;
+
+ /* test in Z-direction */
+ FINDMINMAX(v0[Z], v1[Z], v2[Z], min, max);
+ if (min > boxhalfsize[Z] || max < -boxhalfsize[Z])
+ return 0;
+
+ /* Bullet 2: */
+ /* test if the box intersects the plane of the triangle */
+ /* compute plane equation of triangle: normal*x+d=0 */
+ normal = e0 ^ e1;
+
+ if (!PlaneBoxOverlap(normal, v0, boxhalfsize))
+ return 0;
+ return 1; /* box and triangle overlaps */
+}
+
+// Slightly modified version of Stan Melax's code for 3x3 matrix diagonalization (Thanks Stan!)
+// source: http://www.melax.com/diag.html?attredirects=0
+void Diagonalize(const double (&A)[3][3], double (&Q)[3][3], double (&D)[3][3])
+{
+ // A must be a symmetric matrix.
+ // returns Q and D such that
+ // Diagonal matrix D = QT * A * Q; and A = Q*D*QT
+ const int maxsteps = 24; // certainly wont need that many.
+ int k0, k1, k2;
+ double o[3], m[3];
+ double q[4] = { 0.0, 0.0, 0.0, 1.0 };
+ double jr[4];
+ double sqw, sqx, sqy, sqz;
+ double tmp1, tmp2, mq;
+ double AQ[3][3];
+ double thet, sgn, t, c;
+ for (int i = 0; i < maxsteps; ++i) {
+ // quat to matrix
+ sqx = q[0] * q[0];
+ sqy = q[1] * q[1];
+ sqz = q[2] * q[2];
+ sqw = q[3] * q[3];
+ Q[0][0] = (sqx - sqy - sqz + sqw);
+ Q[1][1] = (-sqx + sqy - sqz + sqw);
+ Q[2][2] = (-sqx - sqy + sqz + sqw);
+ tmp1 = q[0] * q[1];
+ tmp2 = q[2] * q[3];
+ Q[1][0] = 2.0 * (tmp1 + tmp2);
+ Q[0][1] = 2.0 * (tmp1 - tmp2);
+ tmp1 = q[0] * q[2];
+ tmp2 = q[1] * q[3];
+ Q[2][0] = 2.0 * (tmp1 - tmp2);
+ Q[0][2] = 2.0 * (tmp1 + tmp2);
+ tmp1 = q[1] * q[2];
+ tmp2 = q[0] * q[3];
+ Q[2][1] = 2.0 * (tmp1 + tmp2);
+ Q[1][2] = 2.0 * (tmp1 - tmp2);
+
+ // AQ = A * Q
+ AQ[0][0] = Q[0][0] * A[0][0] + Q[1][0] * A[0][1] + Q[2][0] * A[0][2];
+ AQ[0][1] = Q[0][1] * A[0][0] + Q[1][1] * A[0][1] + Q[2][1] * A[0][2];
+ AQ[0][2] = Q[0][2] * A[0][0] + Q[1][2] * A[0][1] + Q[2][2] * A[0][2];
+ AQ[1][0] = Q[0][0] * A[0][1] + Q[1][0] * A[1][1] + Q[2][0] * A[1][2];
+ AQ[1][1] = Q[0][1] * A[0][1] + Q[1][1] * A[1][1] + Q[2][1] * A[1][2];
+ AQ[1][2] = Q[0][2] * A[0][1] + Q[1][2] * A[1][1] + Q[2][2] * A[1][2];
+ AQ[2][0] = Q[0][0] * A[0][2] + Q[1][0] * A[1][2] + Q[2][0] * A[2][2];
+ AQ[2][1] = Q[0][1] * A[0][2] + Q[1][1] * A[1][2] + Q[2][1] * A[2][2];
+ AQ[2][2] = Q[0][2] * A[0][2] + Q[1][2] * A[1][2] + Q[2][2] * A[2][2];
+ // D = Qt * AQ
+ D[0][0] = AQ[0][0] * Q[0][0] + AQ[1][0] * Q[1][0] + AQ[2][0] * Q[2][0];
+ D[0][1] = AQ[0][0] * Q[0][1] + AQ[1][0] * Q[1][1] + AQ[2][0] * Q[2][1];
+ D[0][2] = AQ[0][0] * Q[0][2] + AQ[1][0] * Q[1][2] + AQ[2][0] * Q[2][2];
+ D[1][0] = AQ[0][1] * Q[0][0] + AQ[1][1] * Q[1][0] + AQ[2][1] * Q[2][0];
+ D[1][1] = AQ[0][1] * Q[0][1] + AQ[1][1] * Q[1][1] + AQ[2][1] * Q[2][1];
+ D[1][2] = AQ[0][1] * Q[0][2] + AQ[1][1] * Q[1][2] + AQ[2][1] * Q[2][2];
+ D[2][0] = AQ[0][2] * Q[0][0] + AQ[1][2] * Q[1][0] + AQ[2][2] * Q[2][0];
+ D[2][1] = AQ[0][2] * Q[0][1] + AQ[1][2] * Q[1][1] + AQ[2][2] * Q[2][1];
+ D[2][2] = AQ[0][2] * Q[0][2] + AQ[1][2] * Q[1][2] + AQ[2][2] * Q[2][2];
+ o[0] = D[1][2];
+ o[1] = D[0][2];
+ o[2] = D[0][1];
+ m[0] = fabs(o[0]);
+ m[1] = fabs(o[1]);
+ m[2] = fabs(o[2]);
+
+ k0 = (m[0] > m[1] && m[0] > m[2]) ? 0 : (m[1] > m[2]) ? 1 : 2; // index of largest element of offdiag
+ k1 = (k0 + 1) % 3;
+ k2 = (k0 + 2) % 3;
+ if (o[k0] == 0.0) {
+ break; // diagonal already
+ }
+ thet = (D[k2][k2] - D[k1][k1]) / (2.0 * o[k0]);
+ sgn = (thet > 0.0) ? 1.0 : -1.0;
+ thet *= sgn; // make it positive
+ t = sgn / (thet + ((thet < 1.E6) ? sqrt(thet * thet + 1.0) : thet)); // sign(T)/(|T|+sqrt(T^2+1))
+ c = 1.0 / sqrt(t * t + 1.0); // c= 1/(t^2+1) , t=s/c
+ if (c == 1.0) {
+ break; // no room for improvement - reached machine precision.
+ }
+ jr[0] = jr[1] = jr[2] = jr[3] = 0.0;
+ jr[k0] = sgn * sqrt((1.0 - c) / 2.0); // using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
+ jr[k0] *= -1.0; // since our quat-to-matrix convention was for v*M instead of M*v
+ jr[3] = sqrt(1.0 - jr[k0] * jr[k0]);
+ if (jr[3] == 1.0) {
+ break; // reached limits of floating point precision
+ }
+ q[0] = (q[3] * jr[0] + q[0] * jr[3] + q[1] * jr[2] - q[2] * jr[1]);
+ q[1] = (q[3] * jr[1] - q[0] * jr[2] + q[1] * jr[3] + q[2] * jr[0]);
+ q[2] = (q[3] * jr[2] + q[0] * jr[1] - q[1] * jr[0] + q[2] * jr[3]);
+ q[3] = (q[3] * jr[3] - q[0] * jr[0] - q[1] * jr[1] - q[2] * jr[2]);
+ mq = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+ q[0] /= mq;
+ q[1] /= mq;
+ q[2] /= mq;
+ q[3] /= mq;
+ }
+}
+const double TetrahedronSet::EPS = 0.0000000000001;
+VoxelSet::VoxelSet()
+{
+ m_minBB[0] = m_minBB[1] = m_minBB[2] = 0.0;
+ m_minBBVoxels[0] = m_minBBVoxels[1] = m_minBBVoxels[2] = 0;
+ m_maxBBVoxels[0] = m_maxBBVoxels[1] = m_maxBBVoxels[2] = 1;
+ m_minBBPts[0] = m_minBBPts[1] = m_minBBPts[2] = 0;
+ m_maxBBPts[0] = m_maxBBPts[1] = m_maxBBPts[2] = 1;
+ m_barycenter[0] = m_barycenter[1] = m_barycenter[2] = 0;
+ m_barycenterPCA[0] = m_barycenterPCA[1] = m_barycenterPCA[2] = 0.0;
+ m_scale = 1.0;
+ m_unitVolume = 1.0;
+ m_numVoxelsOnSurface = 0;
+ m_numVoxelsInsideSurface = 0;
+ memset(m_Q, 0, sizeof(double) * 9);
+ memset(m_D, 0, sizeof(double) * 9);
+}
+VoxelSet::~VoxelSet(void)
+{
+}
+void VoxelSet::ComputeBB()
+{
+ const size_t nVoxels = m_voxels.Size();
+ if (nVoxels == 0)
+ return;
+ for (int h = 0; h < 3; ++h) {
+ m_minBBVoxels[h] = m_voxels[0].m_coord[h];
+ m_maxBBVoxels[h] = m_voxels[0].m_coord[h];
+ }
+ Vec3<double> bary(0.0);
+ for (size_t p = 0; p < nVoxels; ++p) {
+ for (int h = 0; h < 3; ++h) {
+ bary[h] += m_voxels[p].m_coord[h];
+ if (m_minBBVoxels[h] > m_voxels[p].m_coord[h])
+ m_minBBVoxels[h] = m_voxels[p].m_coord[h];
+ if (m_maxBBVoxels[h] < m_voxels[p].m_coord[h])
+ m_maxBBVoxels[h] = m_voxels[p].m_coord[h];
+ }
+ }
+ bary /= (double)nVoxels;
+ for (int h = 0; h < 3; ++h) {
+ m_minBBPts[h] = m_minBBVoxels[h] * m_scale + m_minBB[h];
+ m_maxBBPts[h] = m_maxBBVoxels[h] * m_scale + m_minBB[h];
+ m_barycenter[h] = (short)(bary[h] + 0.5);
+ }
+}
+void VoxelSet::ComputeConvexHull(Mesh& meshCH, const size_t sampling) const
+{
+ const size_t CLUSTER_SIZE = 65536;
+ const size_t nVoxels = m_voxels.Size();
+ if (nVoxels == 0)
+ return;
+
+ SArray<Vec3<double> > cpoints;
+
+ Vec3<double>* points = new Vec3<double>[CLUSTER_SIZE];
+ size_t p = 0;
+ size_t s = 0;
+ short i, j, k;
+ while (p < nVoxels) {
+ size_t q = 0;
+ while (q < CLUSTER_SIZE && p < nVoxels) {
+ if (m_voxels[p].m_data == PRIMITIVE_ON_SURFACE) {
+ ++s;
+ if (s == sampling) {
+ s = 0;
+ i = m_voxels[p].m_coord[0];
+ j = m_voxels[p].m_coord[1];
+ k = m_voxels[p].m_coord[2];
+ Vec3<double> p0((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale);
+ Vec3<double> p1((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale);
+ Vec3<double> p2((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale);
+ Vec3<double> p3((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale);
+ Vec3<double> p4((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale);
+ Vec3<double> p5((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale);
+ Vec3<double> p6((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale);
+ Vec3<double> p7((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale);
+ points[q++] = p0 + m_minBB;
+ points[q++] = p1 + m_minBB;
+ points[q++] = p2 + m_minBB;
+ points[q++] = p3 + m_minBB;
+ points[q++] = p4 + m_minBB;
+ points[q++] = p5 + m_minBB;
+ points[q++] = p6 + m_minBB;
+ points[q++] = p7 + m_minBB;
+ }
+ }
+ ++p;
+ }
+ btConvexHullComputer ch;
+ ch.compute((double*)points, 3 * sizeof(double), (int)q, -1.0, -1.0);
+ for (int v = 0; v < ch.vertices.size(); v++) {
+ cpoints.PushBack(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ()));
+ }
+ }
+ delete[] points;
+
+ points = cpoints.Data();
+ btConvexHullComputer ch;
+ ch.compute((double*)points, 3 * sizeof(double), (int)cpoints.Size(), -1.0, -1.0);
+ meshCH.ResizePoints(0);
+ meshCH.ResizeTriangles(0);
+ for (int v = 0; v < ch.vertices.size(); v++) {
+ meshCH.AddPoint(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ()));
+ }
+ const int nt = ch.faces.size();
+ for (int t = 0; t < nt; ++t) {
+ const btConvexHullComputer::Edge* sourceEdge = &(ch.edges[ch.faces[t]]);
+ int a = sourceEdge->getSourceVertex();
+ int b = sourceEdge->getTargetVertex();
+ const btConvexHullComputer::Edge* edge = sourceEdge->getNextEdgeOfFace();
+ int c = edge->getTargetVertex();
+ while (c != a) {
+ meshCH.AddTriangle(Vec3<int>(a, b, c));
+ edge = edge->getNextEdgeOfFace();
+ b = c;
+ c = edge->getTargetVertex();
+ }
+ }
+}
+void VoxelSet::GetPoints(const Voxel& voxel,
+ Vec3<double>* const pts) const
+{
+ short i = voxel.m_coord[0];
+ short j = voxel.m_coord[1];
+ short k = voxel.m_coord[2];
+ pts[0][0] = (i - 0.5) * m_scale + m_minBB[0];
+ pts[1][0] = (i + 0.5) * m_scale + m_minBB[0];
+ pts[2][0] = (i + 0.5) * m_scale + m_minBB[0];
+ pts[3][0] = (i - 0.5) * m_scale + m_minBB[0];
+ pts[4][0] = (i - 0.5) * m_scale + m_minBB[0];
+ pts[5][0] = (i + 0.5) * m_scale + m_minBB[0];
+ pts[6][0] = (i + 0.5) * m_scale + m_minBB[0];
+ pts[7][0] = (i - 0.5) * m_scale + m_minBB[0];
+ pts[0][1] = (j - 0.5) * m_scale + m_minBB[1];
+ pts[1][1] = (j - 0.5) * m_scale + m_minBB[1];
+ pts[2][1] = (j + 0.5) * m_scale + m_minBB[1];
+ pts[3][1] = (j + 0.5) * m_scale + m_minBB[1];
+ pts[4][1] = (j - 0.5) * m_scale + m_minBB[1];
+ pts[5][1] = (j - 0.5) * m_scale + m_minBB[1];
+ pts[6][1] = (j + 0.5) * m_scale + m_minBB[1];
+ pts[7][1] = (j + 0.5) * m_scale + m_minBB[1];
+ pts[0][2] = (k - 0.5) * m_scale + m_minBB[2];
+ pts[1][2] = (k - 0.5) * m_scale + m_minBB[2];
+ pts[2][2] = (k - 0.5) * m_scale + m_minBB[2];
+ pts[3][2] = (k - 0.5) * m_scale + m_minBB[2];
+ pts[4][2] = (k + 0.5) * m_scale + m_minBB[2];
+ pts[5][2] = (k + 0.5) * m_scale + m_minBB[2];
+ pts[6][2] = (k + 0.5) * m_scale + m_minBB[2];
+ pts[7][2] = (k + 0.5) * m_scale + m_minBB[2];
+}
+void VoxelSet::Intersect(const Plane& plane,
+ SArray<Vec3<double> >* const positivePts,
+ SArray<Vec3<double> >* const negativePts,
+ const size_t sampling) const
+{
+ const size_t nVoxels = m_voxels.Size();
+ if (nVoxels == 0)
+ return;
+ const double d0 = m_scale;
+ double d;
+ Vec3<double> pts[8];
+ Vec3<double> pt;
+ Voxel voxel;
+ size_t sp = 0;
+ size_t sn = 0;
+ for (size_t v = 0; v < nVoxels; ++v) {
+ voxel = m_voxels[v];
+ pt = GetPoint(voxel);
+ d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d;
+ // if (d >= 0.0 && d <= d0) positivePts->PushBack(pt);
+ // else if (d < 0.0 && -d <= d0) negativePts->PushBack(pt);
+ if (d >= 0.0) {
+ if (d <= d0) {
+ GetPoints(voxel, pts);
+ for (int k = 0; k < 8; ++k) {
+ positivePts->PushBack(pts[k]);
+ }
+ }
+ else {
+ if (++sp == sampling) {
+ // positivePts->PushBack(pt);
+ GetPoints(voxel, pts);
+ for (int k = 0; k < 8; ++k) {
+ positivePts->PushBack(pts[k]);
+ }
+ sp = 0;
+ }
+ }
+ }
+ else {
+ if (-d <= d0) {
+ GetPoints(voxel, pts);
+ for (int k = 0; k < 8; ++k) {
+ negativePts->PushBack(pts[k]);
+ }
+ }
+ else {
+ if (++sn == sampling) {
+ // negativePts->PushBack(pt);
+ GetPoints(voxel, pts);
+ for (int k = 0; k < 8; ++k) {
+ negativePts->PushBack(pts[k]);
+ }
+ sn = 0;
+ }
+ }
+ }
+ }
+}
+void VoxelSet::ComputeExteriorPoints(const Plane& plane,
+ const Mesh& mesh,
+ SArray<Vec3<double> >* const exteriorPts) const
+{
+ const size_t nVoxels = m_voxels.Size();
+ if (nVoxels == 0)
+ return;
+ double d;
+ Vec3<double> pt;
+ Vec3<double> pts[8];
+ Voxel voxel;
+ for (size_t v = 0; v < nVoxels; ++v) {
+ voxel = m_voxels[v];
+ pt = GetPoint(voxel);
+ d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d;
+ if (d >= 0.0) {
+ if (!mesh.IsInside(pt)) {
+ GetPoints(voxel, pts);
+ for (int k = 0; k < 8; ++k) {
+ exteriorPts->PushBack(pts[k]);
+ }
+ }
+ }
+ }
+}
+void VoxelSet::ComputeClippedVolumes(const Plane& plane,
+ double& positiveVolume,
+ double& negativeVolume) const
+{
+ negativeVolume = 0.0;
+ positiveVolume = 0.0;
+ const size_t nVoxels = m_voxels.Size();
+ if (nVoxels == 0)
+ return;
+ double d;
+ Vec3<double> pt;
+ size_t nPositiveVoxels = 0;
+ for (size_t v = 0; v < nVoxels; ++v) {
+ pt = GetPoint(m_voxels[v]);
+ d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d;
+ nPositiveVoxels += (d >= 0.0);
+ }
+ size_t nNegativeVoxels = nVoxels - nPositiveVoxels;
+ positiveVolume = m_unitVolume * nPositiveVoxels;
+ negativeVolume = m_unitVolume * nNegativeVoxels;
+}
+void VoxelSet::SelectOnSurface(PrimitiveSet* const onSurfP) const
+{
+ VoxelSet* const onSurf = (VoxelSet*)onSurfP;
+ const size_t nVoxels = m_voxels.Size();
+ if (nVoxels == 0)
+ return;
+
+ for (int h = 0; h < 3; ++h) {
+ onSurf->m_minBB[h] = m_minBB[h];
+ }
+ onSurf->m_voxels.Resize(0);
+ onSurf->m_scale = m_scale;
+ onSurf->m_unitVolume = m_unitVolume;
+ onSurf->m_numVoxelsOnSurface = 0;
+ onSurf->m_numVoxelsInsideSurface = 0;
+ Voxel voxel;
+ for (size_t v = 0; v < nVoxels; ++v) {
+ voxel = m_voxels[v];
+ if (voxel.m_data == PRIMITIVE_ON_SURFACE) {
+ onSurf->m_voxels.PushBack(voxel);
+ ++onSurf->m_numVoxelsOnSurface;
+ }
+ }
+}
+void VoxelSet::Clip(const Plane& plane,
+ PrimitiveSet* const positivePartP,
+ PrimitiveSet* const negativePartP) const
+{
+ VoxelSet* const positivePart = (VoxelSet*)positivePartP;
+ VoxelSet* const negativePart = (VoxelSet*)negativePartP;
+ const size_t nVoxels = m_voxels.Size();
+ if (nVoxels == 0)
+ return;
+
+ for (int h = 0; h < 3; ++h) {
+ negativePart->m_minBB[h] = positivePart->m_minBB[h] = m_minBB[h];
+ }
+ positivePart->m_voxels.Resize(0);
+ negativePart->m_voxels.Resize(0);
+ positivePart->m_voxels.Allocate(nVoxels);
+ negativePart->m_voxels.Allocate(nVoxels);
+ negativePart->m_scale = positivePart->m_scale = m_scale;
+ negativePart->m_unitVolume = positivePart->m_unitVolume = m_unitVolume;
+ negativePart->m_numVoxelsOnSurface = positivePart->m_numVoxelsOnSurface = 0;
+ negativePart->m_numVoxelsInsideSurface = positivePart->m_numVoxelsInsideSurface = 0;
+
+ double d;
+ Vec3<double> pt;
+ Voxel voxel;
+ const double d0 = m_scale;
+ for (size_t v = 0; v < nVoxels; ++v) {
+ voxel = m_voxels[v];
+ pt = GetPoint(voxel);
+ d = plane.m_a * pt[0] + plane.m_b * pt[1] + plane.m_c * pt[2] + plane.m_d;
+ if (d >= 0.0) {
+ if (voxel.m_data == PRIMITIVE_ON_SURFACE || d <= d0) {
+ voxel.m_data = PRIMITIVE_ON_SURFACE;
+ positivePart->m_voxels.PushBack(voxel);
+ ++positivePart->m_numVoxelsOnSurface;
+ }
+ else {
+ positivePart->m_voxels.PushBack(voxel);
+ ++positivePart->m_numVoxelsInsideSurface;
+ }
+ }
+ else {
+ if (voxel.m_data == PRIMITIVE_ON_SURFACE || -d <= d0) {
+ voxel.m_data = PRIMITIVE_ON_SURFACE;
+ negativePart->m_voxels.PushBack(voxel);
+ ++negativePart->m_numVoxelsOnSurface;
+ }
+ else {
+ negativePart->m_voxels.PushBack(voxel);
+ ++negativePart->m_numVoxelsInsideSurface;
+ }
+ }
+ }
+}
+void VoxelSet::Convert(Mesh& mesh, const VOXEL_VALUE value) const
+{
+ const size_t nVoxels = m_voxels.Size();
+ if (nVoxels == 0)
+ return;
+ Voxel voxel;
+ Vec3<double> pts[8];
+ for (size_t v = 0; v < nVoxels; ++v) {
+ voxel = m_voxels[v];
+ if (voxel.m_data == value) {
+ GetPoints(voxel, pts);
+ int s = (int)mesh.GetNPoints();
+ for (int k = 0; k < 8; ++k) {
+ mesh.AddPoint(pts[k]);
+ }
+ mesh.AddTriangle(Vec3<int>(s + 0, s + 2, s + 1));
+ mesh.AddTriangle(Vec3<int>(s + 0, s + 3, s + 2));
+ mesh.AddTriangle(Vec3<int>(s + 4, s + 5, s + 6));
+ mesh.AddTriangle(Vec3<int>(s + 4, s + 6, s + 7));
+ mesh.AddTriangle(Vec3<int>(s + 7, s + 6, s + 2));
+ mesh.AddTriangle(Vec3<int>(s + 7, s + 2, s + 3));
+ mesh.AddTriangle(Vec3<int>(s + 4, s + 1, s + 5));
+ mesh.AddTriangle(Vec3<int>(s + 4, s + 0, s + 1));
+ mesh.AddTriangle(Vec3<int>(s + 6, s + 5, s + 1));
+ mesh.AddTriangle(Vec3<int>(s + 6, s + 1, s + 2));
+ mesh.AddTriangle(Vec3<int>(s + 7, s + 0, s + 4));
+ mesh.AddTriangle(Vec3<int>(s + 7, s + 3, s + 0));
+ }
+ }
+}
+void VoxelSet::ComputePrincipalAxes()
+{
+ const size_t nVoxels = m_voxels.Size();
+ if (nVoxels == 0)
+ return;
+ m_barycenterPCA[0] = m_barycenterPCA[1] = m_barycenterPCA[2] = 0.0;
+ for (size_t v = 0; v < nVoxels; ++v) {
+ Voxel& voxel = m_voxels[v];
+ m_barycenterPCA[0] += voxel.m_coord[0];
+ m_barycenterPCA[1] += voxel.m_coord[1];
+ m_barycenterPCA[2] += voxel.m_coord[2];
+ }
+ m_barycenterPCA /= (double)nVoxels;
+
+ double covMat[3][3] = { { 0.0, 0.0, 0.0 },
+ { 0.0, 0.0, 0.0 },
+ { 0.0, 0.0, 0.0 } };
+ double x, y, z;
+ for (size_t v = 0; v < nVoxels; ++v) {
+ Voxel& voxel = m_voxels[v];
+ x = voxel.m_coord[0] - m_barycenter[0];
+ y = voxel.m_coord[1] - m_barycenter[1];
+ z = voxel.m_coord[2] - m_barycenter[2];
+ covMat[0][0] += x * x;
+ covMat[1][1] += y * y;
+ covMat[2][2] += z * z;
+ covMat[0][1] += x * y;
+ covMat[0][2] += x * z;
+ covMat[1][2] += y * z;
+ }
+ covMat[0][0] /= nVoxels;
+ covMat[1][1] /= nVoxels;
+ covMat[2][2] /= nVoxels;
+ covMat[0][1] /= nVoxels;
+ covMat[0][2] /= nVoxels;
+ covMat[1][2] /= nVoxels;
+ covMat[1][0] = covMat[0][1];
+ covMat[2][0] = covMat[0][2];
+ covMat[2][1] = covMat[1][2];
+ Diagonalize(covMat, m_Q, m_D);
+}
+Volume::Volume()
+{
+ m_dim[0] = m_dim[1] = m_dim[2] = 0;
+ m_minBB[0] = m_minBB[1] = m_minBB[2] = 0.0;
+ m_maxBB[0] = m_maxBB[1] = m_maxBB[2] = 1.0;
+ m_numVoxelsOnSurface = 0;
+ m_numVoxelsInsideSurface = 0;
+ m_numVoxelsOutsideSurface = 0;
+ m_scale = 1.0;
+ m_data = 0;
+}
+Volume::~Volume(void)
+{
+ delete[] m_data;
+}
+void Volume::Allocate()
+{
+ delete[] m_data;
+ size_t size = m_dim[0] * m_dim[1] * m_dim[2];
+ m_data = new unsigned char[size];
+ memset(m_data, PRIMITIVE_UNDEFINED, sizeof(unsigned char) * size);
+}
+void Volume::Free()
+{
+ delete[] m_data;
+ m_data = 0;
+}
+void Volume::FillOutsideSurface(const size_t i0,
+ const size_t j0,
+ const size_t k0,
+ const size_t i1,
+ const size_t j1,
+ const size_t k1)
+{
+ const short neighbours[6][3] = { { 1, 0, 0 },
+ { 0, 1, 0 },
+ { 0, 0, 1 },
+ { -1, 0, 0 },
+ { 0, -1, 0 },
+ { 0, 0, -1 } };
+ std::queue<Vec3<short> > fifo;
+ Vec3<short> current;
+ short a, b, c;
+ for (size_t i = i0; i < i1; ++i) {
+ for (size_t j = j0; j < j1; ++j) {
+ for (size_t k = k0; k < k1; ++k) {
+
+ if (GetVoxel(i, j, k) == PRIMITIVE_UNDEFINED) {
+ current[0] = (short)i;
+ current[1] = (short)j;
+ current[2] = (short)k;
+ fifo.push(current);
+ GetVoxel(current[0], current[1], current[2]) = PRIMITIVE_OUTSIDE_SURFACE;
+ ++m_numVoxelsOutsideSurface;
+ while (fifo.size() > 0) {
+ current = fifo.front();
+ fifo.pop();
+ for (int h = 0; h < 6; ++h) {
+ a = current[0] + neighbours[h][0];
+ b = current[1] + neighbours[h][1];
+ c = current[2] + neighbours[h][2];
+ if (a < 0 || a >= (int)m_dim[0] || b < 0 || b >= (int)m_dim[1] || c < 0 || c >= (int)m_dim[2]) {
+ continue;
+ }
+ unsigned char& v = GetVoxel(a, b, c);
+ if (v == PRIMITIVE_UNDEFINED) {
+ v = PRIMITIVE_OUTSIDE_SURFACE;
+ ++m_numVoxelsOutsideSurface;
+ fifo.push(Vec3<short>(a, b, c));
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+}
+void Volume::FillInsideSurface()
+{
+ const size_t i0 = m_dim[0];
+ const size_t j0 = m_dim[1];
+ const size_t k0 = m_dim[2];
+ for (size_t i = 0; i < i0; ++i) {
+ for (size_t j = 0; j < j0; ++j) {
+ for (size_t k = 0; k < k0; ++k) {
+ unsigned char& v = GetVoxel(i, j, k);
+ if (v == PRIMITIVE_UNDEFINED) {
+ v = PRIMITIVE_INSIDE_SURFACE;
+ ++m_numVoxelsInsideSurface;
+ }
+ }
+ }
+ }
+}
+void Volume::Convert(Mesh& mesh, const VOXEL_VALUE value) const
+{
+ const size_t i0 = m_dim[0];
+ const size_t j0 = m_dim[1];
+ const size_t k0 = m_dim[2];
+ for (size_t i = 0; i < i0; ++i) {
+ for (size_t j = 0; j < j0; ++j) {
+ for (size_t k = 0; k < k0; ++k) {
+ const unsigned char& voxel = GetVoxel(i, j, k);
+ if (voxel == value) {
+ Vec3<double> p0((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale);
+ Vec3<double> p1((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k - 0.5) * m_scale);
+ Vec3<double> p2((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale);
+ Vec3<double> p3((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k - 0.5) * m_scale);
+ Vec3<double> p4((i - 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale);
+ Vec3<double> p5((i + 0.5) * m_scale, (j - 0.5) * m_scale, (k + 0.5) * m_scale);
+ Vec3<double> p6((i + 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale);
+ Vec3<double> p7((i - 0.5) * m_scale, (j + 0.5) * m_scale, (k + 0.5) * m_scale);
+ int s = (int)mesh.GetNPoints();
+ mesh.AddPoint(p0 + m_minBB);
+ mesh.AddPoint(p1 + m_minBB);
+ mesh.AddPoint(p2 + m_minBB);
+ mesh.AddPoint(p3 + m_minBB);
+ mesh.AddPoint(p4 + m_minBB);
+ mesh.AddPoint(p5 + m_minBB);
+ mesh.AddPoint(p6 + m_minBB);
+ mesh.AddPoint(p7 + m_minBB);
+ mesh.AddTriangle(Vec3<int>(s + 0, s + 2, s + 1));
+ mesh.AddTriangle(Vec3<int>(s + 0, s + 3, s + 2));
+ mesh.AddTriangle(Vec3<int>(s + 4, s + 5, s + 6));
+ mesh.AddTriangle(Vec3<int>(s + 4, s + 6, s + 7));
+ mesh.AddTriangle(Vec3<int>(s + 7, s + 6, s + 2));
+ mesh.AddTriangle(Vec3<int>(s + 7, s + 2, s + 3));
+ mesh.AddTriangle(Vec3<int>(s + 4, s + 1, s + 5));
+ mesh.AddTriangle(Vec3<int>(s + 4, s + 0, s + 1));
+ mesh.AddTriangle(Vec3<int>(s + 6, s + 5, s + 1));
+ mesh.AddTriangle(Vec3<int>(s + 6, s + 1, s + 2));
+ mesh.AddTriangle(Vec3<int>(s + 7, s + 0, s + 4));
+ mesh.AddTriangle(Vec3<int>(s + 7, s + 3, s + 0));
+ }
+ }
+ }
+ }
+}
+void Volume::Convert(VoxelSet& vset) const
+{
+ for (int h = 0; h < 3; ++h) {
+ vset.m_minBB[h] = m_minBB[h];
+ }
+ vset.m_voxels.Allocate(m_numVoxelsInsideSurface + m_numVoxelsOnSurface);
+ vset.m_scale = m_scale;
+ vset.m_unitVolume = m_scale * m_scale * m_scale;
+ const short i0 = (short)m_dim[0];
+ const short j0 = (short)m_dim[1];
+ const short k0 = (short)m_dim[2];
+ Voxel voxel;
+ vset.m_numVoxelsOnSurface = 0;
+ vset.m_numVoxelsInsideSurface = 0;
+ for (short i = 0; i < i0; ++i) {
+ for (short j = 0; j < j0; ++j) {
+ for (short k = 0; k < k0; ++k) {
+ const unsigned char& value = GetVoxel(i, j, k);
+ if (value == PRIMITIVE_INSIDE_SURFACE) {
+ voxel.m_coord[0] = i;
+ voxel.m_coord[1] = j;
+ voxel.m_coord[2] = k;
+ voxel.m_data = PRIMITIVE_INSIDE_SURFACE;
+ vset.m_voxels.PushBack(voxel);
+ ++vset.m_numVoxelsInsideSurface;
+ }
+ else if (value == PRIMITIVE_ON_SURFACE) {
+ voxel.m_coord[0] = i;
+ voxel.m_coord[1] = j;
+ voxel.m_coord[2] = k;
+ voxel.m_data = PRIMITIVE_ON_SURFACE;
+ vset.m_voxels.PushBack(voxel);
+ ++vset.m_numVoxelsOnSurface;
+ }
+ }
+ }
+ }
+}
+
+void Volume::Convert(TetrahedronSet& tset) const
+{
+ tset.m_tetrahedra.Allocate(5 * (m_numVoxelsInsideSurface + m_numVoxelsOnSurface));
+ tset.m_scale = m_scale;
+ const short i0 = (short)m_dim[0];
+ const short j0 = (short)m_dim[1];
+ const short k0 = (short)m_dim[2];
+ tset.m_numTetrahedraOnSurface = 0;
+ tset.m_numTetrahedraInsideSurface = 0;
+ Tetrahedron tetrahedron;
+ for (short i = 0; i < i0; ++i) {
+ for (short j = 0; j < j0; ++j) {
+ for (short k = 0; k < k0; ++k) {
+ const unsigned char& value = GetVoxel(i, j, k);
+ if (value == PRIMITIVE_INSIDE_SURFACE || value == PRIMITIVE_ON_SURFACE) {
+ tetrahedron.m_data = value;
+ Vec3<double> p1((i - 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]);
+ Vec3<double> p2((i + 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]);
+ Vec3<double> p3((i + 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]);
+ Vec3<double> p4((i - 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k - 0.5) * m_scale + m_minBB[2]);
+ Vec3<double> p5((i - 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]);
+ Vec3<double> p6((i + 0.5) * m_scale + m_minBB[0], (j - 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]);
+ Vec3<double> p7((i + 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]);
+ Vec3<double> p8((i - 0.5) * m_scale + m_minBB[0], (j + 0.5) * m_scale + m_minBB[1], (k + 0.5) * m_scale + m_minBB[2]);
+
+ tetrahedron.m_pts[0] = p2;
+ tetrahedron.m_pts[1] = p4;
+ tetrahedron.m_pts[2] = p7;
+ tetrahedron.m_pts[3] = p5;
+ tset.m_tetrahedra.PushBack(tetrahedron);
+
+ tetrahedron.m_pts[0] = p6;
+ tetrahedron.m_pts[1] = p2;
+ tetrahedron.m_pts[2] = p7;
+ tetrahedron.m_pts[3] = p5;
+ tset.m_tetrahedra.PushBack(tetrahedron);
+
+ tetrahedron.m_pts[0] = p3;
+ tetrahedron.m_pts[1] = p4;
+ tetrahedron.m_pts[2] = p7;
+ tetrahedron.m_pts[3] = p2;
+ tset.m_tetrahedra.PushBack(tetrahedron);
+
+ tetrahedron.m_pts[0] = p1;
+ tetrahedron.m_pts[1] = p4;
+ tetrahedron.m_pts[2] = p2;
+ tetrahedron.m_pts[3] = p5;
+ tset.m_tetrahedra.PushBack(tetrahedron);
+
+ tetrahedron.m_pts[0] = p8;
+ tetrahedron.m_pts[1] = p5;
+ tetrahedron.m_pts[2] = p7;
+ tetrahedron.m_pts[3] = p4;
+ tset.m_tetrahedra.PushBack(tetrahedron);
+ if (value == PRIMITIVE_INSIDE_SURFACE) {
+ tset.m_numTetrahedraInsideSurface += 5;
+ }
+ else {
+ tset.m_numTetrahedraOnSurface += 5;
+ }
+ }
+ }
+ }
+ }
+}
+
+void Volume::AlignToPrincipalAxes(double (&rot)[3][3]) const
+{
+ const short i0 = (short)m_dim[0];
+ const short j0 = (short)m_dim[1];
+ const short k0 = (short)m_dim[2];
+ Vec3<double> barycenter(0.0);
+ size_t nVoxels = 0;
+ for (short i = 0; i < i0; ++i) {
+ for (short j = 0; j < j0; ++j) {
+ for (short k = 0; k < k0; ++k) {
+ const unsigned char& value = GetVoxel(i, j, k);
+ if (value == PRIMITIVE_INSIDE_SURFACE || value == PRIMITIVE_ON_SURFACE) {
+ barycenter[0] += i;
+ barycenter[1] += j;
+ barycenter[2] += k;
+ ++nVoxels;
+ }
+ }
+ }
+ }
+ barycenter /= (double)nVoxels;
+
+ double covMat[3][3] = { { 0.0, 0.0, 0.0 },
+ { 0.0, 0.0, 0.0 },
+ { 0.0, 0.0, 0.0 } };
+ double x, y, z;
+ for (short i = 0; i < i0; ++i) {
+ for (short j = 0; j < j0; ++j) {
+ for (short k = 0; k < k0; ++k) {
+ const unsigned char& value = GetVoxel(i, j, k);
+ if (value == PRIMITIVE_INSIDE_SURFACE || value == PRIMITIVE_ON_SURFACE) {
+ x = i - barycenter[0];
+ y = j - barycenter[1];
+ z = k - barycenter[2];
+ covMat[0][0] += x * x;
+ covMat[1][1] += y * y;
+ covMat[2][2] += z * z;
+ covMat[0][1] += x * y;
+ covMat[0][2] += x * z;
+ covMat[1][2] += y * z;
+ }
+ }
+ }
+ }
+ covMat[1][0] = covMat[0][1];
+ covMat[2][0] = covMat[0][2];
+ covMat[2][1] = covMat[1][2];
+ double D[3][3];
+ Diagonalize(covMat, rot, D);
+}
+TetrahedronSet::TetrahedronSet()
+{
+ m_minBB[0] = m_minBB[1] = m_minBB[2] = 0.0;
+ m_maxBB[0] = m_maxBB[1] = m_maxBB[2] = 1.0;
+ m_barycenter[0] = m_barycenter[1] = m_barycenter[2] = 0.0;
+ m_scale = 1.0;
+ m_numTetrahedraOnSurface = 0;
+ m_numTetrahedraInsideSurface = 0;
+ memset(m_Q, 0, sizeof(double) * 9);
+ memset(m_D, 0, sizeof(double) * 9);
+}
+TetrahedronSet::~TetrahedronSet(void)
+{
+}
+void TetrahedronSet::ComputeBB()
+{
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return;
+
+ for (int h = 0; h < 3; ++h) {
+ m_minBB[h] = m_maxBB[h] = m_tetrahedra[0].m_pts[0][h];
+ m_barycenter[h] = 0.0;
+ }
+ for (size_t p = 0; p < nTetrahedra; ++p) {
+ for (int i = 0; i < 4; ++i) {
+ for (int h = 0; h < 3; ++h) {
+ if (m_minBB[h] > m_tetrahedra[p].m_pts[i][h])
+ m_minBB[h] = m_tetrahedra[p].m_pts[i][h];
+ if (m_maxBB[h] < m_tetrahedra[p].m_pts[i][h])
+ m_maxBB[h] = m_tetrahedra[p].m_pts[i][h];
+ m_barycenter[h] += m_tetrahedra[p].m_pts[i][h];
+ }
+ }
+ }
+ m_barycenter /= (double)(4 * nTetrahedra);
+}
+void TetrahedronSet::ComputeConvexHull(Mesh& meshCH, const size_t sampling) const
+{
+ const size_t CLUSTER_SIZE = 65536;
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return;
+
+ SArray<Vec3<double> > cpoints;
+
+ Vec3<double>* points = new Vec3<double>[CLUSTER_SIZE];
+ size_t p = 0;
+ while (p < nTetrahedra) {
+ size_t q = 0;
+ size_t s = 0;
+ while (q < CLUSTER_SIZE && p < nTetrahedra) {
+ if (m_tetrahedra[p].m_data == PRIMITIVE_ON_SURFACE) {
+ ++s;
+ if (s == sampling) {
+ s = 0;
+ for (int a = 0; a < 4; ++a) {
+ points[q++] = m_tetrahedra[p].m_pts[a];
+ for (int xx = 0; xx < 3; ++xx) {
+ assert(m_tetrahedra[p].m_pts[a][xx] + EPS >= m_minBB[xx]);
+ assert(m_tetrahedra[p].m_pts[a][xx] <= m_maxBB[xx] + EPS);
+ }
+ }
+ }
+ }
+ ++p;
+ }
+ btConvexHullComputer ch;
+ ch.compute((double*)points, 3 * sizeof(double), (int)q, -1.0, -1.0);
+ for (int v = 0; v < ch.vertices.size(); v++) {
+ cpoints.PushBack(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ()));
+ }
+ }
+ delete[] points;
+
+ points = cpoints.Data();
+ btConvexHullComputer ch;
+ ch.compute((double*)points, 3 * sizeof(double), (int)cpoints.Size(), -1.0, -1.0);
+ meshCH.ResizePoints(0);
+ meshCH.ResizeTriangles(0);
+ for (int v = 0; v < ch.vertices.size(); v++) {
+ meshCH.AddPoint(Vec3<double>(ch.vertices[v].getX(), ch.vertices[v].getY(), ch.vertices[v].getZ()));
+ }
+ const int nt = ch.faces.size();
+ for (int t = 0; t < nt; ++t) {
+ const btConvexHullComputer::Edge* sourceEdge = &(ch.edges[ch.faces[t]]);
+ int a = sourceEdge->getSourceVertex();
+ int b = sourceEdge->getTargetVertex();
+ const btConvexHullComputer::Edge* edge = sourceEdge->getNextEdgeOfFace();
+ int c = edge->getTargetVertex();
+ while (c != a) {
+ meshCH.AddTriangle(Vec3<int>(a, b, c));
+ edge = edge->getNextEdgeOfFace();
+ b = c;
+ c = edge->getTargetVertex();
+ }
+ }
+}
+inline bool TetrahedronSet::Add(Tetrahedron& tetrahedron)
+{
+ double v = ComputeVolume4(tetrahedron.m_pts[0], tetrahedron.m_pts[1], tetrahedron.m_pts[2], tetrahedron.m_pts[3]);
+
+ const double EPS = 0.0000000001;
+ if (fabs(v) < EPS) {
+ return false;
+ }
+ else if (v < 0.0) {
+ Vec3<double> tmp = tetrahedron.m_pts[0];
+ tetrahedron.m_pts[0] = tetrahedron.m_pts[1];
+ tetrahedron.m_pts[1] = tmp;
+ }
+
+ for (int a = 0; a < 4; ++a) {
+ for (int xx = 0; xx < 3; ++xx) {
+ assert(tetrahedron.m_pts[a][xx] + EPS >= m_minBB[xx]);
+ assert(tetrahedron.m_pts[a][xx] <= m_maxBB[xx] + EPS);
+ }
+ }
+ m_tetrahedra.PushBack(tetrahedron);
+ return true;
+}
+
+void TetrahedronSet::AddClippedTetrahedra(const Vec3<double> (&pts)[10], const int nPts)
+{
+ const int tetF[4][3] = { { 0, 1, 2 }, { 2, 1, 3 }, { 3, 1, 0 }, { 3, 0, 2 } };
+ if (nPts < 4) {
+ return;
+ }
+ else if (nPts == 4) {
+ Tetrahedron tetrahedron;
+ tetrahedron.m_data = PRIMITIVE_ON_SURFACE;
+ tetrahedron.m_pts[0] = pts[0];
+ tetrahedron.m_pts[1] = pts[1];
+ tetrahedron.m_pts[2] = pts[2];
+ tetrahedron.m_pts[3] = pts[3];
+ if (Add(tetrahedron)) {
+ ++m_numTetrahedraOnSurface;
+ }
+ }
+ else if (nPts == 5) {
+ const int tet[15][4] = {
+ { 0, 1, 2, 3 }, { 1, 2, 3, 4 }, { 0, 2, 3, 4 }, { 0, 1, 3, 4 }, { 0, 1, 2, 4 },
+ };
+ const int rem[5] = { 4, 0, 1, 2, 3 };
+ double maxVol = 0.0;
+ int h0 = -1;
+ Tetrahedron tetrahedron0;
+ tetrahedron0.m_data = PRIMITIVE_ON_SURFACE;
+ for (int h = 0; h < 5; ++h) {
+ double v = ComputeVolume4(pts[tet[h][0]], pts[tet[h][1]], pts[tet[h][2]], pts[tet[h][3]]);
+ if (v > maxVol) {
+ h0 = h;
+ tetrahedron0.m_pts[0] = pts[tet[h][0]];
+ tetrahedron0.m_pts[1] = pts[tet[h][1]];
+ tetrahedron0.m_pts[2] = pts[tet[h][2]];
+ tetrahedron0.m_pts[3] = pts[tet[h][3]];
+ maxVol = v;
+ }
+ else if (-v > maxVol) {
+ h0 = h;
+ tetrahedron0.m_pts[0] = pts[tet[h][1]];
+ tetrahedron0.m_pts[1] = pts[tet[h][0]];
+ tetrahedron0.m_pts[2] = pts[tet[h][2]];
+ tetrahedron0.m_pts[3] = pts[tet[h][3]];
+ maxVol = -v;
+ }
+ }
+ if (h0 == -1)
+ return;
+ if (Add(tetrahedron0)) {
+ ++m_numTetrahedraOnSurface;
+ }
+ else {
+ return;
+ }
+ int a = rem[h0];
+ maxVol = 0.0;
+ int h1 = -1;
+ Tetrahedron tetrahedron1;
+ tetrahedron1.m_data = PRIMITIVE_ON_SURFACE;
+ for (int h = 0; h < 4; ++h) {
+ double v = ComputeVolume4(pts[a], tetrahedron0.m_pts[tetF[h][0]], tetrahedron0.m_pts[tetF[h][1]], tetrahedron0.m_pts[tetF[h][2]]);
+ if (v > maxVol) {
+ h1 = h;
+ tetrahedron1.m_pts[0] = pts[a];
+ tetrahedron1.m_pts[1] = tetrahedron0.m_pts[tetF[h][0]];
+ tetrahedron1.m_pts[2] = tetrahedron0.m_pts[tetF[h][1]];
+ tetrahedron1.m_pts[3] = tetrahedron0.m_pts[tetF[h][2]];
+ maxVol = v;
+ }
+ }
+ if (h1 == -1 && Add(tetrahedron1)) {
+ ++m_numTetrahedraOnSurface;
+ }
+ }
+ else if (nPts == 6) {
+
+ const int tet[15][4] = { { 2, 3, 4, 5 }, { 1, 3, 4, 5 }, { 1, 2, 4, 5 }, { 1, 2, 3, 5 }, { 1, 2, 3, 4 },
+ { 0, 3, 4, 5 }, { 0, 2, 4, 5 }, { 0, 2, 3, 5 }, { 0, 2, 3, 4 }, { 0, 1, 4, 5 },
+ { 0, 1, 3, 5 }, { 0, 1, 3, 4 }, { 0, 1, 2, 5 }, { 0, 1, 2, 4 }, { 0, 1, 2, 3 } };
+ const int rem[15][2] = { { 0, 1 }, { 0, 2 }, { 0, 3 }, { 0, 4 }, { 0, 5 },
+ { 1, 2 }, { 1, 3 }, { 1, 4 }, { 1, 5 }, { 2, 3 },
+ { 2, 4 }, { 2, 5 }, { 3, 4 }, { 3, 5 }, { 4, 5 } };
+ double maxVol = 0.0;
+ int h0 = -1;
+ Tetrahedron tetrahedron0;
+ tetrahedron0.m_data = PRIMITIVE_ON_SURFACE;
+ for (int h = 0; h < 15; ++h) {
+ double v = ComputeVolume4(pts[tet[h][0]], pts[tet[h][1]], pts[tet[h][2]], pts[tet[h][3]]);
+ if (v > maxVol) {
+ h0 = h;
+ tetrahedron0.m_pts[0] = pts[tet[h][0]];
+ tetrahedron0.m_pts[1] = pts[tet[h][1]];
+ tetrahedron0.m_pts[2] = pts[tet[h][2]];
+ tetrahedron0.m_pts[3] = pts[tet[h][3]];
+ maxVol = v;
+ }
+ else if (-v > maxVol) {
+ h0 = h;
+ tetrahedron0.m_pts[0] = pts[tet[h][1]];
+ tetrahedron0.m_pts[1] = pts[tet[h][0]];
+ tetrahedron0.m_pts[2] = pts[tet[h][2]];
+ tetrahedron0.m_pts[3] = pts[tet[h][3]];
+ maxVol = -v;
+ }
+ }
+ if (h0 == -1)
+ return;
+ if (Add(tetrahedron0)) {
+ ++m_numTetrahedraOnSurface;
+ }
+ else {
+ return;
+ }
+
+ int a0 = rem[h0][0];
+ int a1 = rem[h0][1];
+ int h1 = -1;
+ Tetrahedron tetrahedron1;
+ tetrahedron1.m_data = PRIMITIVE_ON_SURFACE;
+ maxVol = 0.0;
+ for (int h = 0; h < 4; ++h) {
+ double v = ComputeVolume4(pts[a0], tetrahedron0.m_pts[tetF[h][0]], tetrahedron0.m_pts[tetF[h][1]], tetrahedron0.m_pts[tetF[h][2]]);
+ if (v > maxVol) {
+ h1 = h;
+ tetrahedron1.m_pts[0] = pts[a0];
+ tetrahedron1.m_pts[1] = tetrahedron0.m_pts[tetF[h][0]];
+ tetrahedron1.m_pts[2] = tetrahedron0.m_pts[tetF[h][1]];
+ tetrahedron1.m_pts[3] = tetrahedron0.m_pts[tetF[h][2]];
+ maxVol = v;
+ }
+ }
+ if (h1 != -1 && Add(tetrahedron1)) {
+ ++m_numTetrahedraOnSurface;
+ }
+ else {
+ h1 = -1;
+ }
+ maxVol = 0.0;
+ int h2 = -1;
+ Tetrahedron tetrahedron2;
+ tetrahedron2.m_data = PRIMITIVE_ON_SURFACE;
+ for (int h = 0; h < 4; ++h) {
+ double v = ComputeVolume4(pts[a0], tetrahedron0.m_pts[tetF[h][0]], tetrahedron0.m_pts[tetF[h][1]], tetrahedron0.m_pts[tetF[h][2]]);
+ if (h == h1)
+ continue;
+ if (v > maxVol) {
+ h2 = h;
+ tetrahedron2.m_pts[0] = pts[a1];
+ tetrahedron2.m_pts[1] = tetrahedron0.m_pts[tetF[h][0]];
+ tetrahedron2.m_pts[2] = tetrahedron0.m_pts[tetF[h][1]];
+ tetrahedron2.m_pts[3] = tetrahedron0.m_pts[tetF[h][2]];
+ maxVol = v;
+ }
+ }
+ if (h1 != -1) {
+ for (int h = 0; h < 4; ++h) {
+ double v = ComputeVolume4(pts[a1], tetrahedron1.m_pts[tetF[h][0]], tetrahedron1.m_pts[tetF[h][1]], tetrahedron1.m_pts[tetF[h][2]]);
+ if (h == 1)
+ continue;
+ if (v > maxVol) {
+ h2 = h;
+ tetrahedron2.m_pts[0] = pts[a1];
+ tetrahedron2.m_pts[1] = tetrahedron1.m_pts[tetF[h][0]];
+ tetrahedron2.m_pts[2] = tetrahedron1.m_pts[tetF[h][1]];
+ tetrahedron2.m_pts[3] = tetrahedron1.m_pts[tetF[h][2]];
+ maxVol = v;
+ }
+ }
+ }
+ if (h2 != -1 && Add(tetrahedron2)) {
+ ++m_numTetrahedraOnSurface;
+ }
+ }
+ else {
+ assert(0);
+ }
+}
+
+void TetrahedronSet::Intersect(const Plane& plane,
+ SArray<Vec3<double> >* const positivePts,
+ SArray<Vec3<double> >* const negativePts,
+ const size_t sampling) const
+{
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return;
+}
+void TetrahedronSet::ComputeExteriorPoints(const Plane& plane,
+ const Mesh& mesh,
+ SArray<Vec3<double> >* const exteriorPts) const
+{
+}
+void TetrahedronSet::ComputeClippedVolumes(const Plane& plane,
+ double& positiveVolume,
+ double& negativeVolume) const
+{
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return;
+}
+
+void TetrahedronSet::SelectOnSurface(PrimitiveSet* const onSurfP) const
+{
+ TetrahedronSet* const onSurf = (TetrahedronSet*)onSurfP;
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return;
+ onSurf->m_tetrahedra.Resize(0);
+ onSurf->m_scale = m_scale;
+ onSurf->m_numTetrahedraOnSurface = 0;
+ onSurf->m_numTetrahedraInsideSurface = 0;
+ onSurf->m_barycenter = m_barycenter;
+ onSurf->m_minBB = m_minBB;
+ onSurf->m_maxBB = m_maxBB;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ onSurf->m_Q[i][j] = m_Q[i][j];
+ onSurf->m_D[i][j] = m_D[i][j];
+ }
+ }
+ Tetrahedron tetrahedron;
+ for (size_t v = 0; v < nTetrahedra; ++v) {
+ tetrahedron = m_tetrahedra[v];
+ if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) {
+ onSurf->m_tetrahedra.PushBack(tetrahedron);
+ ++onSurf->m_numTetrahedraOnSurface;
+ }
+ }
+}
+void TetrahedronSet::Clip(const Plane& plane,
+ PrimitiveSet* const positivePartP,
+ PrimitiveSet* const negativePartP) const
+{
+ TetrahedronSet* const positivePart = (TetrahedronSet*)positivePartP;
+ TetrahedronSet* const negativePart = (TetrahedronSet*)negativePartP;
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return;
+ positivePart->m_tetrahedra.Resize(0);
+ negativePart->m_tetrahedra.Resize(0);
+ positivePart->m_tetrahedra.Allocate(nTetrahedra);
+ negativePart->m_tetrahedra.Allocate(nTetrahedra);
+ negativePart->m_scale = positivePart->m_scale = m_scale;
+ negativePart->m_numTetrahedraOnSurface = positivePart->m_numTetrahedraOnSurface = 0;
+ negativePart->m_numTetrahedraInsideSurface = positivePart->m_numTetrahedraInsideSurface = 0;
+ negativePart->m_barycenter = m_barycenter;
+ positivePart->m_barycenter = m_barycenter;
+ negativePart->m_minBB = m_minBB;
+ positivePart->m_minBB = m_minBB;
+ negativePart->m_maxBB = m_maxBB;
+ positivePart->m_maxBB = m_maxBB;
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ negativePart->m_Q[i][j] = positivePart->m_Q[i][j] = m_Q[i][j];
+ negativePart->m_D[i][j] = positivePart->m_D[i][j] = m_D[i][j];
+ }
+ }
+
+ Tetrahedron tetrahedron;
+ double delta, alpha;
+ int sign[4];
+ int npos, nneg;
+ Vec3<double> posPts[10];
+ Vec3<double> negPts[10];
+ Vec3<double> P0, P1, M;
+ const Vec3<double> n(plane.m_a, plane.m_b, plane.m_c);
+ const int edges[6][2] = { { 0, 1 }, { 0, 2 }, { 0, 3 }, { 1, 2 }, { 1, 3 }, { 2, 3 } };
+ double dist;
+ for (size_t v = 0; v < nTetrahedra; ++v) {
+ tetrahedron = m_tetrahedra[v];
+ npos = nneg = 0;
+ for (int i = 0; i < 4; ++i) {
+ dist = plane.m_a * tetrahedron.m_pts[i][0] + plane.m_b * tetrahedron.m_pts[i][1] + plane.m_c * tetrahedron.m_pts[i][2] + plane.m_d;
+ if (dist > 0.0) {
+ sign[i] = 1;
+ posPts[npos] = tetrahedron.m_pts[i];
+ ++npos;
+ }
+ else {
+ sign[i] = -1;
+ negPts[nneg] = tetrahedron.m_pts[i];
+ ++nneg;
+ }
+ }
+
+ if (npos == 4) {
+ positivePart->Add(tetrahedron);
+ if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) {
+ ++positivePart->m_numTetrahedraOnSurface;
+ }
+ else {
+ ++positivePart->m_numTetrahedraInsideSurface;
+ }
+ }
+ else if (nneg == 4) {
+ negativePart->Add(tetrahedron);
+ if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) {
+ ++negativePart->m_numTetrahedraOnSurface;
+ }
+ else {
+ ++negativePart->m_numTetrahedraInsideSurface;
+ }
+ }
+ else {
+ int nnew = 0;
+ for (int j = 0; j < 6; ++j) {
+ if (sign[edges[j][0]] * sign[edges[j][1]] == -1) {
+ P0 = tetrahedron.m_pts[edges[j][0]];
+ P1 = tetrahedron.m_pts[edges[j][1]];
+ delta = (P0 - P1) * n;
+ alpha = -(plane.m_d + (n * P1)) / delta;
+ assert(alpha >= 0.0 && alpha <= 1.0);
+ M = alpha * P0 + (1 - alpha) * P1;
+ for (int xx = 0; xx < 3; ++xx) {
+ assert(M[xx] + EPS >= m_minBB[xx]);
+ assert(M[xx] <= m_maxBB[xx] + EPS);
+ }
+ posPts[npos++] = M;
+ negPts[nneg++] = M;
+ ++nnew;
+ }
+ }
+ negativePart->AddClippedTetrahedra(negPts, nneg);
+ positivePart->AddClippedTetrahedra(posPts, npos);
+ }
+ }
+}
+void TetrahedronSet::Convert(Mesh& mesh, const VOXEL_VALUE value) const
+{
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return;
+ for (size_t v = 0; v < nTetrahedra; ++v) {
+ const Tetrahedron& tetrahedron = m_tetrahedra[v];
+ if (tetrahedron.m_data == value) {
+ int s = (int)mesh.GetNPoints();
+ mesh.AddPoint(tetrahedron.m_pts[0]);
+ mesh.AddPoint(tetrahedron.m_pts[1]);
+ mesh.AddPoint(tetrahedron.m_pts[2]);
+ mesh.AddPoint(tetrahedron.m_pts[3]);
+ mesh.AddTriangle(Vec3<int>(s + 0, s + 1, s + 2));
+ mesh.AddTriangle(Vec3<int>(s + 2, s + 1, s + 3));
+ mesh.AddTriangle(Vec3<int>(s + 3, s + 1, s + 0));
+ mesh.AddTriangle(Vec3<int>(s + 3, s + 0, s + 2));
+ }
+ }
+}
+const double TetrahedronSet::ComputeVolume() const
+{
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return 0.0;
+ double volume = 0.0;
+ for (size_t v = 0; v < nTetrahedra; ++v) {
+ const Tetrahedron& tetrahedron = m_tetrahedra[v];
+ volume += fabs(ComputeVolume4(tetrahedron.m_pts[0], tetrahedron.m_pts[1], tetrahedron.m_pts[2], tetrahedron.m_pts[3]));
+ }
+ return volume / 6.0;
+}
+const double TetrahedronSet::ComputeMaxVolumeError() const
+{
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return 0.0;
+ double volume = 0.0;
+ for (size_t v = 0; v < nTetrahedra; ++v) {
+ const Tetrahedron& tetrahedron = m_tetrahedra[v];
+ if (tetrahedron.m_data == PRIMITIVE_ON_SURFACE) {
+ volume += fabs(ComputeVolume4(tetrahedron.m_pts[0], tetrahedron.m_pts[1], tetrahedron.m_pts[2], tetrahedron.m_pts[3]));
+ }
+ }
+ return volume / 6.0;
+}
+void TetrahedronSet::RevertAlignToPrincipalAxes()
+{
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return;
+ double x, y, z;
+ for (size_t v = 0; v < nTetrahedra; ++v) {
+ Tetrahedron& tetrahedron = m_tetrahedra[v];
+ for (int i = 0; i < 4; ++i) {
+ x = tetrahedron.m_pts[i][0] - m_barycenter[0];
+ y = tetrahedron.m_pts[i][1] - m_barycenter[1];
+ z = tetrahedron.m_pts[i][2] - m_barycenter[2];
+ tetrahedron.m_pts[i][0] = m_Q[0][0] * x + m_Q[0][1] * y + m_Q[0][2] * z + m_barycenter[0];
+ tetrahedron.m_pts[i][1] = m_Q[1][0] * x + m_Q[1][1] * y + m_Q[1][2] * z + m_barycenter[1];
+ tetrahedron.m_pts[i][2] = m_Q[2][0] * x + m_Q[2][1] * y + m_Q[2][2] * z + m_barycenter[2];
+ }
+ }
+ ComputeBB();
+}
+void TetrahedronSet::ComputePrincipalAxes()
+{
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return;
+ double covMat[3][3] = { { 0.0, 0.0, 0.0 },
+ { 0.0, 0.0, 0.0 },
+ { 0.0, 0.0, 0.0 } };
+ double x, y, z;
+ for (size_t v = 0; v < nTetrahedra; ++v) {
+ Tetrahedron& tetrahedron = m_tetrahedra[v];
+ for (int i = 0; i < 4; ++i) {
+ x = tetrahedron.m_pts[i][0] - m_barycenter[0];
+ y = tetrahedron.m_pts[i][1] - m_barycenter[1];
+ z = tetrahedron.m_pts[i][2] - m_barycenter[2];
+ covMat[0][0] += x * x;
+ covMat[1][1] += y * y;
+ covMat[2][2] += z * z;
+ covMat[0][1] += x * y;
+ covMat[0][2] += x * z;
+ covMat[1][2] += y * z;
+ }
+ }
+ double n = nTetrahedra * 4.0;
+ covMat[0][0] /= n;
+ covMat[1][1] /= n;
+ covMat[2][2] /= n;
+ covMat[0][1] /= n;
+ covMat[0][2] /= n;
+ covMat[1][2] /= n;
+ covMat[1][0] = covMat[0][1];
+ covMat[2][0] = covMat[0][2];
+ covMat[2][1] = covMat[1][2];
+ Diagonalize(covMat, m_Q, m_D);
+}
+void TetrahedronSet::AlignToPrincipalAxes()
+{
+ const size_t nTetrahedra = m_tetrahedra.Size();
+ if (nTetrahedra == 0)
+ return;
+ double x, y, z;
+ for (size_t v = 0; v < nTetrahedra; ++v) {
+ Tetrahedron& tetrahedron = m_tetrahedra[v];
+ for (int i = 0; i < 4; ++i) {
+ x = tetrahedron.m_pts[i][0] - m_barycenter[0];
+ y = tetrahedron.m_pts[i][1] - m_barycenter[1];
+ z = tetrahedron.m_pts[i][2] - m_barycenter[2];
+ tetrahedron.m_pts[i][0] = m_Q[0][0] * x + m_Q[1][0] * y + m_Q[2][0] * z + m_barycenter[0];
+ tetrahedron.m_pts[i][1] = m_Q[0][1] * x + m_Q[1][1] * y + m_Q[2][1] * z + m_barycenter[1];
+ tetrahedron.m_pts[i][2] = m_Q[0][2] * x + m_Q[1][2] * y + m_Q[2][2] * z + m_barycenter[2];
+ }
+ }
+ ComputeBB();
+}
+}
diff --git a/Extras/HACD/hacdVector.h b/Extras/VHACD/test/inc/oclHelper.h
similarity index 53%
copy from Extras/HACD/hacdVector.h
copy to Extras/VHACD/test/inc/oclHelper.h
index 0c8dd02..ff63c30 100644
--- a/Extras/HACD/hacdVector.h
+++ b/Extras/VHACD/test/inc/oclHelper.h
@@ -12,56 +12,39 @@
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 HOLDER 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 PRO [...]
*/
+#ifdef OPENCL_FOUND
+
#pragma once
-#ifndef HACD_VECTOR_H
-#define HACD_VECTOR_H
-#include<math.h>
-#include<iostream>
-#include "hacdVersion.h"
+#ifndef OCL_HELPER_H
+#define OCL_HELPER_H
-namespace HACD
-{
- typedef double Real;
- //! Vector dim 3.
- template < typename T > class Vec3
- {
- public:
- T & X();
- T & Y();
- T & Z();
- const T & X() const;
- const T & Y() const;
- const T & Z() const;
- void Normalize();
- T GetNorm() const;
- void operator= (const Vec3 & rhs);
- void operator+=(const Vec3 & rhs);
- void operator-=(const Vec3 & rhs);
- void operator-=(T a);
- void operator+=(T a);
- void operator/=(T a);
- void operator*=(T a);
- Vec3 operator^ (const Vec3 & rhs) const;
- T operator* (const Vec3 & rhs) const;
- Vec3 operator+ (const Vec3 & rhs) const;
- Vec3 operator- (const Vec3 & rhs) const;
- Vec3 operator- () const;
- Vec3 operator* (T rhs) const;
- Vec3 operator/ (T rhs) const;
- Vec3();
- Vec3(T a);
- Vec3(T x, T y, T z);
- Vec3(const Vec3 & rhs);
- /*virtual*/ ~Vec3(void);
+#include <string>
+#include <vector>
- private:
- T m_data[3];
- };
- template<typename T>
- const bool Colinear(const Vec3<T> & a, const Vec3<T> & b, const Vec3<T> & c);
- template<typename T>
- const T Volume(const Vec3<T> & a, const Vec3<T> & b, const Vec3<T> & c, const Vec3<T> & d);
-
-}
-#include "hacdVector.inl" // template implementation
+#ifdef __MACH__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
#endif
+
+class OCLHelper {
+public:
+ OCLHelper(void){};
+ ~OCLHelper(void){};
+ bool InitPlatform(const unsigned int platformIndex = 0);
+ bool InitDevice(const unsigned int deviceIndex);
+ bool GetPlatformsInfo(std::vector<std::string>& info, const std::string& indentation);
+ bool GetDevicesInfo(std::vector<std::string>& info, const std::string& indentation);
+ cl_platform_id* GetPlatform() { return &m_platform; }
+ const cl_platform_id* GetPlatform() const { return &m_platform; }
+ cl_device_id* GetDevice() { return &m_device; }
+ const cl_device_id* GetDevice() const { return &m_device; }
+private:
+ cl_platform_id m_platform;
+ cl_device_id m_device;
+ cl_int m_lastError;
+};
+
+#endif // OCL_HELPER_H
+
+#endif //OPENCL_FOUND
diff --git a/Extras/VHACD/test/src/main.cpp b/Extras/VHACD/test/src/main.cpp
new file mode 100644
index 0000000..6b12047
--- /dev/null
+++ b/Extras/VHACD/test/src/main.cpp
@@ -0,0 +1,648 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+
+
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. The names of the contributors may not 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 HOLDER 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 PRO [...]
+ */
+
+#define _CRT_SECURE_NO_WARNINGS
+#include <algorithm>
+#include <assert.h>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <sstream>
+#include <stdio.h>
+#include <string.h>
+#include <string>
+#include <vector>
+
+//#define _CRTDBG_MAP_ALLOC
+
+#ifdef _CRTDBG_MAP_ALLOC
+#include <crtdbg.h>
+#include <stdlib.h>
+#endif // _CRTDBG_MAP_ALLOC
+
+#include "VHACD.h"
+
+
+using namespace VHACD;
+using namespace std;
+
+class MyCallback : public IVHACD::IUserCallback {
+public:
+ MyCallback(void) {}
+ ~MyCallback(){};
+ void Update(const double overallProgress, const double stageProgress, const double operationProgress,
+ const char* const stage, const char* const operation)
+ {
+ cout << setfill(' ') << setw(3) << (int)(overallProgress + 0.5) << "% "
+ << "[ " << stage << " " << setfill(' ') << setw(3) << (int)(stageProgress + 0.5) << "% ] "
+ << operation << " " << setfill(' ') << setw(3) << (int)(operationProgress + 0.5) << "%" << endl;
+ };
+};
+class MyLogger : public IVHACD::IUserLogger {
+public:
+ MyLogger(void) {}
+ MyLogger(const string& fileName) { OpenFile(fileName); }
+ ~MyLogger(){};
+ void Log(const char* const msg)
+ {
+ if (m_file.is_open()) {
+ m_file << msg;
+ m_file.flush();
+ }
+ }
+ void OpenFile(const string& fileName)
+ {
+ m_file.open(fileName.c_str());
+ }
+
+private:
+ ofstream m_file;
+};
+struct Material {
+
+ float m_diffuseColor[3];
+ float m_ambientIntensity;
+ float m_specularColor[3];
+ float m_emissiveColor[3];
+ float m_shininess;
+ float m_transparency;
+ Material(void)
+ {
+ m_diffuseColor[0] = 0.5f;
+ m_diffuseColor[1] = 0.5f;
+ m_diffuseColor[2] = 0.5f;
+ m_specularColor[0] = 0.5f;
+ m_specularColor[1] = 0.5f;
+ m_specularColor[2] = 0.5f;
+ m_ambientIntensity = 0.4f;
+ m_emissiveColor[0] = 0.0f;
+ m_emissiveColor[1] = 0.0f;
+ m_emissiveColor[2] = 0.0f;
+ m_shininess = 0.4f;
+ m_transparency = 0.5f;
+ };
+};
+struct Parameters {
+ unsigned int m_oclPlatformID;
+ unsigned int m_oclDeviceID;
+ string m_fileNameIn;
+ string m_fileNameOut;
+ string m_fileNameLog;
+ bool m_run;
+ IVHACD::Parameters m_paramsVHACD;
+ Parameters(void)
+ {
+ m_run = true;
+ m_oclPlatformID = 0;
+ m_oclDeviceID = 0;
+ m_fileNameIn = "";
+ m_fileNameOut = "output.obj";
+ m_fileNameLog = "log.txt";
+ }
+};
+bool LoadOFF(const string& fileName, vector<float>& points, vector<int>& triangles, IVHACD::IUserLogger& logger);
+bool LoadOBJ(const string& fileName, vector<float>& points, vector<int>& triangles, IVHACD::IUserLogger& logger);
+bool SaveOFF(const string& fileName, const float* const& points, const int* const& triangles, const unsigned int& nPoints,
+ const unsigned int& nTriangles, IVHACD::IUserLogger& logger);
+bool SaveOBJ(ofstream& fout, const double* const& points, const int* const& triangles, const unsigned int& nPoints,
+ const unsigned int& nTriangles, const Material& material, IVHACD::IUserLogger& logger, int convexPart, int vertexOffset);
+
+bool SaveVRML2(ofstream& fout, const double* const& points, const int* const& triangles, const unsigned int& nPoints,
+ const unsigned int& nTriangles, const Material& material, IVHACD::IUserLogger& logger);
+void GetFileExtension(const string& fileName, string& fileExtension);
+void ComputeRandomColor(Material& mat);
+void Usage(const Parameters& params);
+void ParseParameters(int argc, char* argv[], Parameters& params);
+
+
+int main(int argc, char* argv[])
+{
+ // --input camel.off --output camel_acd.obj --log log.txt --resolution 1000000 --depth 20 --concavity 0.0025 --planeDownsampling 4 --convexhullDownsampling 4 --alpha 0.05 --beta 0.05 --gamma 0.00125 --pca 0 --mode 0 --maxNumVerticesPerCH 256 --minVolumePerCH 0.0001 --convexhullApproximation 1 --oclDeviceID 2
+ {
+ // set parameters
+ Parameters params;
+ ParseParameters(argc, argv, params);
+ MyCallback myCallback;
+ MyLogger myLogger(params.m_fileNameLog);
+ params.m_paramsVHACD.m_logger = &myLogger;
+ params.m_paramsVHACD.m_callback = &myCallback;
+ Usage(params);
+ if (!params.m_run) {
+ return 0;
+ }
+
+ std::ostringstream msg;
+
+ msg << "+ OpenCL (OFF)" << std::endl;
+
+ msg << "+ Parameters" << std::endl;
+ msg << "\t input " << params.m_fileNameIn << endl;
+ msg << "\t resolution " << params.m_paramsVHACD.m_resolution << endl;
+ msg << "\t max. depth " << params.m_paramsVHACD.m_depth << endl;
+ msg << "\t max. concavity " << params.m_paramsVHACD.m_concavity << endl;
+ msg << "\t plane down-sampling " << params.m_paramsVHACD.m_planeDownsampling << endl;
+ msg << "\t convex-hull down-sampling " << params.m_paramsVHACD.m_convexhullDownsampling << endl;
+ msg << "\t alpha " << params.m_paramsVHACD.m_alpha << endl;
+ msg << "\t beta " << params.m_paramsVHACD.m_beta << endl;
+ msg << "\t gamma " << params.m_paramsVHACD.m_gamma << endl;
+ msg << "\t pca " << params.m_paramsVHACD.m_pca << endl;
+ msg << "\t mode " << params.m_paramsVHACD.m_mode << endl;
+ msg << "\t max. vertices per convex-hull " << params.m_paramsVHACD.m_maxNumVerticesPerCH << endl;
+ msg << "\t min. volume to add vertices to convex-hulls " << params.m_paramsVHACD.m_minVolumePerCH << endl;
+ msg << "\t convex-hull approximation " << params.m_paramsVHACD.m_convexhullApproximation << endl;
+ msg << "\t OpenCL acceleration " << params.m_paramsVHACD.m_oclAcceleration << endl;
+ msg << "\t OpenCL platform ID " << params.m_oclPlatformID << endl;
+ msg << "\t OpenCL device ID " << params.m_oclDeviceID << endl;
+ msg << "\t output " << params.m_fileNameOut << endl;
+ msg << "\t log " << params.m_fileNameLog << endl;
+ msg << "+ Load mesh" << std::endl;
+ myLogger.Log(msg.str().c_str());
+
+ cout << msg.str().c_str();
+
+ // load mesh
+ vector<float> points;
+ vector<int> triangles;
+ string fileExtension;
+ GetFileExtension(params.m_fileNameIn, fileExtension);
+ if (fileExtension == ".OFF") {
+ if (!LoadOFF(params.m_fileNameIn, points, triangles, myLogger)) {
+ return -1;
+ }
+ }
+ else if (fileExtension == ".OBJ") {
+ if (!LoadOBJ(params.m_fileNameIn, points, triangles, myLogger)) {
+ return -1;
+ }
+ }
+ else {
+ myLogger.Log("Format not supported!\n");
+ return -1;
+ }
+
+ // run V-HACD
+ IVHACD* interfaceVHACD = CreateVHACD();
+
+#ifdef CL_VERSION_1_1
+ if (params.m_paramsVHACD.m_oclAcceleration) {
+ bool res = interfaceVHACD->OCLInit(oclHelper.GetDevice(), &myLogger);
+ if (!res) {
+ params.m_paramsVHACD.m_oclAcceleration = false;
+ }
+ }
+#endif //CL_VERSION_1_1
+ bool res = interfaceVHACD->Compute(&points[0], 3, (unsigned int)points.size() / 3,
+ &triangles[0], 3, (unsigned int)triangles.size() / 3, params.m_paramsVHACD);
+ if (res) {
+ // save output
+ unsigned int nConvexHulls = interfaceVHACD->GetNConvexHulls();
+ msg.str("");
+ msg << "+ Generate output: " << nConvexHulls << " convex-hulls " << endl;
+ myLogger.Log(msg.str().c_str());
+ ofstream foutCH(params.m_fileNameOut.c_str());
+ IVHACD::ConvexHull ch;
+ if (foutCH.is_open()) {
+ Material mat;
+ int vertexOffset = 1;//obj wavefront starts counting at 1...
+ for (unsigned int p = 0; p < nConvexHulls; ++p) {
+ interfaceVHACD->GetConvexHull(p, ch);
+
+
+ SaveOBJ(foutCH, ch.m_points, ch.m_triangles, ch.m_nPoints, ch.m_nTriangles, mat, myLogger, p, vertexOffset);
+ vertexOffset+=ch.m_nPoints;
+ msg.str("");
+ msg << "\t CH[" << setfill('0') << setw(5) << p << "] " << ch.m_nPoints << " V, " << ch.m_nTriangles << " T" << endl;
+ myLogger.Log(msg.str().c_str());
+ }
+ foutCH.close();
+ }
+ }
+ else {
+ myLogger.Log("Decomposition cancelled by user!\n");
+ }
+
+#ifdef CL_VERSION_1_1
+ if (params.m_paramsVHACD.m_oclAcceleration) {
+ bool res = interfaceVHACD->OCLRelease(&myLogger);
+ if (!res) {
+ assert(-1);
+ }
+ }
+#endif //CL_VERSION_1_1
+
+ interfaceVHACD->Clean();
+ interfaceVHACD->Release();
+ }
+#ifdef _CRTDBG_MAP_ALLOC
+ _CrtDumpMemoryLeaks();
+#endif // _CRTDBG_MAP_ALLOC
+ return 0;
+}
+
+void Usage(const Parameters& params)
+{
+ std::ostringstream msg;
+ msg << "V-HACD V" << VHACD_VERSION_MAJOR << "." << VHACD_VERSION_MINOR << endl;
+ msg << "Syntax: testVHACD [options] --input infile.obj --output outfile.obj --log logfile.txt" << endl
+ << endl;
+ msg << "Options:" << endl;
+ msg << " --input Wavefront .obj input file name" << endl;
+ msg << " --output VRML 2.0 output file name" << endl;
+ msg << " --log Log file name" << endl;
+ msg << " --resolution Maximum number of voxels generated during the voxelization stage (default=100,000, range=10,000-16,000,000)" << endl;
+ msg << " --depth Maximum number of clipping stages. During each split stage, parts with a concavity higher than the user defined threshold are clipped according the \"best\" clipping plane (default=20, range=1-32)" << endl;
+ msg << " --concavity Maximum allowed concavity (default=0.0025, range=0.0-1.0)" << endl;
+ msg << " --planeDownsampling Controls the granularity of the search for the \"best\" clipping plane (default=4, range=1-16)" << endl;
+ msg << " --convexhullDownsampling Controls the precision of the convex-hull generation process during the clipping plane selection stage (default=4, range=1-16)" << endl;
+ msg << " --alpha Controls the bias toward clipping along symmetry planes (default=0.05, range=0.0-1.0)" << endl;
+ msg << " --beta Controls the bias toward clipping along revolution axes (default=0.05, range=0.0-1.0)" << endl;
+ msg << " --gamma Controls the maximum allowed concavity during the merge stage (default=0.00125, range=0.0-1.0)" << endl;
+ msg << " --delta Controls the bias toward maximaxing local concavity (default=0.05, range=0.0-1.0)" << endl;
+ msg << " --pca Enable/disable normalizing the mesh before applying the convex decomposition (default=0, range={0,1})" << endl;
+ msg << " --mode 0: voxel-based approximate convex decomposition, 1: tetrahedron-based approximate convex decomposition (default=0, range={0,1})" << endl;
+ msg << " --maxNumVerticesPerCH Controls the maximum number of triangles per convex-hull (default=64, range=4-1024)" << endl;
+ msg << " --minVolumePerCH Controls the adaptive sampling of the generated convex-hulls (default=0.0001, range=0.0-0.01)" << endl;
+ msg << " --convexhullApproximation Enable/disable approximation when computing convex-hulls (default=1, range={0,1})" << endl;
+ msg << " --oclAcceleration Enable/disable OpenCL acceleration (default=0, range={0,1})" << endl;
+ msg << " --oclPlatformID OpenCL platform id (default=0, range=0-# OCL platforms)" << endl;
+ msg << " --oclDeviceID OpenCL device id (default=0, range=0-# OCL devices)" << endl;
+ msg << " --help Print usage" << endl
+ << endl;
+ msg << "Examples:" << endl;
+ msg << " testVHACD.exe --input bunny.obj --output bunny_acd.obj --log log.txt" << endl
+ << endl;
+ cout << msg.str();
+ if (params.m_paramsVHACD.m_logger) {
+ params.m_paramsVHACD.m_logger->Log(msg.str().c_str());
+ }
+}
+void ParseParameters(int argc, char* argv[], Parameters& params)
+{
+ for (int i = 1; i < argc; ++i) {
+ if (!strcmp(argv[i], "--input")) {
+ if (++i < argc)
+ params.m_fileNameIn = argv[i];
+ }
+ else if (!strcmp(argv[i], "--output")) {
+ if (++i < argc)
+ params.m_fileNameOut = argv[i];
+ }
+ else if (!strcmp(argv[i], "--log")) {
+ if (++i < argc)
+ params.m_fileNameLog = argv[i];
+ }
+ else if (!strcmp(argv[i], "--resolution")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_resolution = atoi(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--depth")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_depth = atoi(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--concavity")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_concavity = atof(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--planeDownsampling")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_planeDownsampling = atoi(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--convexhullDownsampling")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_convexhullDownsampling = atoi(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--alpha")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_alpha = atof(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--beta")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_beta = atof(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--gamma")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_gamma = atof(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--pca")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_pca = atoi(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--mode")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_mode = atoi(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--maxNumVerticesPerCH")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_maxNumVerticesPerCH = atoi(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--minVolumePerCH")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_minVolumePerCH = atof(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--convexhullApproximation")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_convexhullApproximation = atoi(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--oclAcceleration")) {
+ if (++i < argc)
+ params.m_paramsVHACD.m_oclAcceleration = atoi(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--oclPlatformID")) {
+ if (++i < argc)
+ params.m_oclPlatformID = atoi(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--oclDeviceID")) {
+ if (++i < argc)
+ params.m_oclDeviceID = atoi(argv[i]);
+ }
+ else if (!strcmp(argv[i], "--help")) {
+ params.m_run = false;
+ }
+ }
+ params.m_paramsVHACD.m_resolution = (params.m_paramsVHACD.m_resolution < 64) ? 0 : params.m_paramsVHACD.m_resolution;
+ params.m_paramsVHACD.m_planeDownsampling = (params.m_paramsVHACD.m_planeDownsampling < 1) ? 1 : params.m_paramsVHACD.m_planeDownsampling;
+ params.m_paramsVHACD.m_convexhullDownsampling = (params.m_paramsVHACD.m_convexhullDownsampling < 1) ? 1 : params.m_paramsVHACD.m_convexhullDownsampling;
+}
+
+void GetFileExtension(const string& fileName, string& fileExtension)
+{
+ size_t lastDotPosition = fileName.find_last_of(".");
+ if (lastDotPosition == string::npos) {
+ fileExtension = "";
+ }
+ else {
+ fileExtension = fileName.substr(lastDotPosition, fileName.size());
+ transform(fileExtension.begin(), fileExtension.end(), fileExtension.begin(), ::toupper);
+ }
+}
+void ComputeRandomColor(Material& mat)
+{
+ mat.m_diffuseColor[0] = mat.m_diffuseColor[1] = mat.m_diffuseColor[2] = 0.0f;
+ while (mat.m_diffuseColor[0] == mat.m_diffuseColor[1] || mat.m_diffuseColor[2] == mat.m_diffuseColor[1] || mat.m_diffuseColor[2] == mat.m_diffuseColor[0]) {
+ mat.m_diffuseColor[0] = (rand() % 100) / 100.0f;
+ mat.m_diffuseColor[1] = (rand() % 100) / 100.0f;
+ mat.m_diffuseColor[2] = (rand() % 100) / 100.0f;
+ }
+}
+bool LoadOFF(const string& fileName, vector<float>& points, vector<int>& triangles, IVHACD::IUserLogger& logger)
+{
+ FILE* fid = fopen(fileName.c_str(), "r");
+ if (fid) {
+ const string strOFF("OFF");
+ char temp[1024];
+ fscanf(fid, "%s", temp);
+ if (string(temp) != strOFF) {
+ logger.Log("Loading error: format not recognized \n");
+ fclose(fid);
+ return false;
+ }
+ else {
+ int nv = 0;
+ int nf = 0;
+ int ne = 0;
+ fscanf(fid, "%i", &nv);
+ fscanf(fid, "%i", &nf);
+ fscanf(fid, "%i", &ne);
+ points.resize(nv * 3);
+ triangles.resize(nf * 3);
+ const int np = nv * 3;
+ for (int p = 0; p < np; p++) {
+ fscanf(fid, "%f", &(points[p]));
+ }
+ int s;
+ for (int t = 0, r = 0; t < nf; ++t) {
+ fscanf(fid, "%i", &s);
+ if (s == 3) {
+ fscanf(fid, "%i", &(triangles[r++]));
+ fscanf(fid, "%i", &(triangles[r++]));
+ fscanf(fid, "%i", &(triangles[r++]));
+ }
+ else // Fix me: support only triangular meshes
+ {
+ for (int h = 0; h < s; ++h)
+ fscanf(fid, "%i", &s);
+ }
+ }
+ fclose(fid);
+ }
+ }
+ else {
+ logger.Log("Loading error: file not found \n");
+ return false;
+ }
+ return true;
+}
+bool LoadOBJ(const string& fileName, vector<float>& points, vector<int>& triangles, IVHACD::IUserLogger& logger)
+{
+ const unsigned int BufferSize = 1024;
+ FILE* fid = fopen(fileName.c_str(), "r");
+
+ if (fid) {
+ char buffer[BufferSize];
+ int ip[4];
+ float x[3];
+ char* pch;
+ char* str;
+ while (!feof(fid)) {
+ if (!fgets(buffer, BufferSize, fid)) {
+ break;
+ }
+ else if (buffer[0] == 'v') {
+ if (buffer[1] == ' ') {
+ str = buffer + 2;
+ for (int k = 0; k < 3; ++k) {
+ pch = strtok(str, " ");
+ if (pch)
+ x[k] = (float)atof(pch);
+ else {
+ return false;
+ }
+ str = NULL;
+ }
+ points.push_back(x[0]);
+ points.push_back(x[1]);
+ points.push_back(x[2]);
+ }
+ }
+ else if (buffer[0] == 'f') {
+
+ pch = str = buffer + 2;
+ int k = 0;
+ while (pch) {
+ pch = strtok(str, " ");
+ if (pch) {
+ ip[k++] = atoi(pch) - 1;
+ }
+ else {
+ break;
+ }
+ str = NULL;
+ }
+ if (k == 3) {
+ triangles.push_back(ip[0]);
+ triangles.push_back(ip[1]);
+ triangles.push_back(ip[2]);
+ }
+ else if (k == 4) {
+ triangles.push_back(ip[0]);
+ triangles.push_back(ip[1]);
+ triangles.push_back(ip[2]);
+
+ triangles.push_back(ip[0]);
+ triangles.push_back(ip[2]);
+ triangles.push_back(ip[3]);
+ }
+ }
+ }
+ fclose(fid);
+ }
+ else {
+ logger.Log("File not found\n");
+ return false;
+ }
+ return true;
+}
+bool SaveOFF(const string& fileName, const float* const& points, const int* const& triangles, const unsigned int& nPoints,
+ const unsigned int& nTriangles, IVHACD::IUserLogger& logger)
+{
+ ofstream fout(fileName.c_str());
+ if (fout.is_open()) {
+ size_t nV = nPoints * 3;
+ size_t nT = nTriangles * 3;
+ fout << "OFF" << std::endl;
+ fout << nPoints << " " << nTriangles << " " << 0 << std::endl;
+ for (size_t v = 0; v < nV; v += 3) {
+ fout << points[v + 0] << " "
+ << points[v + 1] << " "
+ << points[v + 2] << std::endl;
+ }
+ for (size_t f = 0; f < nT; f += 3) {
+ fout << "3 " << triangles[f + 0] << " "
+ << triangles[f + 1] << " "
+ << triangles[f + 2] << std::endl;
+ }
+ fout.close();
+ return true;
+ }
+ else {
+ logger.Log("Can't open file\n");
+ return false;
+ }
+}
+
+bool SaveOBJ(ofstream& fout, const double* const& points, const int* const& triangles, const unsigned int& nPoints,
+ const unsigned int& nTriangles, const Material& material, IVHACD::IUserLogger& logger, int convexPart, int vertexOffset)
+{
+ if (fout.is_open()) {
+
+ fout.setf(std::ios::fixed, std::ios::floatfield);
+ fout.setf(std::ios::showpoint);
+ fout.precision(6);
+ size_t nV = nPoints * 3;
+ size_t nT = nTriangles * 3;
+
+ fout << "o convex_" << convexPart << std::endl;
+
+ if (nV > 0) {
+ for (size_t v = 0; v < nV; v += 3) {
+ fout << "v " << points[v + 0] << " " << points[v + 1] << " " << points[v + 2] << std::endl;
+ }
+ }
+ if (nT > 0) {
+ for (size_t f = 0; f < nT; f += 3) {
+ fout << "f "
+ << triangles[f + 0]+vertexOffset << " "
+ << triangles[f + 1]+vertexOffset << " "
+ << triangles[f + 2]+vertexOffset << " " << std::endl;
+ }
+ }
+ return true;
+ }
+ else {
+ logger.Log("Can't open file\n");
+ return false;
+ }
+}
+
+
+
+bool SaveVRML2(ofstream& fout, const double* const& points, const int* const& triangles, const unsigned int& nPoints,
+ const unsigned int& nTriangles, const Material& material, IVHACD::IUserLogger& logger)
+{
+ if (fout.is_open()) {
+ fout.setf(std::ios::fixed, std::ios::floatfield);
+ fout.setf(std::ios::showpoint);
+ fout.precision(6);
+ size_t nV = nPoints * 3;
+ size_t nT = nTriangles * 3;
+ fout << "#VRML V2.0 utf8" << std::endl;
+ fout << "" << std::endl;
+ fout << "# Vertices: " << nPoints << std::endl;
+ fout << "# Triangles: " << nTriangles << std::endl;
+ fout << "" << std::endl;
+ fout << "Group {" << std::endl;
+ fout << " children [" << std::endl;
+ fout << " Shape {" << std::endl;
+ fout << " appearance Appearance {" << std::endl;
+ fout << " material Material {" << std::endl;
+ fout << " diffuseColor " << material.m_diffuseColor[0] << " "
+ << material.m_diffuseColor[1] << " "
+ << material.m_diffuseColor[2] << std::endl;
+ fout << " ambientIntensity " << material.m_ambientIntensity << std::endl;
+ fout << " specularColor " << material.m_specularColor[0] << " "
+ << material.m_specularColor[1] << " "
+ << material.m_specularColor[2] << std::endl;
+ fout << " emissiveColor " << material.m_emissiveColor[0] << " "
+ << material.m_emissiveColor[1] << " "
+ << material.m_emissiveColor[2] << std::endl;
+ fout << " shininess " << material.m_shininess << std::endl;
+ fout << " transparency " << material.m_transparency << std::endl;
+ fout << " }" << std::endl;
+ fout << " }" << std::endl;
+ fout << " geometry IndexedFaceSet {" << std::endl;
+ fout << " ccw TRUE" << std::endl;
+ fout << " solid TRUE" << std::endl;
+ fout << " convex TRUE" << std::endl;
+ if (nV > 0) {
+ fout << " coord DEF co Coordinate {" << std::endl;
+ fout << " point [" << std::endl;
+ for (size_t v = 0; v < nV; v += 3) {
+ fout << " " << points[v + 0] << " "
+ << points[v + 1] << " "
+ << points[v + 2] << "," << std::endl;
+ }
+ fout << " ]" << std::endl;
+ fout << " }" << std::endl;
+ }
+ if (nT > 0) {
+ fout << " coordIndex [ " << std::endl;
+ for (size_t f = 0; f < nT; f += 3) {
+ fout << " " << triangles[f + 0] << ", "
+ << triangles[f + 1] << ", "
+ << triangles[f + 2] << ", -1," << std::endl;
+ }
+ fout << " ]" << std::endl;
+ }
+ fout << " }" << std::endl;
+ fout << " }" << std::endl;
+ fout << " ]" << std::endl;
+ fout << "}" << std::endl;
+ return true;
+ }
+ else {
+ logger.Log("Can't open file\n");
+ return false;
+ }
+}
diff --git a/Extras/VHACD/test/src/oclHelper.cpp b/Extras/VHACD/test/src/oclHelper.cpp
new file mode 100644
index 0000000..efaa9fa
--- /dev/null
+++ b/Extras/VHACD/test/src/oclHelper.cpp
@@ -0,0 +1,329 @@
+/* Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+All rights reserved.
+
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+2. 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.
+
+3. The names of the contributors may not 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 HOLDER 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 PROF [...]
+*/
+#ifdef OPENCL_FOUND
+
+#include "oclHelper.h"
+#include <assert.h>
+#include <sstream>
+
+bool OCLHelper::InitPlatform(const unsigned int platformIndex)
+{
+ cl_uint numPlatforms;
+ m_lastError = clGetPlatformIDs(1, NULL, &numPlatforms);
+ if (m_lastError != CL_SUCCESS || platformIndex >= numPlatforms)
+ return false;
+
+ cl_platform_id* platforms = new cl_platform_id[numPlatforms];
+ m_lastError = clGetPlatformIDs(numPlatforms, platforms, NULL);
+ if (m_lastError != CL_SUCCESS) {
+ delete[] platforms;
+ return false;
+ }
+
+ m_platform = platforms[platformIndex];
+ delete[] platforms;
+ return true;
+}
+bool OCLHelper::GetPlatformsInfo(std::vector<std::string>& info, const std::string& indentation)
+{
+ const char* platformInfoParameters[] = { "CL_PLATFORM_NAME",
+ "CL_PLATFORM_VENDOR",
+ "CL_PLATFORM_VERSION",
+ "CL_PLATFORM_PROFILE",
+ "CL_PLATFORM_EXTENSIONS" };
+
+ cl_uint numPlatforms;
+ m_lastError = clGetPlatformIDs(1, NULL, &numPlatforms);
+ if (m_lastError != CL_SUCCESS)
+ return false;
+
+ cl_platform_id* platforms = new cl_platform_id[numPlatforms];
+ m_lastError = clGetPlatformIDs(numPlatforms, platforms, NULL);
+ if (m_lastError != CL_SUCCESS) {
+ delete[] platforms;
+ return false;
+ }
+
+ size_t bufferSize = 4096;
+ char* buffer = new char[bufferSize];
+ size_t size;
+ info.resize(numPlatforms);
+ for (cl_uint i = 0; i < numPlatforms; ++i) {
+ for (int j = CL_PLATFORM_PROFILE; j <= CL_PLATFORM_EXTENSIONS; ++j) {
+ info[i] += indentation + platformInfoParameters[j - CL_PLATFORM_PROFILE] + std::string(": ");
+ m_lastError = clGetPlatformInfo(platforms[i], j, 0, NULL, &size);
+ if (m_lastError != CL_SUCCESS) {
+ delete[] buffer;
+ delete[] platforms;
+ return false;
+ }
+ if (bufferSize < size) {
+ delete[] buffer;
+ bufferSize = size;
+ buffer = new char[bufferSize];
+ }
+ m_lastError = clGetPlatformInfo(platforms[i], j, size, buffer, NULL);
+ if (m_lastError != CL_SUCCESS) {
+ delete[] buffer;
+ delete[] platforms;
+ return false;
+ }
+ info[i] += buffer + std::string("\n");
+ }
+ }
+ delete[] platforms;
+ delete[] buffer;
+ return true;
+}
+bool OCLHelper::InitDevice(const unsigned int deviceIndex)
+{
+ cl_uint numDevices;
+ m_lastError = clGetDeviceIDs(m_platform, CL_DEVICE_TYPE_ALL, 0, NULL, &numDevices);
+ if (m_lastError != CL_SUCCESS || deviceIndex >= numDevices)
+ return false;
+
+ cl_device_id* devices = new cl_device_id[numDevices];
+ m_lastError = clGetDeviceIDs(m_platform, CL_DEVICE_TYPE_ALL, numDevices, devices, NULL);
+ if (m_lastError != CL_SUCCESS) {
+ delete[] devices;
+ return false;
+ }
+ m_device = devices[deviceIndex];
+ delete[] devices;
+ return true;
+}
+bool OCLHelper::GetDevicesInfo(std::vector<std::string>& info, const std::string& indentation)
+{
+ enum {
+ DATA_TYPE_CL_UINT,
+ DATA_TYPE_CL_BOOL,
+ DATA_TYPE_STRING,
+ DATA_TYPE_CL_ULONG,
+ DATA_TYPE_CL_DEVICE_FP_CONFIG,
+ DATA_TYPE_CL_DEVICE_EXEC_CAPABILITIES,
+ DATA_TYPE_CL_DEVICE_MEM_CACHE_TYPE,
+ DATA_TYPE_CL_DEVICE_MEM_LOCAL_TYPE,
+ DATA_TYPE_CL_DEVICE_CMD_QUEUE_PROP,
+ DATA_TYPE_CL_DEVICE_TYPE,
+ DATA_TYPE_SIZE_T,
+ DATA_TYPE_SIZE_T_3,
+ };
+ typedef struct
+ {
+ cl_device_info id;
+ const char* name;
+ int type;
+ } DeviceInfoParam;
+ const int numDeviceInfoParameters = 49;
+ const DeviceInfoParam deviceInfoParameters[numDeviceInfoParameters] = {
+ { CL_DEVICE_NAME, "CL_DEVICE_NAME", DATA_TYPE_STRING },
+ { CL_DEVICE_PROFILE, "CL_DEVICE_PROFILE", DATA_TYPE_STRING },
+ { CL_DEVICE_VENDOR, "CL_DEVICE_VENDOR", DATA_TYPE_STRING },
+ { CL_DEVICE_VERSION, "CL_DEVICE_VERSION", DATA_TYPE_STRING },
+ { CL_DRIVER_VERSION, "CL_DRIVER_VERSION", DATA_TYPE_STRING },
+ { CL_DEVICE_EXTENSIONS, "CL_DEVICE_EXTENSIONS", DATA_TYPE_STRING },
+ { CL_DEVICE_VERSION, "CL_DEVICE_VERSION", DATA_TYPE_STRING },
+ { CL_DEVICE_ADDRESS_BITS, "CL_DEVICE_ADDRESS_BITS", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_GLOBAL_MEM_CACHELINE_SIZE, "CL_DEVICE_GLOBAL_MEM_CACHELINE_SIZE", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_MAX_CLOCK_FREQUENCY, "CL_DEVICE_MAX_CLOCK_FREQUENCY", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_MAX_COMPUTE_UNITS, "CL_DEVICE_MAX_COMPUTE_UNITS", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_MAX_CONSTANT_ARGS, "CL_DEVICE_MAX_CONSTANT_ARGS", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_MAX_READ_IMAGE_ARGS, "CL_DEVICE_MAX_READ_IMAGE_ARGS", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_MAX_SAMPLERS, "CL_DEVICE_MAX_SAMPLERS", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_MAX_WORK_ITEM_DIMENSIONS, "CL_DEVICE_MAX_WORK_ITEM_DIMENSIONS", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_MAX_WRITE_IMAGE_ARGS, "CL_DEVICE_MAX_WRITE_IMAGE_ARGS", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_MEM_BASE_ADDR_ALIGN, "CL_DEVICE_MEM_BASE_ADDR_ALIGN", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_MIN_DATA_TYPE_ALIGN_SIZE, "CL_DEVICE_MIN_DATA_TYPE_ALIGN_SIZE", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_PREFERRED_VECTOR_WIDTH_CHAR, "CL_DEVICE_PREFERRED_VECTOR_WIDTH_CHAR", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_PREFERRED_VECTOR_WIDTH_SHORT, "CL_DEVICE_PREFERRED_VECTOR_WIDTH_SHORT", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_PREFERRED_VECTOR_WIDTH_INT, "CL_DEVICE_PREFERRED_VECTOR_WIDTH_INT", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_PREFERRED_VECTOR_WIDTH_LONG, "CL_DEVICE_PREFERRED_VECTOR_WIDTH_LONG", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_PREFERRED_VECTOR_WIDTH_FLOAT, "CL_DEVICE_PREFERRED_VECTOR_WIDTH_FLOAT", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_PREFERRED_VECTOR_WIDTH_DOUBLE, "CL_DEVICE_PREFERRED_VECTOR_WIDTH_DOUBLE", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_VENDOR_ID, "CL_DEVICE_VENDOR_ID", DATA_TYPE_CL_UINT },
+ { CL_DEVICE_AVAILABLE, "CL_DEVICE_AVAILABLE", DATA_TYPE_CL_BOOL },
+ { CL_DEVICE_COMPILER_AVAILABLE, "CL_DEVICE_COMPILER_AVAILABLE", DATA_TYPE_CL_BOOL },
+ { CL_DEVICE_ENDIAN_LITTLE, "CL_DEVICE_ENDIAN_LITTLE", DATA_TYPE_CL_BOOL },
+ { CL_DEVICE_ERROR_CORRECTION_SUPPORT, "CL_DEVICE_ERROR_CORRECTION_SUPPORT", DATA_TYPE_CL_BOOL },
+ { CL_DEVICE_IMAGE_SUPPORT, "CL_DEVICE_IMAGE_SUPPORT", DATA_TYPE_CL_BOOL },
+ { CL_DEVICE_EXECUTION_CAPABILITIES, "CL_DEVICE_EXECUTION_CAPABILITIES", DATA_TYPE_CL_DEVICE_EXEC_CAPABILITIES },
+ { CL_DEVICE_GLOBAL_MEM_CACHE_SIZE, "CL_DEVICE_GLOBAL_MEM_CACHE_SIZE", DATA_TYPE_CL_ULONG },
+ { CL_DEVICE_GLOBAL_MEM_SIZE, "CL_DEVICE_GLOBAL_MEM_SIZE", DATA_TYPE_CL_ULONG },
+ { CL_DEVICE_LOCAL_MEM_SIZE, "CL_DEVICE_LOCAL_MEM_SIZE", DATA_TYPE_CL_ULONG },
+ { CL_DEVICE_MAX_CONSTANT_BUFFER_SIZE, "CL_DEVICE_MAX_CONSTANT_BUFFER_SIZE", DATA_TYPE_CL_ULONG },
+ { CL_DEVICE_MAX_MEM_ALLOC_SIZE, "CL_DEVICE_MAX_MEM_ALLOC_SIZE", DATA_TYPE_CL_ULONG },
+ { CL_DEVICE_GLOBAL_MEM_CACHE_TYPE, "CL_DEVICE_GLOBAL_MEM_CACHE_TYPE", DATA_TYPE_CL_DEVICE_MEM_CACHE_TYPE },
+ { CL_DEVICE_IMAGE2D_MAX_HEIGHT, "CL_DEVICE_IMAGE2D_MAX_HEIGHT", DATA_TYPE_SIZE_T },
+ { CL_DEVICE_IMAGE2D_MAX_WIDTH, "CL_DEVICE_IMAGE2D_MAX_WIDTH", DATA_TYPE_SIZE_T },
+ { CL_DEVICE_IMAGE3D_MAX_DEPTH, "CL_DEVICE_IMAGE3D_MAX_DEPTH", DATA_TYPE_SIZE_T },
+ { CL_DEVICE_IMAGE3D_MAX_HEIGHT, "CL_DEVICE_IMAGE3D_MAX_HEIGHT", DATA_TYPE_SIZE_T },
+ { CL_DEVICE_IMAGE3D_MAX_WIDTH, "CL_DEVICE_IMAGE3D_MAX_WIDTH", DATA_TYPE_SIZE_T },
+ { CL_DEVICE_MAX_PARAMETER_SIZE, "CL_DEVICE_MAX_PARAMETER_SIZE", DATA_TYPE_SIZE_T },
+ { CL_DEVICE_MAX_WORK_GROUP_SIZE, "CL_DEVICE_MAX_WORK_GROUP_SIZE", DATA_TYPE_SIZE_T },
+ { CL_DEVICE_PROFILING_TIMER_RESOLUTION, "CL_DEVICE_PROFILING_TIMER_RESOLUTION", DATA_TYPE_SIZE_T },
+ { CL_DEVICE_QUEUE_PROPERTIES, "CL_DEVICE_QUEUE_PROPERTIES", DATA_TYPE_CL_DEVICE_CMD_QUEUE_PROP },
+ { CL_DEVICE_TYPE, "CL_DEVICE_TYPE", DATA_TYPE_CL_DEVICE_TYPE },
+ { CL_DEVICE_LOCAL_MEM_TYPE, "CL_DEVICE_LOCAL_MEM_TYPE", DATA_TYPE_CL_DEVICE_MEM_LOCAL_TYPE },
+ { CL_DEVICE_MAX_WORK_ITEM_SIZES, "CL_DEVICE_MAX_WORK_ITEM_SIZES", DATA_TYPE_SIZE_T_3 }
+ // { CL_DEVICE_DOUBLE_FP_CONFIG, "CL_DEVICE_DOUBLE_FP_CONFIG", DATA_TYPE_CL_DEVICE_FP_CONFIG },
+ //
+ };
+ cl_uint numDevices;
+ m_lastError = clGetDeviceIDs(m_platform, CL_DEVICE_TYPE_ALL, 0, NULL, &numDevices);
+ if (m_lastError != CL_SUCCESS)
+ return false;
+
+ cl_device_id* devices = new cl_device_id[numDevices];
+ m_lastError = clGetDeviceIDs(m_platform, CL_DEVICE_TYPE_ALL, numDevices, devices, NULL);
+ if (m_lastError != CL_SUCCESS) {
+ delete[] devices;
+ return false;
+ }
+ size_t bufferSize = 4096;
+ char* buffer = new char[bufferSize];
+ size_t size;
+ info.resize(numDevices);
+
+ for (cl_uint i = 0; i < numDevices; ++i) {
+ for (int j = 0; j < numDeviceInfoParameters; ++j) {
+ const DeviceInfoParam& infoParam = deviceInfoParameters[j];
+ info[i] += indentation + infoParam.name + std::string(": ");
+
+ if (infoParam.type == DATA_TYPE_STRING) {
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, 0, NULL, &size);
+ if (m_lastError == CL_SUCCESS) {
+ if (bufferSize < size) {
+ delete[] buffer;
+ bufferSize = size;
+ buffer = new char[bufferSize];
+ }
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, size, buffer, NULL);
+ if (m_lastError != CL_SUCCESS) {
+ delete[] devices;
+ delete[] buffer;
+ return false;
+ }
+ info[i] += buffer + std::string("\n");
+ }
+ }
+ else if (infoParam.type == DATA_TYPE_CL_UINT) {
+ cl_uint value;
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, sizeof(cl_uint), &value, &size);
+ if (m_lastError == CL_SUCCESS) {
+ std::ostringstream svalue;
+ svalue << value;
+ info[i] += svalue.str() + "\n";
+ }
+ }
+ else if (infoParam.type == DATA_TYPE_CL_BOOL) {
+ cl_bool value;
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, sizeof(cl_bool), &value, &size);
+ if (m_lastError == CL_SUCCESS) {
+ std::ostringstream svalue;
+ svalue << value;
+ info[i] += svalue.str() + "\n";
+ }
+ }
+ else if (infoParam.type == DATA_TYPE_CL_ULONG) {
+ cl_ulong value;
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, sizeof(cl_ulong), &value, &size);
+ if (m_lastError == CL_SUCCESS) {
+ std::ostringstream svalue;
+ svalue << value;
+ info[i] += svalue.str() + "\n";
+ }
+ }
+ else if (infoParam.type == DATA_TYPE_CL_DEVICE_FP_CONFIG) {
+ cl_device_fp_config value;
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, sizeof(cl_device_fp_config), &value, &size);
+ if (m_lastError == CL_SUCCESS) {
+ std::ostringstream svalue;
+ svalue << value;
+ info[i] += svalue.str() + "\n";
+ }
+ }
+ else if (infoParam.type == DATA_TYPE_CL_DEVICE_EXEC_CAPABILITIES) {
+ cl_device_exec_capabilities value;
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, sizeof(cl_device_exec_capabilities), &value, &size);
+ if (m_lastError == CL_SUCCESS) {
+ std::ostringstream svalue;
+ svalue << value;
+ info[i] += svalue.str() + "\n";
+ }
+ }
+ else if (infoParam.type == DATA_TYPE_CL_DEVICE_MEM_CACHE_TYPE) {
+ cl_device_mem_cache_type value;
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, sizeof(cl_device_mem_cache_type), &value, &size);
+ if (m_lastError == CL_SUCCESS) {
+ std::ostringstream svalue;
+ svalue << value;
+ info[i] += svalue.str() + "\n";
+ }
+ }
+ else if (infoParam.type == DATA_TYPE_CL_DEVICE_MEM_LOCAL_TYPE) {
+ cl_device_local_mem_type value;
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, sizeof(cl_device_local_mem_type), &value, &size);
+ if (m_lastError == CL_SUCCESS) {
+ std::ostringstream svalue;
+ svalue << value;
+ info[i] += svalue.str() + "\n";
+ }
+ }
+ else if (infoParam.type == DATA_TYPE_CL_DEVICE_CMD_QUEUE_PROP) {
+ cl_command_queue_properties value;
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, sizeof(cl_command_queue_properties), &value, &size);
+ if (m_lastError == CL_SUCCESS) {
+ std::ostringstream svalue;
+ svalue << value;
+ info[i] += svalue.str() + "\n";
+ }
+ }
+ else if (infoParam.type == DATA_TYPE_CL_DEVICE_TYPE) {
+ cl_device_type value;
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, sizeof(cl_device_type), &value, &size);
+ if (m_lastError == CL_SUCCESS) {
+ std::ostringstream svalue;
+ svalue << value;
+ info[i] += svalue.str() + "\n";
+ }
+ }
+ else if (infoParam.type == DATA_TYPE_SIZE_T) {
+ size_t value;
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, sizeof(size_t), &value, &size);
+ if (m_lastError == CL_SUCCESS) {
+ std::ostringstream svalue;
+ svalue << value;
+ info[i] += svalue.str() + "\n";
+ }
+ }
+ else if (infoParam.type == DATA_TYPE_SIZE_T_3) {
+ size_t value[3];
+ m_lastError = clGetDeviceInfo(devices[i], infoParam.id, 3 * sizeof(size_t), &value, &size);
+ if (m_lastError == CL_SUCCESS) {
+ std::ostringstream svalue;
+ svalue << "(" << value[0] << ", " << value[1] << ", " << value[2] << ")";
+ info[i] += svalue.str() + "\n";
+ }
+ }
+ else {
+ assert(0);
+ }
+ }
+ }
+ delete[] devices;
+ delete[] buffer;
+ return true;
+}
+#endif // OPENCL_FOUND
diff --git a/Extras/VHACD/test/src/premake4.lua b/Extras/VHACD/test/src/premake4.lua
new file mode 100644
index 0000000..a329f54
--- /dev/null
+++ b/Extras/VHACD/test/src/premake4.lua
@@ -0,0 +1,25 @@
+
+project "test_vhacd"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+
+includedirs {"../../public"}
+
+links {
+ "vhacd"
+}
+
+language "C++"
+
+files {
+ "main.cpp",
+}
+
+
+if os.is("Linux") then
+ links {"pthread"}
+end
\ No newline at end of file
diff --git a/Extras/premake4.lua b/Extras/premake4.lua
index dd4853f..c366078 100644
--- a/Extras/premake4.lua
+++ b/Extras/premake4.lua
@@ -1,5 +1,6 @@
include "HACD"
+include "VHACD"
include "ConvexDecomposition"
include "InverseDynamics"
include "Serialize/BulletFileLoader"
diff --git a/README.md b/README.md
index 2f254a0..b5b9737 100644
--- a/README.md
+++ b/README.md
@@ -2,18 +2,19 @@
[](https://travis-ci.org/bulletphysics/bullet3)
[](https://ci.appveyor.com/project/erwincoumans/bullet3)
-# Bullet 2.x including experimental Bullet 3 OpenCL.
+# Bullet 2.x including pybullet, Virtual Reality support
This is the official repository of Bullet 2.x, moved from http://bullet.googlecode.com
-It includes the optional work-in-progress Bullet 3 GPU pipeline.
+It includes the optional experimental Bullet 3 GPU pipeline.
-The Bullet 2 API will stay default and up-to-date while slowly moving to Bullet 3.
-The steps towards Bullet 3 are in a nutshell:
+The Bullet 2 API will stay default and up-to-date while slowly moving to a new API.
+The steps towards a new API is in a nutshell:
1. The old Bullet2 demos are being merged into the examples/ExampleBrowser
-2. A new Bullet 3 API is created
-3. All Bullet2 functionality is added to Bullet 3.
-4. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl
+2. A new physics-engine agnostic C-API is created, see examples/SharedMemory/PhysicsClientC_API.h
+3. Python bindings in pybullet are on top of this C-API, see examples/pybullet
+4. A Virtual Reality sandbox using openvr for HTC Vive and Oculus Rift is available
+5. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl
You can still use svn or svn externals using the github git repository: use svn co https://github.com/bulletphysics/bullet3/trunk
@@ -34,7 +35,7 @@ The entire collision detection and rigid body dynamics is executed on the GPU.
A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better.
We succesfully tested the software under Windows, Linux and Mac OSX.
The software currently doesn't work on OpenCL CPU devices. It might run
-on a laptop GPU but performance is likely not very good. Note that
+on a laptop GPU but performance will not likely be very good. Note that
often an OpenCL drivers fails to compile a kernel. Some unit tests exist to
track down the issue, but more work is required to cover all OpenCL kernels.
@@ -47,7 +48,19 @@ All source code files are licensed under the permissive zlib license
**Windows**
-Click on build3/vs2010.bat and open build3/vs2010/0MySolution.sln
+Click on build_visual_studio.bat and open build3/vs2010/0MySolution.sln
+
+**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift**
+
+Click on build_visual_studio_vr_pybullet_double.bat and open build3/vs2010/0MySolution.sln
+Edit this batch file to choose where Python include/lib directories are located.
+Build and run the App_SharedMemoryPhysics_VR project, preferably in Release/optimized build.
+You can connect from Python pybullet to the sandbox using:
+
+```
+import pybullet as p
+p.connect(p.SHARED_MEMORY)
+```
**Linux and Mac OSX gnu make**
@@ -55,7 +68,7 @@ In a terminal type:
cd build3
-Dependend on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines
+Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines
./premake4_linux gmake
./premake4_linux64 gmake
@@ -80,15 +93,14 @@ You can just run it though a terminal/command prompt, or by clicking it.
```
[--start_demo_name="Demo Name"] Start with a selected demo
-[--enable_experimental_opencl] Enable some experimental OpenCL examples
[--mp4=moviename.mp4] Create a mp4 movie of the window, requires ffmpeg installed
[--mouse_move_multiplier=0.400000] Set the mouse move sensitivity
[--mouse_wheel_multiplier=0.01] Set the mouse wheel sensitivity
-[--background_color_red= 0.9] Set the red component for background color. Same for green and blue.
+[--background_color_red= 0.9] Set the red component for background color. Same for green and blue
[--fixed_timestep= 0.0] Use either a real-time delta time (0.0) or a fixed step size (0.016666)
```
-You can use mouse picking to grab objects. When holding the ALT of CONTROL key, you have Maya style camera mouse controls.
-Press F1 to create series of screenshot. Hit ESCAPE to exit the demo app.
+You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls.
+Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app.
See docs folder for further information.
diff --git a/VERSION b/VERSION
index 660a62c..7cf322c 100644
--- a/VERSION
+++ b/VERSION
@@ -1 +1 @@
-2.84
+2.85
diff --git a/appveyor.yml b/appveyor.yml
index 55af07e..42a1c32 100644
--- a/appveyor.yml
+++ b/appveyor.yml
@@ -2,8 +2,8 @@ build:
project: build3/vs2010/0_Bullet3Solution.sln
build_script:
- - mkdir cmake
- - cd cmake
+ - mkdir cm
+ - cd cm
- cmake .. -G"Visual Studio 14 2015 Win64"
- cmake --build . --target ALL_BUILD --config Release -- /maxcpucount:4 /verbosity:quiet
diff --git a/build_and_run_cmake.sh b/build_and_run_cmake.sh
new file mode 100755
index 0000000..13cb76c
--- /dev/null
+++ b/build_and_run_cmake.sh
@@ -0,0 +1,7 @@
+#!/bin/sh
+rm CMakeCache.txt
+mkdir build_cmake
+cd build_cmake
+cmake -DBUILD_PYBULLET=OFF -DCMAKE_BUILD_TYPE=Release ..
+make -j12
+examples/ExampleBrowser/App_ExampleBrowser
diff --git a/build_and_run_premake.sh b/build_and_run_premake.sh
new file mode 100755
index 0000000..ba43a6a
--- /dev/null
+++ b/build_and_run_premake.sh
@@ -0,0 +1,7 @@
+#!/bin/sh
+cd build3
+./premake4_linux64 gmake
+./premake4_osx gmake
+cd gmake
+make -j12
+../../bin/App_BulletExampleBrowser_gmake_x64_release
diff --git a/build_visual_studio.bat b/build_visual_studio.bat
new file mode 100644
index 0000000..d8a85bc
--- /dev/null
+++ b/build_visual_studio.bat
@@ -0,0 +1,5 @@
+
+
+cd build3
+premake4 --targetdir="../bin" vs2010
+start vs2010
diff --git a/build_visual_studio_vr_pybullet_double.bat b/build_visual_studio_vr_pybullet_double.bat
new file mode 100644
index 0000000..71d6761
--- /dev/null
+++ b/build_visual_studio_vr_pybullet_double.bat
@@ -0,0 +1,8 @@
+
+IF NOT EXIST bin mkdir bin
+IF NOT EXIST bin\openvr_api.dll copy examples\ThirdPartyLibs\openvr\bin\win32\openvr_api.dll bin
+
+cd build3
+
+premake4 --double --enable_openvr --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --targetdir="../bin" vs2010
+start vs2010
diff --git a/data/Quadrotor/quadrotor.urdf b/data/Quadrotor/quadrotor.urdf
new file mode 100644
index 0000000..7158dc4
--- /dev/null
+++ b/data/Quadrotor/quadrotor.urdf
@@ -0,0 +1,62 @@
+<?xml version="1.0" ?>
+
+<!-- adapted from Daniel Mellinger, Nathan Michael, Vijay Kumar, "Trajectory Generation and Control for Precise Aggressive Maneuvers with Quadrotors" -->
+
+<robot xmlns="http://drake.mit.edu"
+ xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:schemaLocation="http://drake.mit.edu ../../../../pods/drake/doc/drakeURDF.xsd" name="quadrotor">
+ <link name="base_link">
+ <inertial>
+ <mass value="0.5"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.0023" ixy="0.0" ixz="0.0" iyy="0.0023" iyz="0.0" izz="0.004"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="quadrotor_base.obj" scale=".1 .1 .1"/>
+ </geometry>
+ </visual>
+ <!-- note: the original hector quadrotor urdf had a (simplified, but still complex) collision mesh, too -->
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder radius=".3" length=".1"/>
+ </geometry>
+ </collision>
+ </link>
+ <frame link="base_link" name="body" rpy="0 0 0" xyz="0 0 0" />
+ <force_element name="prop1">
+ <propellor lower_limit="0" upper_limit="10" scale_factor_thrust="1.0" scale_factor_moment="0.0245">
+ <parent link="base_link"/>
+ <origin xyz=".1750 0 0"/>
+ <axis xyz="0 0 1"/>
+ </propellor>
+ </force_element>
+
+ <force_element name="prop2">
+ <propellor lower_limit="0" upper_limit="10" scale_factor_thrust="1.0" scale_factor_moment="-0.0245">
+ <parent link="base_link"/>
+ <origin xyz="0 .1750 0 "/>
+ <axis xyz="0 0 1"/>
+ </propellor>
+ </force_element>
+
+ <force_element name="prop3">
+ <propellor lower_limit="0" upper_limit="10" scale_factor_thrust="1.0" scale_factor_moment="0.0245">
+ <parent link="base_link"/>
+ <origin xyz="-.1750 0 0"/>
+ <axis xyz="0 0 1"/>
+ </propellor>
+ </force_element>
+
+ <force_element name="prop4">
+ <propellor lower_limit="0" upper_limit="10" scale_factor_thrust="1.0" scale_factor_moment="-0.0245">
+ <parent link="base_link"/>
+ <origin xyz="0 -.1750 0"/>
+ <axis xyz="0 0 1"/>
+ </propellor>
+ </force_element>
+
+</robot>
+
diff --git a/data/Quadrotor/quadrotor_base.obj b/data/Quadrotor/quadrotor_base.obj
new file mode 100644
index 0000000..dc5129f
--- /dev/null
+++ b/data/Quadrotor/quadrotor_base.obj
@@ -0,0 +1,1696 @@
+####
+#
+# OBJ File Generated by Meshlab
+#
+####
+# Object quadrotor_base.obj
+#
+# Vertices: 300
+# Faces: 1080
+#
+####
+vn 4.712352 -0.000018 -5.804880
+v 0.823618 0.673869 -1.366201
+vn -4.712348 0.000002 -5.804878
+v -0.823618 0.673869 -1.366201
+vn -4.712421 4.104710 -4.104690
+v -0.823618 0.685264 -1.361481
+vn 4.712396 4.104707 -4.104671
+v 0.823618 0.685264 -1.361481
+vn -4.712366 5.804888 0.000002
+v -0.823618 0.689984 -1.350086
+vn 4.712383 5.804898 0.000024
+v 0.823618 0.689984 -1.350086
+vn -4.712404 4.104681 4.104702
+v -0.823618 0.685264 -1.338691
+vn 4.712404 4.104684 4.104700
+v 0.823618 0.685264 -1.338691
+vn -4.712383 -0.000002 5.804899
+v -0.823618 0.673869 -1.333971
+vn 4.712387 0.000007 5.804901
+v 0.823618 0.673869 -1.333971
+vn -4.712386 -4.104694 4.104674
+v -0.823618 0.662474 -1.338691
+vn 4.712373 -4.104692 4.104666
+v 0.823618 0.662474 -1.338691
+vn -4.712401 -5.804910 -0.000002
+v -0.823618 0.657754 -1.350086
+vn 4.712401 -5.804910 -0.000024
+v 0.823618 0.657754 -1.350086
+vn -4.712403 -4.104718 -4.104666
+v -0.823618 0.662474 -1.361481
+vn 4.712417 -4.104721 -4.104674
+v 0.823618 0.662474 -1.361481
+vn 12.566370 0.000000 0.000000
+v 0.823618 0.673869 -1.350086
+vn -12.566372 0.000000 0.000000
+v -0.823618 0.673869 -1.350086
+vn 12.566370 0.000000 0.000000
+v 0.823617 -0.673869 -1.350086
+vn 4.712387 4.104727 -4.104664
+v 0.823617 -0.662474 -1.361481
+vn 4.712359 -0.000001 -5.804899
+v 0.823617 -0.673869 -1.366201
+vn -12.566370 0.000000 0.000000
+v -0.823618 -0.673869 -1.350086
+vn -4.712344 0.000012 -5.804890
+v -0.823618 -0.673869 -1.366201
+vn -4.712438 4.104721 -4.104712
+v -0.823618 -0.662474 -1.361481
+vn 4.712419 5.804935 0.000004
+v 0.823617 -0.657754 -1.350086
+vn -4.712384 5.804914 -0.000056
+v -0.823618 -0.657754 -1.350086
+vn 4.712369 4.104695 4.104680
+v 0.823617 -0.662474 -1.338691
+vn -4.712369 4.104702 4.104674
+v -0.823618 -0.662474 -1.338691
+vn 4.712395 -0.000010 5.804920
+v 0.823617 -0.673869 -1.333971
+vn -4.712395 -0.000001 5.804920
+v -0.823618 -0.673869 -1.333971
+vn 4.712382 -4.104702 4.104685
+v 0.823617 -0.685264 -1.338691
+vn -4.712382 -4.104696 4.104691
+v -0.823618 -0.685264 -1.338691
+vn 4.712401 -5.804924 -0.000004
+v 0.823617 -0.689984 -1.350086
+vn -4.712401 -5.804924 0.000004
+v -0.823618 -0.689984 -1.350086
+vn 4.712399 -4.104721 -4.104680
+v 0.823617 -0.685264 -1.361481
+vn -4.712399 -4.104727 -4.104674
+v -0.823618 -0.685264 -1.361481
+vn -5.235984 6.069087 0.000004
+v 0.561441 0.081383 0.000000
+vn -5.235989 5.255984 -3.034544
+v 0.561441 0.070480 -0.040692
+vn -5.235989 3.034546 -5.255983
+v 0.561441 0.040692 -0.070480
+vn -5.235986 0.000001 -6.069088
+v 0.561441 0.000000 -0.081383
+vn -5.235989 -3.034547 -5.255976
+v 0.561441 -0.040692 -0.070480
+vn -5.235989 -5.256006 -3.034549
+v 0.561441 -0.070480 -0.040692
+vn -5.235986 -6.069103 0.000002
+v 0.561441 -0.081383 0.000000
+vn -5.235991 -5.256007 3.034550
+v 0.561441 -0.070480 0.040692
+vn -5.235989 -3.034554 5.255973
+v 0.561441 -0.040692 0.070480
+vn -5.235984 -0.000003 6.069086
+v 0.561441 0.000000 0.081383
+vn -5.235991 3.034543 5.255986
+v 0.561441 0.040692 0.070480
+vn -5.235989 5.255980 3.034551
+v 0.561441 0.070480 0.040692
+vn 5.235987 6.069087 0.000004
+v 2.084521 0.081383 0.000000
+vn 5.235984 5.255982 -3.034545
+v 2.084521 0.070480 -0.040692
+vn 5.235995 3.034549 -5.255984
+v 2.084521 0.040692 -0.070480
+vn 5.235981 0.000005 -6.069085
+v 2.084521 0.000000 -0.081383
+vn 5.235990 -3.034547 -5.255983
+v 2.084521 -0.040692 -0.070480
+vn 5.235993 -5.255982 -3.034551
+v 2.084521 -0.070480 -0.040692
+vn 5.235984 -6.069095 -0.000011
+v 2.084521 -0.081383 0.000000
+vn 5.235989 -5.255995 3.034542
+v 2.084521 -0.070480 0.040692
+vn 5.235988 -3.034554 5.255987
+v 2.084521 -0.040692 0.070480
+vn 5.235987 -0.000004 6.069088
+v 2.084521 0.000000 0.081383
+vn 5.235989 3.034540 5.255986
+v 2.084521 0.040692 0.070480
+vn 5.235988 5.255981 3.034548
+v 2.084521 0.070480 0.040692
+vn -12.566370 -0.000008 -0.000000
+v 0.561441 0.000000 0.000000
+vn 12.566370 0.000000 0.000000
+v 2.084521 0.000000 0.000000
+vn 0.355499 4.972161 -2.775034
+v 0.673869 0.689984 -1.332763
+vn 3.292306 3.292306 -2.318008
+v 0.685264 0.685264 -1.332763
+vn 4.972162 0.355500 -2.775033
+v 0.689984 0.673869 -1.332763
+vn 4.490691 -3.564183 -4.375483
+v 0.685264 0.662474 -1.332763
+vn 0.542895 -5.966685 -6.684689
+v 0.673869 0.657754 -1.332763
+vn -4.122719 -4.122719 -7.710718
+v 0.662474 0.662474 -1.332763
+vn -5.966685 0.542895 -6.684690
+v 0.657754 0.673869 -1.332763
+vn -3.564185 4.490689 -4.375484
+v 0.662474 0.685264 -1.332763
+vn -0.542897 5.966677 6.684656
+v 0.299497 0.315612 -0.303241
+vn 4.122744 4.122751 7.710777
+v 0.310892 0.310892 -0.303241
+vn 5.966676 -0.542893 6.684656
+v 0.315612 0.299497 -0.303241
+vn 3.564179 -4.490701 4.375489
+v 0.310892 0.288102 -0.303241
+vn -0.355480 -4.972164 2.775051
+v 0.299497 0.283382 -0.303241
+vn -3.292296 -3.292280 2.317987
+v 0.288102 0.288102 -0.303241
+vn -4.972173 -0.355465 2.775058
+v 0.283382 0.299497 -0.303241
+vn -4.490700 3.564181 4.375489
+v 0.288102 0.310892 -0.303241
+vn 0.000000 0.000000 -12.566372
+v 0.673869 0.673869 -1.332763
+vn 0.000000 0.000000 12.566369
+v 0.299497 0.299497 -0.303241
+vn 5.966708 0.542897 6.684694
+v 0.315612 -0.299497 -0.303241
+vn 4.972180 -0.355517 -2.775037
+v 0.689984 -0.673869 -1.332763
+vn 4.490703 3.564161 -4.375446
+v 0.685264 -0.662474 -1.332763
+vn 3.564178 4.490701 4.375466
+v 0.310892 -0.288102 -0.303241
+vn 0.542888 5.966686 -6.684695
+v 0.673869 -0.657754 -1.332763
+vn -0.355491 4.972161 2.775047
+v 0.299497 -0.283382 -0.303241
+vn -4.122740 4.122723 -7.710721
+v 0.662474 -0.662474 -1.332763
+vn -3.292300 3.292297 2.318002
+v 0.288102 -0.288102 -0.303241
+vn -5.966705 -0.542928 -6.684684
+v 0.657754 -0.673869 -1.332763
+vn -4.972191 0.355500 2.775033
+v 0.283382 -0.299497 -0.303241
+vn -3.564183 -4.490709 -4.375484
+v 0.662474 -0.685264 -1.332763
+vn -4.490696 -3.564181 4.375465
+v 0.288102 -0.310892 -0.303241
+vn 0.355491 -4.972157 -2.775042
+v 0.673869 -0.689984 -1.332763
+vn -0.542871 -5.966687 6.684691
+v 0.299497 -0.315612 -0.303241
+vn 3.292302 -3.292305 -2.318004
+v 0.685264 -0.685264 -1.332763
+vn 4.122734 -4.122715 7.710742
+v 0.310892 -0.310892 -0.303241
+vn 0.000000 0.000000 12.566369
+v 0.299497 -0.299497 -0.303241
+vn 0.000000 0.000000 -12.566370
+v 0.673869 -0.673869 -1.332763
+vn 0.000000 0.000000 -12.566370
+v -0.673869 -0.673869 -1.332763
+vn -3.292290 -3.292300 -2.317995
+v -0.685264 -0.685264 -1.332763
+vn -0.355509 -4.972198 -2.775056
+v -0.673869 -0.689984 -1.332763
+vn 0.000000 0.000000 12.566370
+v -0.299497 -0.299497 -0.303241
+vn 0.542906 -5.966710 6.684697
+v -0.299497 -0.315612 -0.303241
+vn -4.122705 -4.122732 7.710721
+v -0.310892 -0.310892 -0.303241
+vn -4.972171 -0.355494 -2.775043
+v -0.689984 -0.673869 -1.332763
+vn -5.966691 0.542908 6.684670
+v -0.315612 -0.299497 -0.303241
+vn -4.490705 3.564189 -4.375466
+v -0.685264 -0.662474 -1.332763
+vn -3.564171 4.490714 4.375473
+v -0.310892 -0.288102 -0.303241
+vn -0.542894 5.966691 -6.684712
+v -0.673869 -0.657754 -1.332763
+vn 0.355498 4.972172 2.775045
+v -0.299497 -0.283382 -0.303241
+vn 4.122747 4.122738 -7.710707
+v -0.662474 -0.662474 -1.332763
+vn 3.292301 3.292283 2.317991
+v -0.288102 -0.288102 -0.303241
+vn 5.966716 -0.542900 -6.684727
+v -0.657754 -0.673869 -1.332763
+vn 4.972217 0.355505 2.775045
+v -0.283382 -0.299497 -0.303241
+vn 3.564143 -4.490687 -4.375431
+v -0.662474 -0.685264 -1.332763
+vn 4.490695 -3.564148 4.375445
+v -0.288102 -0.310892 -0.303241
+vn -5.966710 -0.542906 6.684696
+v -0.315612 0.299497 -0.303241
+vn -4.972151 0.355530 -2.775038
+v -0.689984 0.673869 -1.332763
+vn -4.490701 -3.564152 -4.375435
+v -0.685264 0.662474 -1.332763
+vn -3.564142 -4.490708 4.375428
+v -0.310892 0.288102 -0.303241
+vn -0.542903 -5.966713 -6.684718
+v -0.673869 0.657754 -1.332763
+vn 0.355513 -4.972204 2.775044
+v -0.299497 0.283382 -0.303241
+vn 4.122728 -4.122761 -7.710725
+v -0.662474 0.662474 -1.332763
+vn 3.292291 -3.292302 2.317997
+v -0.288102 0.288102 -0.303241
+vn 5.966693 0.542908 -6.684679
+v -0.657754 0.673869 -1.332763
+vn 4.972177 -0.355509 2.775049
+v -0.283382 0.299497 -0.303241
+vn 3.564167 4.490698 -4.375471
+v -0.662474 0.685264 -1.332763
+vn 4.490724 3.564162 4.375457
+v -0.288102 0.310892 -0.303241
+vn -0.355461 4.972190 -2.775077
+v -0.673868 0.689984 -1.332763
+vn 0.542909 5.966693 6.684693
+v -0.299497 0.315612 -0.303241
+vn -3.292262 3.292327 -2.317995
+v -0.685264 0.685264 -1.332763
+vn -4.122717 4.122703 7.710712
+v -0.310892 0.310892 -0.303241
+vn 0.000000 0.000000 12.566369
+v -0.299497 0.299497 -0.303241
+vn 0.000000 0.000000 -12.566371
+v -0.673869 0.673869 -1.332763
+vn -0.000000 0.000003 -12.550662
+v 1.969194 0.000000 0.187186
+vn 0.382901 -0.158603 -0.001035
+v 2.886225 -0.379846 0.221851
+vn 0.293061 -0.293061 -0.001034
+v 2.671059 -0.701864 0.221851
+vn -0.000000 -0.000002 12.545240
+v 1.969194 0.000000 0.262060
+vn 0.158603 -0.382902 -0.001034
+v 2.349041 -0.917030 0.221851
+vn 0.000000 -0.414450 -0.001035
+v 1.969195 -0.992586 0.221851
+vn -0.158603 -0.382902 -0.001034
+v 1.589348 -0.917030 0.221851
+vn -0.293061 -0.293061 -0.001034
+v 1.267330 -0.701865 0.221851
+vn -0.382902 -0.158603 -0.001034
+v 1.052164 -0.379847 0.221851
+vn -0.414451 -0.000000 -0.001035
+v 0.976608 -0.000000 0.221851
+vn -0.382902 0.158603 -0.001035
+v 1.052164 0.379846 0.221852
+vn -0.293061 0.293061 -0.001035
+v 1.267330 0.701864 0.221852
+vn -0.158603 0.382902 -0.001035
+v 1.589348 0.917030 0.221852
+vn -0.000000 0.414450 -0.001034
+v 1.969194 0.992586 0.221852
+vn 0.158603 0.382902 -0.001035
+v 2.349040 0.917030 0.221852
+vn 0.293061 0.293061 -0.001034
+v 2.671058 0.701864 0.221852
+vn 0.382902 0.158603 -0.001034
+v 2.886224 0.379846 0.221852
+vn 0.414451 0.000001 -0.001035
+v 2.961780 0.000000 0.221851
+vn -0.000008 12.566368 -0.000000
+v 0.000000 -0.561441 0.000000
+vn 6.069087 5.235987 -0.000000
+v 0.081383 -0.561441 0.000000
+vn 5.255986 5.235990 -3.034542
+v 0.070480 -0.561441 -0.040692
+vn 0.000000 -12.566371 0.000000
+v -0.000000 -2.084521 0.000000
+vn 5.255986 -5.235988 -3.034540
+v 0.070480 -2.084521 -0.040692
+vn 6.069087 -5.235988 0.000004
+v 0.081383 -2.084521 0.000000
+vn 3.034549 5.235987 -5.255981
+v 0.040692 -0.561441 -0.070480
+vn 3.034550 -5.235990 -5.255981
+v 0.040691 -2.084521 -0.070480
+vn 0.000003 5.235986 -6.069087
+v 0.000000 -0.561441 -0.081383
+vn 0.000004 -5.235984 -6.069086
+v -0.000000 -2.084521 -0.081383
+vn -3.034544 5.235990 -5.255979
+v -0.040692 -0.561441 -0.070480
+vn -3.034543 -5.235991 -5.255985
+v -0.040692 -2.084521 -0.070480
+vn -5.256004 5.235986 -3.034550
+v -0.070480 -0.561441 -0.040692
+vn -5.255981 -5.235989 -3.034549
+v -0.070480 -2.084521 -0.040692
+vn -6.069103 5.235987 0.000003
+v -0.081383 -0.561441 0.000000
+vn -6.069096 -5.235986 -0.000007
+v -0.081384 -2.084521 0.000000
+vn -5.256007 5.235991 3.034549
+v -0.070480 -0.561441 0.040692
+vn -5.255994 -5.235988 3.034542
+v -0.070480 -2.084521 0.040692
+vn -3.034551 5.235987 5.255974
+v -0.040692 -0.561441 0.070480
+vn -3.034557 -5.235989 5.255986
+v -0.040692 -2.084521 0.070480
+vn -0.000000 5.235986 6.069087
+v -0.000000 -0.561441 0.081383
+vn -0.000008 -5.235987 6.069087
+v -0.000000 -2.084521 0.081383
+vn 3.034541 5.235992 5.255988
+v 0.040691 -0.561441 0.070480
+vn 3.034538 -5.235988 5.255987
+v 0.040691 -2.084521 0.070480
+vn 5.255979 5.235983 3.034549
+v 0.070480 -0.561441 0.040692
+vn 5.255981 -5.235987 3.034548
+v 0.070480 -2.084521 0.040692
+vn 0.158603 -0.382902 -0.001035
+v 0.379846 -2.886224 0.221852
+vn 0.000003 0.000000 -12.550662
+v -0.000000 -1.969194 0.187186
+vn 0.000001 -0.414451 -0.001034
+v 0.000000 -2.961780 0.221851
+vn -0.000003 0.000000 12.545240
+v -0.000000 -1.969194 0.262060
+vn 0.293061 -0.293061 -0.001035
+v 0.701864 -2.671058 0.221852
+vn 0.382902 -0.158603 -0.001034
+v 0.917030 -2.349040 0.221852
+vn 0.414450 0.000000 -0.001035
+v 0.992586 -1.969194 0.221852
+vn 0.382902 0.158603 -0.001034
+v 0.917030 -1.589348 0.221852
+vn 0.293061 0.293061 -0.001034
+v 0.701864 -1.267330 0.221852
+vn 0.158603 0.382902 -0.001035
+v 0.379846 -1.052164 0.221852
+vn -0.000000 0.414451 -0.001035
+v -0.000000 -0.976608 0.221851
+vn -0.158603 0.382902 -0.001035
+v -0.379847 -1.052164 0.221851
+vn -0.293061 0.293061 -0.001035
+v -0.701865 -1.267330 0.221851
+vn -0.382902 0.158603 -0.001034
+v -0.917030 -1.589348 0.221851
+vn -0.414450 0.000000 -0.001035
+v -0.992586 -1.969194 0.221851
+vn -0.382902 -0.158603 -0.001034
+v -0.917031 -2.349040 0.221851
+vn -0.293061 -0.293060 -0.001035
+v -0.701865 -2.671059 0.221851
+vn -0.158603 -0.382901 -0.001035
+v -0.379847 -2.886225 0.221851
+vn -0.414451 -0.000000 -0.001035
+v -2.961780 0.000000 0.221851
+vn -0.382901 0.158603 -0.001034
+v -2.886225 0.379847 0.221851
+vn 0.000000 -0.000003 -12.550660
+v -1.969194 0.000001 0.187186
+vn 0.000000 0.000003 12.545241
+v -1.969194 0.000001 0.262060
+vn -0.293060 0.293061 -0.001034
+v -2.671059 0.701865 0.221851
+vn -0.158603 0.382902 -0.001035
+v -2.349040 0.917031 0.221851
+vn 0.000000 0.414450 -0.001035
+v -1.969194 0.992587 0.221851
+vn 0.158603 0.382902 -0.001035
+v -1.589348 0.917031 0.221851
+vn 0.293061 0.293061 -0.001034
+v -1.267330 0.701865 0.221851
+vn 0.382902 0.158603 -0.001034
+v -1.052164 0.379847 0.221851
+vn 0.414451 0.000000 -0.001035
+v -0.976608 0.000000 0.221851
+vn 0.382902 -0.158603 -0.001034
+v -1.052164 -0.379846 0.221852
+vn 0.293061 -0.293061 -0.001034
+v -1.267330 -0.701864 0.221852
+vn 0.158603 -0.382902 -0.001034
+v -1.589348 -0.917029 0.221852
+vn 0.000000 -0.414450 -0.001034
+v -1.969194 -0.992585 0.221852
+vn -0.158603 -0.382902 -0.001034
+v -2.349041 -0.917029 0.221852
+vn -0.293061 -0.293061 -0.001034
+v -2.671058 -0.701864 0.221852
+vn -0.382902 -0.158603 -0.001035
+v -2.886224 -0.379846 0.221852
+vn 5.235981 -5.255978 3.034548
+v -0.561441 -0.070480 0.040692
+vn 5.235986 -6.069087 -0.000000
+v -0.561441 -0.081383 0.000000
+vn -5.235989 -6.069088 0.000004
+v -2.084521 -0.081383 0.000000
+vn -5.235990 -5.255980 3.034549
+v -2.084521 -0.070479 0.040692
+vn 5.235992 -3.034540 5.255989
+v -0.561441 -0.040691 0.070480
+vn -5.235989 -3.034539 5.255987
+v -2.084521 -0.040691 0.070480
+vn 5.235986 0.000001 6.069087
+v -0.561441 0.000000 0.081383
+vn -5.235987 0.000008 6.069087
+v -2.084521 0.000001 0.081383
+vn 5.235988 3.034551 5.255973
+v -0.561441 0.040692 0.070480
+vn -5.235989 3.034557 5.255987
+v -2.084521 0.040692 0.070480
+vn 5.235992 5.256007 3.034549
+v -0.561441 0.070480 0.040692
+vn -5.235987 5.255994 3.034543
+v -2.084521 0.070481 0.040692
+vn 5.235989 6.069104 0.000003
+v -0.561441 0.081383 0.000000
+vn -5.235985 6.069095 -0.000007
+v -2.084521 0.081384 0.000000
+vn 5.235990 5.256006 -3.034548
+v -0.561441 0.070480 -0.040692
+vn -5.235988 5.255981 -3.034549
+v -2.084521 0.070480 -0.040692
+vn 5.235989 3.034546 -5.255977
+v -0.561441 0.040692 -0.070480
+vn -5.235990 3.034543 -5.255985
+v -2.084521 0.040692 -0.070480
+vn 5.235987 -0.000001 -6.069087
+v -0.561441 0.000000 -0.081383
+vn -5.235984 -0.000004 -6.069086
+v -2.084521 0.000000 -0.081383
+vn 5.235985 -3.034547 -5.255981
+v -0.561441 -0.040691 -0.070480
+vn -5.235991 -3.034550 -5.255980
+v -2.084521 -0.040691 -0.070480
+vn 5.235989 -5.255986 -3.034541
+v -0.561441 -0.070480 -0.040692
+vn -5.235990 -5.255986 -3.034541
+v -2.084521 -0.070479 -0.040692
+vn -12.566370 0.000000 0.000000
+v -2.084521 0.000001 0.000000
+vn 12.566370 0.000008 -0.000000
+v -0.561441 0.000000 0.000000
+vn 0.000008 -12.566371 -0.000000
+v 0.000000 0.561441 0.000000
+vn -6.069087 -5.235986 -0.000000
+v -0.081383 0.561441 0.000000
+vn -5.255983 -5.235986 -3.034544
+v -0.070480 0.561441 -0.040692
+vn 0.000000 12.566370 0.000000
+v 0.000001 2.084521 0.000000
+vn -5.255984 5.235989 -3.034543
+v -0.070479 2.084521 -0.040692
+vn -6.069086 5.235986 0.000008
+v -0.081382 2.084521 0.000000
+vn -3.034545 -5.235988 -5.255983
+v -0.040691 0.561441 -0.070480
+vn -3.034548 5.235993 -5.255983
+v -0.040691 2.084521 -0.070480
+vn -0.000002 -5.235987 -6.069087
+v 0.000000 0.561441 -0.081383
+vn -0.000004 5.235984 -6.069086
+v 0.000001 2.084521 -0.081383
+vn 3.034546 -5.235990 -5.255977
+v 0.040692 0.561441 -0.070480
+vn 3.034543 5.235989 -5.255985
+v 0.040692 2.084521 -0.070480
+vn 5.256006 -5.235992 -3.034552
+v 0.070480 0.561441 -0.040692
+vn 5.255981 5.235987 -3.034550
+v 0.070481 2.084521 -0.040692
+vn 6.069102 -5.235987 -0.000002
+v 0.081384 0.561441 0.000000
+vn 6.069095 5.235984 -0.000008
+v 0.081384 2.084521 0.000000
+vn 5.256007 -5.235992 3.034549
+v 0.070480 0.561441 0.040692
+vn 5.255994 5.235986 3.034542
+v 0.070481 2.084521 0.040692
+vn 3.034550 -5.235988 5.255974
+v 0.040692 0.561441 0.070480
+vn 3.034556 5.235989 5.255987
+v 0.040693 2.084521 0.070480
+vn 0.000000 -5.235987 6.069087
+v 0.000000 0.561441 0.081383
+vn 0.000009 5.235988 6.069088
+v 0.000001 2.084521 0.081383
+vn -3.034540 -5.235992 5.255989
+v -0.040691 0.561441 0.070480
+vn -3.034540 5.235986 5.255985
+v -0.040691 2.084521 0.070480
+vn -5.255979 -5.235981 3.034548
+v -0.070480 0.561441 0.040692
+vn -5.255982 5.235995 3.034551
+v -0.070479 2.084521 0.040692
+vn -0.158603 0.382902 -0.001035
+v -0.379845 2.886224 0.221852
+vn -0.000003 -0.000000 -12.550661
+v 0.000001 1.969194 0.187186
+vn -0.000000 0.414451 -0.001034
+v 0.000001 2.961780 0.221851
+vn 0.000003 -0.000000 12.545241
+v 0.000001 1.969194 0.262060
+vn -0.293061 0.293061 -0.001034
+v -0.701863 2.671058 0.221852
+vn -0.382902 0.158603 -0.001034
+v -0.917029 2.349041 0.221852
+vn -0.414450 0.000000 -0.001035
+v -0.992585 1.969195 0.221852
+vn -0.382902 -0.158603 -0.001034
+v -0.917029 1.589348 0.221852
+vn -0.293061 -0.293061 -0.001035
+v -0.701864 1.267330 0.221852
+vn -0.158603 -0.382902 -0.001035
+v -0.379846 1.052164 0.221852
+vn -0.000000 -0.414451 -0.001034
+v 0.000001 0.976608 0.221851
+vn 0.158603 -0.382902 -0.001035
+v 0.379847 1.052164 0.221851
+vn 0.293061 -0.293061 -0.001034
+v 0.701865 1.267330 0.221851
+vn 0.382902 -0.158603 -0.001035
+v 0.917031 1.589348 0.221851
+vn 0.414450 -0.000000 -0.001035
+v 0.992587 1.969194 0.221851
+vn 0.382902 0.158603 -0.001035
+v 0.917031 2.349040 0.221851
+vn 0.293061 0.293060 -0.001034
+v 0.701866 2.671059 0.221851
+vn 0.158603 0.382901 -0.001034
+v 0.379847 2.886225 0.221851
+vn 2.221441 5.363034 -4.712390
+v 0.336934 0.561557 -0.299497
+vn 2.221442 -5.363034 -4.712392
+v 0.336934 -0.561557 -0.299497
+vn -2.221441 -5.363034 -4.712388
+v -0.336934 -0.561557 -0.299497
+vn -2.221440 5.363033 -4.712384
+v -0.336934 0.561558 -0.299497
+vn 2.221442 5.363033 4.712387
+v 0.336935 0.561557 0.299497
+vn 2.221439 -5.363034 4.712385
+v 0.336934 -0.561558 0.299497
+vn -2.221441 -5.363034 4.712389
+v -0.336934 -0.561557 0.299497
+vn -2.221442 5.363035 4.712392
+v -0.336934 0.561557 0.299497
+vn 5.363035 2.221442 -4.712393
+v 0.561557 0.336934 -0.299497
+vn -5.363034 2.221443 -4.712390
+v -0.561557 0.336935 -0.299497
+vn 5.363034 2.221440 4.712386
+v 0.561558 0.336934 0.299497
+vn -5.363035 2.221442 4.712391
+v -0.561557 0.336934 0.299497
+vn 5.363035 -2.221442 -4.712392
+v 0.561557 -0.336934 -0.299497
+vn -5.363034 -2.221440 -4.712389
+v -0.561557 -0.336934 -0.299497
+vn 5.363033 -2.221442 4.712387
+v 0.561557 -0.336935 0.299497
+vn -5.363034 -2.221441 4.712388
+v -0.561557 -0.336934 0.299497
+# 300 vertices, 0 vertices normals
+
+f 61//61 37//37 38//38
+f 62//62 50//50 49//49
+f 61//61 38//38 39//39
+f 62//62 51//51 50//50
+f 61//61 39//39 40//40
+f 62//62 52//52 51//51
+f 61//61 40//40 41//41
+f 62//62 53//53 52//52
+f 61//61 41//41 42//42
+f 62//62 54//54 53//53
+f 61//61 42//42 43//43
+f 62//62 55//55 54//54
+f 61//61 43//43 44//44
+f 62//62 56//56 55//55
+f 61//61 44//44 45//45
+f 62//62 57//57 56//56
+f 61//61 45//45 46//46
+f 62//62 58//58 57//57
+f 61//61 46//46 47//47
+f 62//62 59//59 58//58
+f 61//61 47//47 48//48
+f 62//62 60//60 59//59
+f 48//48 37//37 61//61
+f 62//62 49//49 60//60
+f 49//49 37//37 48//48
+f 49//49 48//48 60//60
+f 47//47 59//59 60//60
+f 47//47 60//60 48//48
+f 46//46 58//58 59//59
+f 46//46 59//59 47//47
+f 45//45 57//57 58//58
+f 45//45 58//58 46//46
+f 44//44 56//56 57//57
+f 44//44 57//57 45//45
+f 43//43 55//55 56//56
+f 43//43 56//56 44//44
+f 42//42 54//54 55//55
+f 42//42 55//55 43//43
+f 41//41 53//53 54//54
+f 41//41 54//54 42//42
+f 40//40 52//52 53//53
+f 40//40 53//53 41//41
+f 39//39 51//51 52//52
+f 39//39 52//52 40//40
+f 38//38 50//50 51//51
+f 38//38 51//51 39//39
+f 37//37 49//49 50//50
+f 37//37 50//50 38//38
+f 152//152 136//136 135//135
+f 138//138 136//136 152//152
+f 137//137 139//139 135//135
+f 138//138 139//139 137//137
+f 140//140 141//141 135//135
+f 138//138 141//141 140//140
+f 142//142 143//143 135//135
+f 138//138 143//143 142//142
+f 144//144 145//145 135//135
+f 138//138 145//145 144//144
+f 146//146 147//147 135//135
+f 138//138 147//147 146//146
+f 148//148 149//149 135//135
+f 138//138 149//149 148//148
+f 150//150 151//151 135//135
+f 138//138 151//151 150//150
+f 182//182 179//179 183//183
+f 183//183 179//179 180//180
+f 182//182 184//184 185//185
+f 185//185 184//184 180//180
+f 182//182 186//186 187//187
+f 187//187 186//186 180//180
+f 182//182 188//188 189//189
+f 189//189 188//188 180//180
+f 182//182 190//190 191//191
+f 191//191 190//190 180//180
+f 182//182 192//192 193//193
+f 193//193 192//192 180//180
+f 182//182 194//194 195//195
+f 195//195 194//194 180//180
+f 182//182 196//196 181//181
+f 181//181 196//196 180//180
+f 197//197 198//198 199//199
+f 200//200 198//198 197//197
+f 201//201 202//202 199//199
+f 200//200 202//202 201//201
+f 203//203 204//204 199//199
+f 200//200 204//204 203//203
+f 205//205 206//206 199//199
+f 200//200 206//206 205//205
+f 207//207 208//208 199//199
+f 200//200 208//208 207//207
+f 209//209 210//210 199//199
+f 200//200 210//210 209//209
+f 211//211 212//212 199//199
+f 200//200 212//212 211//211
+f 213//213 214//214 199//199
+f 200//200 214//214 213//213
+f 270//270 267//267 271//271
+f 271//271 267//267 268//268
+f 270//270 272//272 273//273
+f 273//273 272//272 268//268
+f 270//270 274//274 275//275
+f 275//275 274//274 268//268
+f 270//270 276//276 277//277
+f 277//277 276//276 268//268
+f 270//270 278//278 279//279
+f 279//279 278//278 268//268
+f 270//270 280//280 281//281
+f 281//281 280//280 268//268
+f 270//270 282//282 283//283
+f 283//283 282//282 268//268
+f 270//270 284//284 269//269
+f 269//269 284//284 268//268
+f 17//17 1//1 4//4
+f 3//3 2//2 18//18
+f 17//17 4//4 6//6
+f 18//18 5//5 3//3
+f 17//17 6//6 8//8
+f 18//18 7//7 5//5
+f 17//17 8//8 10//10
+f 18//18 9//9 7//7
+f 17//17 10//10 12//12
+f 18//18 11//11 9//9
+f 17//17 12//12 14//14
+f 18//18 13//13 11//11
+f 17//17 14//14 16//16
+f 18//18 15//15 13//13
+f 16//16 1//1 17//17
+f 18//18 2//2 15//15
+f 19//19 21//21 20//20
+f 22//22 24//24 23//23
+f 19//19 20//20 25//25
+f 22//22 26//26 24//24
+f 19//19 25//25 27//27
+f 22//22 28//28 26//26
+f 19//19 27//27 29//29
+f 22//22 30//30 28//28
+f 19//19 29//29 31//31
+f 22//22 32//32 30//30
+f 19//19 31//31 33//33
+f 22//22 34//34 32//32
+f 19//19 33//33 35//35
+f 22//22 36//36 34//34
+f 35//35 21//21 19//19
+f 36//36 22//22 23//23
+f 79//79 63//63 64//64
+f 80//80 72//72 71//71
+f 79//79 64//64 65//65
+f 80//80 73//73 72//72
+f 79//79 65//65 66//66
+f 80//80 74//74 73//73
+f 79//79 66//66 67//67
+f 80//80 75//75 74//74
+f 79//79 67//67 68//68
+f 80//80 76//76 75//75
+f 79//79 68//68 69//69
+f 80//80 77//77 76//76
+f 79//79 69//69 70//70
+f 80//80 78//78 77//77
+f 70//70 63//63 79//79
+f 80//80 71//71 78//78
+f 97//97 81//81 84//84
+f 83//83 82//82 98//98
+f 97//97 84//84 86//86
+f 98//98 85//85 83//83
+f 97//97 86//86 88//88
+f 98//98 87//87 85//85
+f 97//97 88//88 90//90
+f 98//98 89//89 87//87
+f 97//97 90//90 92//92
+f 98//98 91//91 89//89
+f 97//97 92//92 94//94
+f 98//98 93//93 91//91
+f 97//97 94//94 96//96
+f 98//98 95//95 93//93
+f 97//97 96//96 81//81
+f 98//98 82//82 95//95
+f 99//99 101//101 100//100
+f 102//102 104//104 103//103
+f 99//99 100//100 105//105
+f 102//102 106//106 104//104
+f 99//99 105//105 107//107
+f 102//102 108//108 106//106
+f 99//99 107//107 109//109
+f 102//102 110//110 108//108
+f 99//99 109//109 111//111
+f 102//102 112//112 110//110
+f 99//99 111//111 113//113
+f 102//102 114//114 112//112
+f 99//99 113//113 115//115
+f 102//102 116//116 114//114
+f 115//115 101//101 99//99
+f 102//102 103//103 116//116
+f 133//133 117//117 120//120
+f 119//119 118//118 134//134
+f 133//133 120//120 122//122
+f 134//134 121//121 119//119
+f 133//133 122//122 124//124
+f 134//134 123//123 121//121
+f 133//133 124//124 126//126
+f 134//134 125//125 123//123
+f 133//133 126//126 128//128
+f 134//134 127//127 125//125
+f 133//133 128//128 130//130
+f 134//134 129//129 127//127
+f 133//133 130//130 132//132
+f 134//134 131//131 129//129
+f 133//133 132//132 117//117
+f 134//134 118//118 131//131
+f 153//153 154//154 155//155
+f 156//156 157//157 158//158
+f 153//153 155//155 159//159
+f 156//156 160//160 157//157
+f 153//153 159//159 161//161
+f 156//156 162//162 160//160
+f 153//153 161//161 163//163
+f 156//156 164//164 162//162
+f 153//153 163//163 165//165
+f 156//156 166//166 164//164
+f 153//153 165//165 167//167
+f 156//156 168//168 166//166
+f 153//153 167//167 169//169
+f 156//156 170//170 168//168
+f 153//153 169//169 171//171
+f 156//156 172//172 170//170
+f 153//153 171//171 173//173
+f 156//156 174//174 172//172
+f 153//153 173//173 175//175
+f 156//156 176//176 174//174
+f 153//153 175//175 177//177
+f 156//156 178//178 176//176
+f 153//153 177//177 154//154
+f 156//156 158//158 178//178
+f 239//239 217//217 218//218
+f 240//240 215//215 216//216
+f 239//239 218//218 220//220
+f 240//240 219//219 215//215
+f 239//239 220//220 222//222
+f 240//240 221//221 219//219
+f 239//239 222//222 224//224
+f 240//240 223//223 221//221
+f 239//239 224//224 226//226
+f 240//240 225//225 223//223
+f 239//239 226//226 228//228
+f 240//240 227//227 225//225
+f 239//239 228//228 230//230
+f 240//240 229//229 227//227
+f 239//239 230//230 232//232
+f 240//240 231//231 229//229
+f 239//239 232//232 234//234
+f 240//240 233//233 231//231
+f 239//239 234//234 236//236
+f 240//240 235//235 233//233
+f 239//239 236//236 238//238
+f 240//240 237//237 235//235
+f 239//239 238//238 217//217
+f 240//240 216//216 237//237
+f 241//241 242//242 243//243
+f 244//244 245//245 246//246
+f 241//241 243//243 247//247
+f 244//244 248//248 245//245
+f 241//241 247//247 249//249
+f 244//244 250//250 248//248
+f 241//241 249//249 251//251
+f 244//244 252//252 250//250
+f 241//241 251//251 253//253
+f 244//244 254//254 252//252
+f 241//241 253//253 255//255
+f 244//244 256//256 254//254
+f 241//241 255//255 257//257
+f 244//244 258//258 256//256
+f 241//241 257//257 259//259
+f 244//244 260//260 258//258
+f 241//241 259//259 261//261
+f 244//244 262//262 260//260
+f 241//241 261//261 263//263
+f 244//244 264//264 262//262
+f 241//241 263//263 265//265
+f 244//244 266//266 264//264
+f 241//241 265//265 242//242
+f 244//244 246//246 266//266
+f 300//300 296//296 298//298
+f 296//296 294//294 298//298
+f 291//291 300//300 298//298
+f 291//291 298//298 287//287
+f 299//299 290//290 286//286
+f 299//299 286//286 297//297
+f 295//295 299//299 297//297
+f 295//295 297//297 293//293
+f 300//300 291//291 299//299
+f 291//291 290//290 299//299
+f 296//296 300//300 299//299
+f 296//296 299//299 295//295
+f 297//297 286//286 287//287
+f 297//297 287//287 298//298
+f 293//293 297//297 298//298
+f 293//293 298//298 294//294
+f 296//296 292//292 294//294
+f 292//292 288//288 294//294
+f 293//293 285//285 289//289
+f 293//293 289//289 295//295
+f 292//292 296//296 289//289
+f 296//296 295//295 289//289
+f 285//285 293//293 294//294
+f 285//285 294//294 288//288
+f 289//289 285//285 292//292
+f 285//285 288//288 292//292
+f 286//286 290//290 291//291
+f 286//286 291//291 287//287
+f 265//265 246//246 242//242
+f 265//265 266//266 246//246
+f 263//263 266//266 265//265
+f 263//263 264//264 266//266
+f 261//261 264//264 263//263
+f 261//261 262//262 264//264
+f 259//259 262//262 261//261
+f 259//259 260//260 262//262
+f 257//257 260//260 259//259
+f 257//257 258//258 260//260
+f 255//255 258//258 257//257
+f 255//255 256//256 258//258
+f 253//253 256//256 255//255
+f 253//253 254//254 256//256
+f 251//251 254//254 253//253
+f 251//251 252//252 254//254
+f 249//249 252//252 251//251
+f 249//249 250//250 252//252
+f 247//247 250//250 249//249
+f 247//247 248//248 250//250
+f 243//243 248//248 247//247
+f 243//243 245//245 248//248
+f 242//242 245//245 243//243
+f 242//242 246//246 245//245
+f 216//216 217//217 238//238
+f 216//216 238//238 237//237
+f 237//237 238//238 236//236
+f 237//237 236//236 235//235
+f 235//235 236//236 234//234
+f 235//235 234//234 233//233
+f 233//233 234//234 232//232
+f 233//233 232//232 231//231
+f 231//231 232//232 230//230
+f 231//231 230//230 229//229
+f 229//229 230//230 228//228
+f 229//229 228//228 227//227
+f 227//227 228//228 226//226
+f 227//227 226//226 225//225
+f 225//225 226//226 224//224
+f 225//225 224//224 223//223
+f 223//223 224//224 222//222
+f 223//223 222//222 221//221
+f 221//221 222//222 220//220
+f 221//221 220//220 219//219
+f 219//219 220//220 218//218
+f 219//219 218//218 215//215
+f 215//215 218//218 217//217
+f 215//215 217//217 216//216
+f 177//177 158//158 154//154
+f 177//177 178//178 158//158
+f 175//175 178//178 177//177
+f 175//175 176//176 178//178
+f 173//173 176//176 175//175
+f 173//173 174//174 176//176
+f 171//171 174//174 173//173
+f 171//171 172//172 174//174
+f 169//169 172//172 171//171
+f 169//169 170//170 172//172
+f 167//167 170//170 169//169
+f 167//167 168//168 170//170
+f 165//165 168//168 167//167
+f 165//165 166//166 168//168
+f 163//163 166//166 165//165
+f 163//163 164//164 166//166
+f 161//161 164//164 163//163
+f 161//161 162//162 164//164
+f 159//159 162//162 161//161
+f 159//159 160//160 162//162
+f 155//155 160//160 159//159
+f 155//155 157//157 160//160
+f 154//154 157//157 155//155
+f 154//154 158//158 157//157
+f 118//118 132//132 131//131
+f 118//118 117//117 132//132
+f 131//131 132//132 129//129
+f 129//129 132//132 130//130
+f 129//129 130//130 127//127
+f 127//127 130//130 128//128
+f 127//127 128//128 125//125
+f 125//125 128//128 126//126
+f 125//125 126//126 123//123
+f 123//123 126//126 124//124
+f 123//123 122//122 121//121
+f 123//123 124//124 122//122
+f 121//121 120//120 119//119
+f 121//121 122//122 120//120
+f 117//117 119//119 120//120
+f 117//117 118//118 119//119
+f 103//103 101//101 115//115
+f 103//103 115//115 116//116
+f 113//113 114//114 116//116
+f 113//113 116//116 115//115
+f 111//111 112//112 114//114
+f 111//111 114//114 113//113
+f 109//109 110//110 111//111
+f 110//110 112//112 111//111
+f 107//107 108//108 109//109
+f 108//108 110//110 109//109
+f 105//105 106//106 107//107
+f 106//106 108//108 107//107
+f 100//100 104//104 105//105
+f 104//104 106//106 105//105
+f 101//101 103//103 104//104
+f 101//101 104//104 100//100
+f 82//82 96//96 95//95
+f 82//82 81//81 96//96
+f 95//95 96//96 93//93
+f 93//93 96//96 94//94
+f 93//93 94//94 91//91
+f 91//91 94//94 92//92
+f 91//91 92//92 89//89
+f 89//89 92//92 90//90
+f 89//89 90//90 87//87
+f 87//87 90//90 88//88
+f 87//87 86//86 85//85
+f 87//87 88//88 86//86
+f 85//85 84//84 83//83
+f 85//85 86//86 84//84
+f 81//81 83//83 84//84
+f 81//81 82//82 83//83
+f 71//71 63//63 70//70
+f 71//71 70//70 78//78
+f 69//69 77//77 78//78
+f 69//69 78//78 70//70
+f 68//68 76//76 77//77
+f 68//68 77//77 69//69
+f 67//67 75//75 68//68
+f 75//75 76//76 68//68
+f 66//66 74//74 67//67
+f 74//74 75//75 67//67
+f 65//65 73//73 66//66
+f 73//73 74//74 66//66
+f 64//64 72//72 65//65
+f 72//72 73//73 65//65
+f 63//63 71//71 72//72
+f 63//63 72//72 64//64
+f 23//23 21//21 35//35
+f 23//23 35//35 36//36
+f 33//33 34//34 36//36
+f 33//33 36//36 35//35
+f 31//31 32//32 34//34
+f 31//31 34//34 33//33
+f 29//29 30//30 32//32
+f 29//29 32//32 31//31
+f 27//27 28//28 30//30
+f 27//27 30//30 29//29
+f 25//25 26//26 28//28
+f 25//25 28//28 27//27
+f 20//20 24//24 26//26
+f 20//20 26//26 25//25
+f 24//24 20//20 21//21
+f 24//24 21//21 23//23
+f 2//2 1//1 16//16
+f 16//16 15//15 2//2
+f 15//15 14//14 13//13
+f 15//15 16//16 14//14
+f 13//13 12//12 11//11
+f 13//13 14//14 12//12
+f 11//11 10//10 9//9
+f 11//11 12//12 10//10
+f 9//9 8//8 7//7
+f 9//9 10//10 8//8
+f 7//7 6//6 5//5
+f 7//7 8//8 6//6
+f 5//5 4//4 3//3
+f 5//5 6//6 4//4
+f 1//1 3//3 4//4
+f 1//1 2//2 3//3
+f 138//138 137//137 136//136
+f 136//136 137//137 135//135
+f 138//138 140//140 139//139
+f 139//139 140//140 135//135
+f 138//138 142//142 141//141
+f 141//141 142//142 135//135
+f 138//138 144//144 143//143
+f 143//143 144//144 135//135
+f 138//138 146//146 145//145
+f 145//145 146//146 135//135
+f 138//138 148//148 147//147
+f 147//147 148//148 135//135
+f 138//138 150//150 149//149
+f 149//149 150//150 135//135
+f 138//138 152//152 151//151
+f 151//151 152//152 135//135
+f 179//179 181//181 180//180
+f 182//182 181//181 179//179
+f 184//184 183//183 180//180
+f 182//182 183//183 184//184
+f 186//186 185//185 180//180
+f 182//182 185//185 186//186
+f 188//188 187//187 180//180
+f 182//182 187//187 188//188
+f 190//190 189//189 180//180
+f 182//182 189//189 190//190
+f 192//192 191//191 180//180
+f 182//182 191//191 192//192
+f 194//194 193//193 180//180
+f 182//182 193//193 194//194
+f 196//196 195//195 180//180
+f 182//182 195//195 196//196
+f 200//200 201//201 198//198
+f 198//198 201//201 199//199
+f 200//200 203//203 202//202
+f 202//202 203//203 199//199
+f 200//200 205//205 204//204
+f 204//204 205//205 199//199
+f 200//200 207//207 206//206
+f 206//206 207//207 199//199
+f 200//200 209//209 208//208
+f 208//208 209//209 199//199
+f 200//200 211//211 210//210
+f 210//210 211//211 199//199
+f 200//200 213//213 212//212
+f 212//212 213//213 199//199
+f 200//200 197//197 214//214
+f 214//214 197//197 199//199
+f 267//267 269//269 268//268
+f 270//270 269//269 267//267
+f 272//272 271//271 268//268
+f 270//270 271//271 272//272
+f 274//274 273//273 268//268
+f 270//270 273//273 274//274
+f 276//276 275//275 268//268
+f 270//270 275//275 276//276
+f 278//278 277//277 268//268
+f 270//270 277//277 278//278
+f 280//280 279//279 268//268
+f 270//270 279//279 280//280
+f 282//282 281//281 268//268
+f 270//270 281//281 282//282
+f 284//284 283//283 268//268
+f 270//270 283//283 284//284
+f 61//61 37//37 38//38
+f 62//62 50//50 49//49
+f 61//61 38//38 39//39
+f 62//62 51//51 50//50
+f 61//61 39//39 40//40
+f 62//62 52//52 51//51
+f 61//61 40//40 41//41
+f 62//62 53//53 52//52
+f 61//61 41//41 42//42
+f 62//62 54//54 53//53
+f 61//61 42//42 43//43
+f 62//62 55//55 54//54
+f 61//61 43//43 44//44
+f 62//62 56//56 55//55
+f 61//61 44//44 45//45
+f 62//62 57//57 56//56
+f 61//61 45//45 46//46
+f 62//62 58//58 57//57
+f 61//61 46//46 47//47
+f 62//62 59//59 58//58
+f 61//61 47//47 48//48
+f 62//62 60//60 59//59
+f 48//48 37//37 61//61
+f 62//62 49//49 60//60
+f 49//49 37//37 48//48
+f 49//49 48//48 60//60
+f 47//47 59//59 60//60
+f 47//47 60//60 48//48
+f 46//46 58//58 59//59
+f 46//46 59//59 47//47
+f 45//45 57//57 58//58
+f 45//45 58//58 46//46
+f 44//44 56//56 57//57
+f 44//44 57//57 45//45
+f 43//43 55//55 56//56
+f 43//43 56//56 44//44
+f 42//42 54//54 55//55
+f 42//42 55//55 43//43
+f 41//41 53//53 54//54
+f 41//41 54//54 42//42
+f 40//40 52//52 53//53
+f 40//40 53//53 41//41
+f 39//39 51//51 52//52
+f 39//39 52//52 40//40
+f 38//38 50//50 51//51
+f 38//38 51//51 39//39
+f 37//37 49//49 50//50
+f 37//37 50//50 38//38
+f 152//152 136//136 135//135
+f 138//138 136//136 152//152
+f 137//137 139//139 135//135
+f 138//138 139//139 137//137
+f 140//140 141//141 135//135
+f 138//138 141//141 140//140
+f 142//142 143//143 135//135
+f 138//138 143//143 142//142
+f 144//144 145//145 135//135
+f 138//138 145//145 144//144
+f 146//146 147//147 135//135
+f 138//138 147//147 146//146
+f 148//148 149//149 135//135
+f 138//138 149//149 148//148
+f 150//150 151//151 135//135
+f 138//138 151//151 150//150
+f 182//182 179//179 183//183
+f 183//183 179//179 180//180
+f 182//182 184//184 185//185
+f 185//185 184//184 180//180
+f 182//182 186//186 187//187
+f 187//187 186//186 180//180
+f 182//182 188//188 189//189
+f 189//189 188//188 180//180
+f 182//182 190//190 191//191
+f 191//191 190//190 180//180
+f 182//182 192//192 193//193
+f 193//193 192//192 180//180
+f 182//182 194//194 195//195
+f 195//195 194//194 180//180
+f 182//182 196//196 181//181
+f 181//181 196//196 180//180
+f 197//197 198//198 199//199
+f 200//200 198//198 197//197
+f 201//201 202//202 199//199
+f 200//200 202//202 201//201
+f 203//203 204//204 199//199
+f 200//200 204//204 203//203
+f 205//205 206//206 199//199
+f 200//200 206//206 205//205
+f 207//207 208//208 199//199
+f 200//200 208//208 207//207
+f 209//209 210//210 199//199
+f 200//200 210//210 209//209
+f 211//211 212//212 199//199
+f 200//200 212//212 211//211
+f 213//213 214//214 199//199
+f 200//200 214//214 213//213
+f 270//270 267//267 271//271
+f 271//271 267//267 268//268
+f 270//270 272//272 273//273
+f 273//273 272//272 268//268
+f 270//270 274//274 275//275
+f 275//275 274//274 268//268
+f 270//270 276//276 277//277
+f 277//277 276//276 268//268
+f 270//270 278//278 279//279
+f 279//279 278//278 268//268
+f 270//270 280//280 281//281
+f 281//281 280//280 268//268
+f 270//270 282//282 283//283
+f 283//283 282//282 268//268
+f 270//270 284//284 269//269
+f 269//269 284//284 268//268
+f 17//17 1//1 4//4
+f 3//3 2//2 18//18
+f 17//17 4//4 6//6
+f 18//18 5//5 3//3
+f 17//17 6//6 8//8
+f 18//18 7//7 5//5
+f 17//17 8//8 10//10
+f 18//18 9//9 7//7
+f 17//17 10//10 12//12
+f 18//18 11//11 9//9
+f 17//17 12//12 14//14
+f 18//18 13//13 11//11
+f 17//17 14//14 16//16
+f 18//18 15//15 13//13
+f 16//16 1//1 17//17
+f 18//18 2//2 15//15
+f 19//19 21//21 20//20
+f 22//22 24//24 23//23
+f 19//19 20//20 25//25
+f 22//22 26//26 24//24
+f 19//19 25//25 27//27
+f 22//22 28//28 26//26
+f 19//19 27//27 29//29
+f 22//22 30//30 28//28
+f 19//19 29//29 31//31
+f 22//22 32//32 30//30
+f 19//19 31//31 33//33
+f 22//22 34//34 32//32
+f 19//19 33//33 35//35
+f 22//22 36//36 34//34
+f 35//35 21//21 19//19
+f 36//36 22//22 23//23
+f 79//79 63//63 64//64
+f 80//80 72//72 71//71
+f 79//79 64//64 65//65
+f 80//80 73//73 72//72
+f 79//79 65//65 66//66
+f 80//80 74//74 73//73
+f 79//79 66//66 67//67
+f 80//80 75//75 74//74
+f 79//79 67//67 68//68
+f 80//80 76//76 75//75
+f 79//79 68//68 69//69
+f 80//80 77//77 76//76
+f 79//79 69//69 70//70
+f 80//80 78//78 77//77
+f 70//70 63//63 79//79
+f 80//80 71//71 78//78
+f 97//97 81//81 84//84
+f 83//83 82//82 98//98
+f 97//97 84//84 86//86
+f 98//98 85//85 83//83
+f 97//97 86//86 88//88
+f 98//98 87//87 85//85
+f 97//97 88//88 90//90
+f 98//98 89//89 87//87
+f 97//97 90//90 92//92
+f 98//98 91//91 89//89
+f 97//97 92//92 94//94
+f 98//98 93//93 91//91
+f 97//97 94//94 96//96
+f 98//98 95//95 93//93
+f 97//97 96//96 81//81
+f 98//98 82//82 95//95
+f 99//99 101//101 100//100
+f 102//102 104//104 103//103
+f 99//99 100//100 105//105
+f 102//102 106//106 104//104
+f 99//99 105//105 107//107
+f 102//102 108//108 106//106
+f 99//99 107//107 109//109
+f 102//102 110//110 108//108
+f 99//99 109//109 111//111
+f 102//102 112//112 110//110
+f 99//99 111//111 113//113
+f 102//102 114//114 112//112
+f 99//99 113//113 115//115
+f 102//102 116//116 114//114
+f 115//115 101//101 99//99
+f 102//102 103//103 116//116
+f 133//133 117//117 120//120
+f 119//119 118//118 134//134
+f 133//133 120//120 122//122
+f 134//134 121//121 119//119
+f 133//133 122//122 124//124
+f 134//134 123//123 121//121
+f 133//133 124//124 126//126
+f 134//134 125//125 123//123
+f 133//133 126//126 128//128
+f 134//134 127//127 125//125
+f 133//133 128//128 130//130
+f 134//134 129//129 127//127
+f 133//133 130//130 132//132
+f 134//134 131//131 129//129
+f 133//133 132//132 117//117
+f 134//134 118//118 131//131
+f 153//153 154//154 155//155
+f 156//156 157//157 158//158
+f 153//153 155//155 159//159
+f 156//156 160//160 157//157
+f 153//153 159//159 161//161
+f 156//156 162//162 160//160
+f 153//153 161//161 163//163
+f 156//156 164//164 162//162
+f 153//153 163//163 165//165
+f 156//156 166//166 164//164
+f 153//153 165//165 167//167
+f 156//156 168//168 166//166
+f 153//153 167//167 169//169
+f 156//156 170//170 168//168
+f 153//153 169//169 171//171
+f 156//156 172//172 170//170
+f 153//153 171//171 173//173
+f 156//156 174//174 172//172
+f 153//153 173//173 175//175
+f 156//156 176//176 174//174
+f 153//153 175//175 177//177
+f 156//156 178//178 176//176
+f 153//153 177//177 154//154
+f 156//156 158//158 178//178
+f 239//239 217//217 218//218
+f 240//240 215//215 216//216
+f 239//239 218//218 220//220
+f 240//240 219//219 215//215
+f 239//239 220//220 222//222
+f 240//240 221//221 219//219
+f 239//239 222//222 224//224
+f 240//240 223//223 221//221
+f 239//239 224//224 226//226
+f 240//240 225//225 223//223
+f 239//239 226//226 228//228
+f 240//240 227//227 225//225
+f 239//239 228//228 230//230
+f 240//240 229//229 227//227
+f 239//239 230//230 232//232
+f 240//240 231//231 229//229
+f 239//239 232//232 234//234
+f 240//240 233//233 231//231
+f 239//239 234//234 236//236
+f 240//240 235//235 233//233
+f 239//239 236//236 238//238
+f 240//240 237//237 235//235
+f 239//239 238//238 217//217
+f 240//240 216//216 237//237
+f 241//241 242//242 243//243
+f 244//244 245//245 246//246
+f 241//241 243//243 247//247
+f 244//244 248//248 245//245
+f 241//241 247//247 249//249
+f 244//244 250//250 248//248
+f 241//241 249//249 251//251
+f 244//244 252//252 250//250
+f 241//241 251//251 253//253
+f 244//244 254//254 252//252
+f 241//241 253//253 255//255
+f 244//244 256//256 254//254
+f 241//241 255//255 257//257
+f 244//244 258//258 256//256
+f 241//241 257//257 259//259
+f 244//244 260//260 258//258
+f 241//241 259//259 261//261
+f 244//244 262//262 260//260
+f 241//241 261//261 263//263
+f 244//244 264//264 262//262
+f 241//241 263//263 265//265
+f 244//244 266//266 264//264
+f 241//241 265//265 242//242
+f 244//244 246//246 266//266
+f 300//300 296//296 298//298
+f 296//296 294//294 298//298
+f 291//291 300//300 298//298
+f 291//291 298//298 287//287
+f 299//299 290//290 286//286
+f 299//299 286//286 297//297
+f 295//295 299//299 297//297
+f 295//295 297//297 293//293
+f 300//300 291//291 299//299
+f 291//291 290//290 299//299
+f 296//296 300//300 299//299
+f 296//296 299//299 295//295
+f 297//297 286//286 287//287
+f 297//297 287//287 298//298
+f 293//293 297//297 298//298
+f 293//293 298//298 294//294
+f 296//296 292//292 294//294
+f 292//292 288//288 294//294
+f 293//293 285//285 289//289
+f 293//293 289//289 295//295
+f 292//292 296//296 289//289
+f 296//296 295//295 289//289
+f 285//285 293//293 294//294
+f 285//285 294//294 288//288
+f 289//289 285//285 292//292
+f 285//285 288//288 292//292
+f 286//286 290//290 291//291
+f 286//286 291//291 287//287
+f 265//265 246//246 242//242
+f 265//265 266//266 246//246
+f 263//263 266//266 265//265
+f 263//263 264//264 266//266
+f 261//261 264//264 263//263
+f 261//261 262//262 264//264
+f 259//259 262//262 261//261
+f 259//259 260//260 262//262
+f 257//257 260//260 259//259
+f 257//257 258//258 260//260
+f 255//255 258//258 257//257
+f 255//255 256//256 258//258
+f 253//253 256//256 255//255
+f 253//253 254//254 256//256
+f 251//251 254//254 253//253
+f 251//251 252//252 254//254
+f 249//249 252//252 251//251
+f 249//249 250//250 252//252
+f 247//247 250//250 249//249
+f 247//247 248//248 250//250
+f 243//243 248//248 247//247
+f 243//243 245//245 248//248
+f 242//242 245//245 243//243
+f 242//242 246//246 245//245
+f 216//216 217//217 238//238
+f 216//216 238//238 237//237
+f 237//237 238//238 236//236
+f 237//237 236//236 235//235
+f 235//235 236//236 234//234
+f 235//235 234//234 233//233
+f 233//233 234//234 232//232
+f 233//233 232//232 231//231
+f 231//231 232//232 230//230
+f 231//231 230//230 229//229
+f 229//229 230//230 228//228
+f 229//229 228//228 227//227
+f 227//227 228//228 226//226
+f 227//227 226//226 225//225
+f 225//225 226//226 224//224
+f 225//225 224//224 223//223
+f 223//223 224//224 222//222
+f 223//223 222//222 221//221
+f 221//221 222//222 220//220
+f 221//221 220//220 219//219
+f 219//219 220//220 218//218
+f 219//219 218//218 215//215
+f 215//215 218//218 217//217
+f 215//215 217//217 216//216
+f 177//177 158//158 154//154
+f 177//177 178//178 158//158
+f 175//175 178//178 177//177
+f 175//175 176//176 178//178
+f 173//173 176//176 175//175
+f 173//173 174//174 176//176
+f 171//171 174//174 173//173
+f 171//171 172//172 174//174
+f 169//169 172//172 171//171
+f 169//169 170//170 172//172
+f 167//167 170//170 169//169
+f 167//167 168//168 170//170
+f 165//165 168//168 167//167
+f 165//165 166//166 168//168
+f 163//163 166//166 165//165
+f 163//163 164//164 166//166
+f 161//161 164//164 163//163
+f 161//161 162//162 164//164
+f 159//159 162//162 161//161
+f 159//159 160//160 162//162
+f 155//155 160//160 159//159
+f 155//155 157//157 160//160
+f 154//154 157//157 155//155
+f 154//154 158//158 157//157
+f 118//118 132//132 131//131
+f 118//118 117//117 132//132
+f 131//131 132//132 129//129
+f 129//129 132//132 130//130
+f 129//129 130//130 127//127
+f 127//127 130//130 128//128
+f 127//127 128//128 125//125
+f 125//125 128//128 126//126
+f 125//125 126//126 123//123
+f 123//123 126//126 124//124
+f 123//123 122//122 121//121
+f 123//123 124//124 122//122
+f 121//121 120//120 119//119
+f 121//121 122//122 120//120
+f 117//117 119//119 120//120
+f 117//117 118//118 119//119
+f 103//103 101//101 115//115
+f 103//103 115//115 116//116
+f 113//113 114//114 116//116
+f 113//113 116//116 115//115
+f 111//111 112//112 114//114
+f 111//111 114//114 113//113
+f 109//109 110//110 111//111
+f 110//110 112//112 111//111
+f 107//107 108//108 109//109
+f 108//108 110//110 109//109
+f 105//105 106//106 107//107
+f 106//106 108//108 107//107
+f 100//100 104//104 105//105
+f 104//104 106//106 105//105
+f 101//101 103//103 104//104
+f 101//101 104//104 100//100
+f 82//82 96//96 95//95
+f 82//82 81//81 96//96
+f 95//95 96//96 93//93
+f 93//93 96//96 94//94
+f 93//93 94//94 91//91
+f 91//91 94//94 92//92
+f 91//91 92//92 89//89
+f 89//89 92//92 90//90
+f 89//89 90//90 87//87
+f 87//87 90//90 88//88
+f 87//87 86//86 85//85
+f 87//87 88//88 86//86
+f 85//85 84//84 83//83
+f 85//85 86//86 84//84
+f 81//81 83//83 84//84
+f 81//81 82//82 83//83
+f 71//71 63//63 70//70
+f 71//71 70//70 78//78
+f 69//69 77//77 78//78
+f 69//69 78//78 70//70
+f 68//68 76//76 77//77
+f 68//68 77//77 69//69
+f 67//67 75//75 68//68
+f 75//75 76//76 68//68
+f 66//66 74//74 67//67
+f 74//74 75//75 67//67
+f 65//65 73//73 66//66
+f 73//73 74//74 66//66
+f 64//64 72//72 65//65
+f 72//72 73//73 65//65
+f 63//63 71//71 72//72
+f 63//63 72//72 64//64
+f 23//23 21//21 35//35
+f 23//23 35//35 36//36
+f 33//33 34//34 36//36
+f 33//33 36//36 35//35
+f 31//31 32//32 34//34
+f 31//31 34//34 33//33
+f 29//29 30//30 32//32
+f 29//29 32//32 31//31
+f 27//27 28//28 30//30
+f 27//27 30//30 29//29
+f 25//25 26//26 28//28
+f 25//25 28//28 27//27
+f 20//20 24//24 26//26
+f 20//20 26//26 25//25
+f 24//24 20//20 21//21
+f 24//24 21//21 23//23
+f 2//2 1//1 16//16
+f 16//16 15//15 2//2
+f 15//15 14//14 13//13
+f 15//15 16//16 14//14
+f 13//13 12//12 11//11
+f 13//13 14//14 12//12
+f 11//11 10//10 9//9
+f 11//11 12//12 10//10
+f 9//9 8//8 7//7
+f 9//9 10//10 8//8
+f 7//7 6//6 5//5
+f 7//7 8//8 6//6
+f 5//5 4//4 3//3
+f 5//5 6//6 4//4
+f 1//1 3//3 4//4
+f 1//1 2//2 3//3
+f 138//138 137//137 136//136
+f 136//136 137//137 135//135
+f 138//138 140//140 139//139
+f 139//139 140//140 135//135
+f 138//138 142//142 141//141
+f 141//141 142//142 135//135
+f 138//138 144//144 143//143
+f 143//143 144//144 135//135
+f 138//138 146//146 145//145
+f 145//145 146//146 135//135
+f 138//138 148//148 147//147
+f 147//147 148//148 135//135
+f 138//138 150//150 149//149
+f 149//149 150//150 135//135
+f 138//138 152//152 151//151
+f 151//151 152//152 135//135
+f 179//179 181//181 180//180
+f 182//182 181//181 179//179
+f 184//184 183//183 180//180
+f 182//182 183//183 184//184
+f 186//186 185//185 180//180
+f 182//182 185//185 186//186
+f 188//188 187//187 180//180
+f 182//182 187//187 188//188
+f 190//190 189//189 180//180
+f 182//182 189//189 190//190
+f 192//192 191//191 180//180
+f 182//182 191//191 192//192
+f 194//194 193//193 180//180
+f 182//182 193//193 194//194
+f 196//196 195//195 180//180
+f 182//182 195//195 196//196
+f 200//200 201//201 198//198
+f 198//198 201//201 199//199
+f 200//200 203//203 202//202
+f 202//202 203//203 199//199
+f 200//200 205//205 204//204
+f 204//204 205//205 199//199
+f 200//200 207//207 206//206
+f 206//206 207//207 199//199
+f 200//200 209//209 208//208
+f 208//208 209//209 199//199
+f 200//200 211//211 210//210
+f 210//210 211//211 199//199
+f 200//200 213//213 212//212
+f 212//212 213//213 199//199
+f 200//200 197//197 214//214
+f 214//214 197//197 199//199
+f 267//267 269//269 268//268
+f 270//270 269//269 267//267
+f 272//272 271//271 268//268
+f 270//270 271//271 272//272
+f 274//274 273//273 268//268
+f 270//270 273//273 274//274
+f 276//276 275//275 268//268
+f 270//270 275//275 276//276
+f 278//278 277//277 268//268
+f 270//270 277//277 278//278
+f 280//280 279//279 268//268
+f 270//270 279//279 280//280
+f 282//282 281//281 268//268
+f 270//270 281//281 282//282
+f 284//284 283//283 268//268
+f 270//270 283//283 284//284
+# 1080 faces, 0 coords texture
+
+# End of File
\ No newline at end of file
diff --git a/data/checker_blue.png b/data/checker_blue.png
new file mode 100644
index 0000000..0894b91
Binary files /dev/null and b/data/checker_blue.png differ
diff --git a/data/checker_grid.jpg b/data/checker_grid.jpg
new file mode 100644
index 0000000..4165504
Binary files /dev/null and b/data/checker_grid.jpg differ
diff --git a/data/checker_huge.gif b/data/checker_huge.gif
new file mode 100644
index 0000000..da5d490
Binary files /dev/null and b/data/checker_huge.gif differ
diff --git a/data/cube.mtl b/data/cube.mtl
index 935e48a..ffce297 100644
--- a/data/cube.mtl
+++ b/data/cube.mtl
@@ -9,6 +9,8 @@ newmtl cube
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
- map_Ka cube.png
+ map_Ka cube.tga
map_Kd cube.png
+
+
diff --git a/data/cube.urdf b/data/cube.urdf
new file mode 100644
index 0000000..679e106
--- /dev/null
+++ b/data/cube.urdf
@@ -0,0 +1,32 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <rolling_friction value="0.0"/>
+ <contact_cfm value="0.0"/>
+ <contact_erp value="1.0"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="1.0"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="cube.obj" scale="1 1 1"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/cube_gripper_left.urdf b/data/cube_gripper_left.urdf
new file mode 100644
index 0000000..26c5389
--- /dev/null
+++ b/data/cube_gripper_left.urdf
@@ -0,0 +1,43 @@
+<?xml version="0.0" ?>
+<robot name="cube_gripper_left.urdf">
+ <link name="world">
+ </link>
+
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <spinning_friction value="0.3"/>
+ <inertia_scaling value="3.0"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value=".1"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0.785398 0.785398" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="cube.obj" scale=".05 .05 .05"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0.785398 0.785398" xyz="0 0 0"/>
+ <geometry>
+ <box size=".05 .05 .05"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="cube_joint" type="prismatic">
+ <parent link="world"/>
+ <child link="baseLink"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+ <dynamics damping="0.0"/>
+ </joint>
+</robot>
+
diff --git a/data/cube_gripper_right.urdf b/data/cube_gripper_right.urdf
new file mode 100644
index 0000000..ca642db
--- /dev/null
+++ b/data/cube_gripper_right.urdf
@@ -0,0 +1,43 @@
+<?xml version="0.0" ?>
+<robot name="cube_gripper_right.urdf">
+ <link name="world">
+ </link>
+
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <spinning_friction value="0.3"/>
+ <inertia_scaling value="3.0"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value=".1"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 -0.785398 -0.785398" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="cube.obj" scale=".05 .05 .05"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 -0.785398 -0.785398" xyz="0 0 0"/>
+ <geometry>
+ <box size=".05 .05 .05"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="cube_joint" type="prismatic">
+ <parent link="world"/>
+ <child link="baseLink"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+ <dynamics damping="0.0"/>
+ </joint>
+</robot>
+
diff --git a/data/cube_no_friction.urdf b/data/cube_no_friction.urdf
new file mode 100644
index 0000000..6df23a8
--- /dev/null
+++ b/data/cube_no_friction.urdf
@@ -0,0 +1,32 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="0.0"/>
+ <rolling_friction value="0.0"/>
+ <stiffness value="300.0"/>
+ <damping value="10.0"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="1.0"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="cube.obj" scale="1 1 1"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/cube_small.sdf b/data/cube_small.sdf
new file mode 100644
index 0000000..828b74d
--- /dev/null
+++ b/data/cube_small.sdf
@@ -0,0 +1,35 @@
+<sdf version='1.6'>
+ <world name='default'>
+ <model name='unit_box_0'>
+ <pose frame=''>0 0 0.107 0 0 0</pose>
+ <link name='unit_box_0::link'>
+ <inertial>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ </inertia>
+ </inertial>
+ <collision name='collision'>
+ <geometry>
+ <box>
+ <size>0.05 0.05 0.05</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name='visual'>
+ <geometry>
+ <mesh>
+ <scale>0.05 0.05 0.05</scale>
+ <uri>cube.obj</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+ </world>
+</sdf>
diff --git a/data/cube_small.urdf b/data/cube_small.urdf
new file mode 100644
index 0000000..299ded8
--- /dev/null
+++ b/data/cube_small.urdf
@@ -0,0 +1,30 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <inertia_scaling value="3.0"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value=".1"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="cube.obj" scale=".05 .05 .05"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size=".05 .05 .05"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/cube_soft.urdf b/data/cube_soft.urdf
new file mode 100644
index 0000000..af40dd3
--- /dev/null
+++ b/data/cube_soft.urdf
@@ -0,0 +1,32 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <rolling_friction value="0.0"/>
+ <stiffness value="300"/>
+ <damping value="10"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="1.0"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="cube.obj" scale="1 1 1"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/door.urdf b/data/door.urdf
new file mode 100644
index 0000000..012597d
--- /dev/null
+++ b/data/door.urdf
@@ -0,0 +1,105 @@
+<?xml version="0.0" ?>
+<robot name="urdf_door">
+ <link name="world"/>
+ <joint name="fixed" type="fixed">
+ <parent link="world"/>
+ <child link="baseLink"/>
+ <origin xyz="0 0 0"/>
+ </joint>
+
+ <link name="baseLink">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0.35"/>
+ <mass value="1.0"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.05 0 0.5"/>
+ <geometry>
+ <box size="0.1 0.1 1"/>
+ </geometry>
+ <material name="framemat0">
+ <color
+ rgba="0.9 0.4 0. 1" />
+ </material>
+ </visual>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.95 0 0.5"/>
+ <geometry>
+ <box size="0.1 0.1 1"/>
+ </geometry>
+ <material name="framemat0"/>
+ </visual>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.5 0 0.95"/>
+ <geometry>
+ <box size="1 0.1 0.1"/>
+ </geometry>
+ <material name="framemat0"/>
+ </visual>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.5 0 0.05"/>
+ <geometry>
+ <box size="1 0.1 0.1"/>
+ </geometry>
+ <material name="framemat0"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0.05 0 0.5"/>
+ <geometry>
+ <box size="0.1 0.1 1"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin rpy="0 0 0" xyz="0.95 0 0.5"/>
+ <geometry>
+ <box size="0.1 0.1 1"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin rpy="0 0 0" xyz="0.5 0 0.95"/>
+ <geometry>
+ <box size="1 0.1 0.1"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin rpy="0 0 0" xyz="0.5 0 0.05"/>
+ <geometry>
+ <box size="1 0.1 0.1"/>
+ </geometry>
+ </collision>
+
+ </link>
+ <link name="childA">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 -0.36"/>
+ <mass value="1.0"/>
+ <inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.4 0 0.4"/>
+ <geometry>
+ <box size="0.9 0.05 0.8"/>
+ </geometry>
+ <material name="doormat0">
+ <color rgba="0.8 0.8 0.3 1" />
+ </material>
+
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0.4 0 0.4"/>
+ <geometry>
+ <box size="0.9 0.05 0.8"/>
+ </geometry>
+ </collision>
+
+ </link>
+ <joint name="joint_baseLink_childA" type="continuous">
+ <parent link="baseLink"/>
+ <child link="childA"/>
+ <dynamics damping="1.0" friction="0.0001"/>
+ <origin xyz="0.05 0 0.1"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+</robot>
+
diff --git a/data/plane.mtl b/data/duck.mtl
similarity index 65%
copy from data/plane.mtl
copy to data/duck.mtl
index 8ce4b82..43e9be2 100644
--- a/data/plane.mtl
+++ b/data/duck.mtl
@@ -1,11 +1,13 @@
# Blender MTL File: 'None'
# Material Count: 1
-newmtl Material
+newmtl blinn3
Ns 96.078431
Ka 0.000000 0.000000 0.000000
Kd 0.640000 0.640000 0.640000
-Ks 0.500000 0.500000 0.500000
+Ks 0.000000 0.000000 0.000000
Ni 1.000000
d 1.000000
illum 2
+map_Ka duckCM.png
+map_Kd duckCM.png
\ No newline at end of file
diff --git a/data/duck.obj b/data/duck.obj
new file mode 100644
index 0000000..b4a71e1
--- /dev/null
+++ b/data/duck.obj
@@ -0,0 +1,8604 @@
+# Blender v2.68 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib duck.mtl
+o LOD3sp_LOD3spShape
+v -0.350226 0.893874 -0.233732
+v -0.195676 0.897173 -0.224879
+v -0.092291 0.915427 -0.171037
+v -0.043305 0.887008 -0.045773
+v -0.450571 0.894178 -0.198240
+v 0.305196 0.116272 -0.251326
+v 0.156992 0.114278 -0.342321
+v -0.518411 0.177055 -0.365602
+v -0.657206 0.183720 -0.270862
+v -0.560117 0.114345 -0.226963
+v 0.232343 0.181488 -0.410429
+v 0.409218 0.186322 -0.296382
+v -0.624487 0.113989 -0.129806
+v -0.602326 0.281944 -0.395949
+v -0.712984 0.290359 -0.293335
+v 0.329737 0.296914 -0.434770
+v 0.489500 0.289358 -0.314102
+v -0.738118 0.417425 -0.298584
+v -0.652513 0.413955 -0.398840
+v -0.726597 0.550030 -0.292468
+v -0.646263 0.554849 -0.382648
+v -0.665829 0.664165 -0.279218
+v -0.555179 0.677340 -0.357358
+v -0.434971 0.756992 -0.318699
+v -0.569340 0.750037 -0.257495
+v -0.147601 0.738701 -0.351574
+v 0.121248 0.739991 -0.289191
+v -0.147016 0.788465 -0.288886
+v 0.033796 0.780576 -0.231953
+v 0.247824 0.782304 -0.076512
+v -0.049422 0.814267 -0.158195
+v 0.547257 0.899761 -0.088449
+v 0.536566 0.747375 -0.269735
+v 0.441714 0.778938 -0.261268
+v 0.647587 0.738997 -0.071530
+v 0.615691 0.656958 -0.252253
+v 0.429296 0.612502 -0.394496
+v 0.649663 0.572673 -0.217398
+v 0.489596 0.492561 -0.398218
+v 0.583698 0.439468 -0.280360
+v 0.317993 0.708398 -0.334366
+v 0.331153 0.823349 -0.086247
+v -0.482452 0.810789 -0.223270
+v -0.341225 0.805718 -0.266473
+v -0.155527 0.813807 -0.244230
+v -0.006560 0.810723 -0.039938
+v -0.151798 0.114100 -0.364164
+v -0.172973 0.173674 -0.432976
+v -0.438649 0.677941 -0.398677
+v -0.559835 0.561625 -0.427808
+v -0.564187 0.417648 -0.444371
+v -0.469566 0.290085 -0.442399
+v -0.180410 0.213370 -0.454158
+v 0.242634 0.297596 -0.464694
+v 0.371346 0.446474 -0.444312
+v 0.358668 0.578953 -0.427333
+v 0.241626 0.668013 -0.396298
+v 0.142645 0.434767 -0.518892
+v -0.101522 0.438267 -0.539252
+v 0.103735 0.363160 -0.518336
+v 0.145047 0.522745 -0.510455
+v -0.093544 0.521796 -0.535056
+v -0.245685 0.369247 -0.524691
+v -0.112191 0.356243 -0.525988
+v -0.270248 0.445585 -0.528835
+v -0.254374 0.550852 -0.520724
+v -0.092313 0.592580 -0.512115
+v -0.212751 0.616417 -0.503945
+v 0.110645 0.573325 -0.503033
+v 0.228339 0.538174 -0.486047
+v 0.223883 0.433299 -0.498488
+v 0.133437 0.344469 -0.507719
+v 0.150193 0.597488 -0.480109
+v -0.123031 0.316369 -0.514681
+v -0.282000 0.349703 -0.514911
+v -0.349314 0.441559 -0.512583
+v -0.337281 0.558993 -0.500475
+v -0.248495 0.633700 -0.486114
+v -0.099616 0.628873 -0.484038
+v -0.067334 0.845266 -0.028379
+v -0.102211 0.849641 -0.131445
+v -0.190500 0.850093 -0.204734
+v -0.352584 0.849759 -0.220089
+v -0.448651 0.851990 -0.187578
+v -0.541484 0.901992 -0.131393
+v -0.258400 0.893162 -0.235593
+v -0.145318 0.904728 -0.205795
+v -0.059928 0.895698 -0.101098
+v -0.591034 0.900324 -0.033769
+v 0.239364 0.115353 -0.306125
+v 0.114590 0.100413 -0.298243
+v 0.245326 0.101147 -0.222069
+v 0.351528 0.116836 -0.177191
+v -0.615472 0.142726 -0.252082
+v -0.436854 0.114300 -0.300164
+v -0.477677 0.141162 -0.336212
+v 0.194590 0.143090 -0.379267
+v 0.360670 0.145054 -0.276816
+v 0.329292 0.185574 -0.367248
+v 0.461733 0.186604 -0.205246
+v -0.733418 0.183468 -0.149194
+v -0.688732 0.234107 -0.284208
+v -0.557803 0.224988 -0.385465
+v 0.270171 0.234671 -0.430366
+v 0.452302 0.236628 -0.309246
+v 0.410731 0.293325 -0.394763
+v -0.794615 0.291894 -0.160672
+v -0.769844 0.234678 -0.155749
+v -0.728865 0.351223 -0.298369
+v -0.632406 0.345574 -0.398307
+v -0.818608 0.421140 -0.167292
+v -0.809318 0.353877 -0.164090
+v -0.660817 0.485555 -0.392449
+v -0.739987 0.485347 -0.296857
+v -0.801688 0.551853 -0.169480
+v -0.815894 0.488387 -0.169628
+v -0.611927 0.620057 -0.372950
+v -0.701700 0.610152 -0.286395
+v -0.748647 0.663268 -0.166010
+v -0.777874 0.610196 -0.167611
+v -0.617637 0.710370 -0.268557
+v -0.671835 0.754493 -0.166366
+v -0.716321 0.711772 -0.166670
+v 0.074004 0.761581 -0.263759
+v -0.148335 0.763145 -0.320471
+v 0.209470 0.750059 -0.213995
+v 0.093096 0.780917 -0.176635
+v -0.023331 0.812658 -0.107014
+v 0.553055 0.814823 -0.201865
+v 0.441025 0.828784 -0.210800
+v 0.483998 0.772606 -0.269595
+v 0.638341 0.698359 -0.177050
+v 0.582579 0.706396 -0.264175
+v 0.581459 0.604109 -0.315845
+v 0.501918 0.687831 -0.330970
+v 0.535557 0.461592 -0.349928
+v 0.630690 0.527290 -0.240553
+v 0.636495 0.607883 -0.233658
+v 0.544261 0.285392 -0.216382
+v 0.124228 0.392454 -0.519419
+v 0.521766 0.341006 -0.309987
+v 0.393582 0.371753 -0.428245
+v 0.632047 0.333562 -0.077735
+v 0.149689 0.480973 -0.515712
+v 0.132184 0.681588 -0.385488
+v 0.208410 0.719312 -0.315875
+v 0.340814 0.736573 -0.231685
+v -0.390864 0.776536 -0.294959
+v -0.522860 0.782905 -0.242985
+v -0.286752 0.750467 -0.347400
+v -0.245284 0.794248 -0.290977
+v -0.582456 0.816454 -0.149372
+v -0.625867 0.788584 -0.161198
+v 0.400580 0.733058 -0.290169
+v 0.624832 0.427738 -0.196053
+v 0.662542 0.577677 -0.160138
+v 0.463972 0.555620 -0.403341
+v -0.461403 0.832928 -0.200590
+v -0.528472 0.875160 -0.123304
+v -0.243957 0.812413 -0.264145
+v -0.175457 0.832299 -0.216568
+v -0.350308 0.869496 -0.219178
+v -0.056606 0.830193 -0.029728
+v -0.392717 0.100042 -0.259897
+v -0.497414 0.099968 -0.197068
+v -0.146156 0.100124 -0.317958
+v -0.282119 0.114278 -0.345791
+v -0.551790 0.100309 -0.114214
+v -0.161259 0.139798 -0.402177
+v -0.330793 0.177336 -0.413765
+v -0.189195 0.362041 -0.525892
+v -0.106044 0.396524 -0.534314
+v -0.182523 0.443116 -0.537955
+v -0.259535 0.399994 -0.527745
+v -0.097355 0.480914 -0.538815
+v -0.169236 0.536009 -0.532898
+v -0.268721 0.498693 -0.526952
+v -0.091260 0.558675 -0.527181
+v -0.159783 0.608706 -0.508542
+v -0.232428 0.592083 -0.511997
+v -0.152006 0.709348 -0.384909
+v 0.380814 0.666122 -0.376368
+v 0.314745 0.632943 -0.412950
+v 0.129337 0.554359 -0.506592
+v 0.298093 0.429681 -0.465354
+v 0.186612 0.311764 -0.478255
+v 0.302660 0.558423 -0.447033
+v -0.362897 0.312217 -0.480605
+v -0.465110 0.427776 -0.477899
+v -0.456636 0.564673 -0.462574
+v -0.328524 0.661703 -0.442955
+v -0.123579 0.672602 -0.432175
+v 0.191490 0.381518 -0.504005
+v 0.236628 0.488825 -0.492194
+v -0.211045 0.327401 -0.514785
+v -0.324268 0.389651 -0.514600
+v -0.352465 0.500413 -0.507771
+v -0.179201 0.637837 -0.484371
+v -0.300180 0.605970 -0.492453
+v 0.204236 0.576424 -0.481257
+v 0.071135 0.605511 -0.482081
+v 0.055476 0.574675 -0.506459
+v 0.062067 0.514293 -0.524750
+v 0.059546 0.433855 -0.531660
+v 0.046697 0.357333 -0.522177
+v 0.057025 0.324450 -0.511085
+v 0.083205 0.178723 -0.433717
+v 0.026968 0.113425 -0.364653
+v 0.272981 0.428866 -0.480687
+v 0.278897 0.550430 -0.465161
+v 0.164436 0.322248 -0.493054
+v 0.185848 0.623861 -0.455990
+v -0.136265 0.280639 -0.499986
+v -0.413922 0.434352 -0.494685
+v -0.323868 0.327171 -0.498711
+v -0.404699 0.563449 -0.480345
+v -0.289733 0.650389 -0.463945
+v -0.110523 0.656624 -0.456909
+v 0.379272 0.117800 -0.069973
+v 0.502675 0.183890 -0.073635
+v -0.758256 0.183928 -0.029395
+v -0.642756 0.116576 -0.026599
+v -0.828128 0.291368 -0.029395
+v -0.858726 0.421384 -0.029395
+v -0.849147 0.552091 -0.032575
+v -0.799360 0.662007 -0.032575
+v -0.729207 0.756569 -0.031167
+v 0.132799 0.782816 -0.067808
+v 0.424217 0.915301 -0.090221
+v 0.662519 0.423750 -0.075912
+v 0.679758 0.585114 -0.069980
+v -0.087642 0.815601 -0.202429
+v -0.086500 0.831534 -0.137124
+v -0.061663 0.863527 -0.033376
+v -0.103479 0.874003 -0.139949
+v -0.079197 0.847475 -0.081836
+v -0.195238 0.870994 -0.206484
+v -0.138638 0.850248 -0.172846
+v -0.351961 0.830430 -0.235111
+v -0.260550 0.849410 -0.223278
+v -0.444291 0.871742 -0.186599
+v -0.551672 0.837325 -0.132216
+v -0.533737 0.855572 -0.123674
+v 0.187264 0.101080 -0.266814
+v 0.285504 0.101785 -0.157995
+v 0.286467 0.144120 -0.339793
+v 0.409463 0.146426 -0.192219
+v -0.686182 0.142022 -0.140757
+v 0.369819 0.237436 -0.385844
+v -0.485084 0.721529 -0.339029
+v 0.147042 0.766096 -0.197209
+v 0.495150 0.838778 -0.207678
+v 0.603339 0.763635 -0.190618
+v 0.545522 0.648765 -0.329243
+v 0.608158 0.559957 -0.285728
+v 0.506442 0.234975 -0.212438
+v 0.451383 0.351824 -0.390803
+v 0.577470 0.335446 -0.215100
+v 0.275531 0.736877 -0.228942
+v -0.266207 0.769559 -0.319144
+v 0.393930 0.779086 -0.222647
+v 0.655928 0.516658 -0.172594
+v 0.448336 0.713336 -0.321213
+v 0.655690 0.635509 -0.165565
+v -0.253055 0.831594 -0.237810
+v -0.121192 0.832358 -0.180750
+v -0.259631 0.100257 -0.300393
+v -0.305288 0.140969 -0.382233
+v -0.184628 0.399920 -0.532994
+v -0.176910 0.489863 -0.538221
+v -0.161429 0.576002 -0.523171
+v -0.322615 0.714100 -0.379727
+v -0.514408 0.626760 -0.414618
+v -0.576406 0.489151 -0.438433
+v -0.529177 0.350586 -0.445773
+v -0.362170 0.231483 -0.439056
+v 0.317681 0.366178 -0.457769
+v 0.378300 0.515724 -0.436927
+v 0.259576 0.365644 -0.472776
+v 0.312662 0.496491 -0.456583
+v -0.267542 0.275116 -0.481161
+v -0.232102 0.678126 -0.435534
+v 0.098115 0.645785 -0.425347
+v 0.057351 0.546841 -0.518166
+v 0.063446 0.475345 -0.530926
+v 0.051828 0.393551 -0.528687
+v 0.130679 0.235531 -0.456457
+v 0.052198 0.140791 -0.402548
+v 0.006794 0.100168 -0.318299
+v 0.287520 0.492710 -0.472864
+v 0.238534 0.366519 -0.487626
+v 0.246964 0.595316 -0.458889
+v 0.071594 0.293978 -0.497079
+v -0.381551 0.375127 -0.497814
+v -0.239612 0.295453 -0.499126
+v -0.422701 0.499123 -0.488776
+v -0.358278 0.616884 -0.471419
+v -0.205055 0.662430 -0.459037
+v 0.089566 0.632617 -0.455834
+v 0.311142 0.101703 -0.063849
+v 0.441714 0.146552 -0.072783
+v -0.706407 0.143913 -0.029283
+v -0.799353 0.234130 -0.029395
+v -0.845803 0.353544 -0.029395
+v -0.859631 0.488595 -0.032575
+v -0.828254 0.610226 -0.032583
+v -0.767546 0.713433 -0.032553
+v 0.192017 0.778115 -0.073361
+v 0.485489 0.922656 -0.090651
+v 0.601596 0.844057 -0.082814
+v 0.557570 0.230430 -0.077306
+v 0.292013 0.796561 -0.080857
+v 0.371064 0.881752 -0.089146
+v 0.680046 0.515783 -0.071537
+v 0.669147 0.656180 -0.070774
+v -0.570756 0.101525 -0.022855
+v -0.066400 0.830734 -0.086410
+v -0.076706 0.867590 -0.086381
+v -0.143983 0.872706 -0.178748
+v -0.262100 0.869437 -0.221654
+v -0.623057 0.925874 -0.029313
+v -0.013848 0.692606 -0.385873
+v -0.015627 0.654519 -0.430262
+v -0.013225 0.641537 -0.456605
+v -0.049555 0.099983 -0.323904
+v -0.046308 0.113477 -0.370599
+v -0.044817 0.139220 -0.409628
+v -0.040821 0.173859 -0.439419
+v -0.030293 0.214474 -0.458963
+v -0.030752 0.282293 -0.499356
+v -0.031101 0.316361 -0.513265
+v -0.030300 0.354574 -0.524675
+v -0.026133 0.394033 -0.533009
+v -0.021136 0.434389 -0.538266
+v -0.017577 0.475190 -0.537873
+v -0.016376 0.513863 -0.533032
+v -0.017029 0.548295 -0.524802
+v -0.017674 0.580161 -0.510099
+v -0.015220 0.615001 -0.483586
+v -0.019364 0.728387 -0.338763
+v -0.036476 0.759142 -0.305480
+v -0.053181 0.784269 -0.269639
+v -0.585778 0.875953 -0.022336
+v -0.591501 0.858263 -0.021172
+v -0.615583 0.842063 -0.025176
+v -0.649644 0.821214 -0.028920
+v -0.690133 0.791475 -0.030099
+v -0.327991 0.897151 0.304233
+v -0.183501 0.894171 0.279721
+v -0.086752 0.895668 0.209938
+v -0.023931 0.903705 0.098568
+v -0.413633 0.901718 0.287054
+v 0.311060 0.114775 0.329686
+v 0.161893 0.114130 0.418242
+v 0.378441 0.117199 0.179362
+v -0.511145 0.178270 0.437764
+v -0.654167 0.183720 0.346672
+v -0.555386 0.114663 0.302365
+v 0.227101 0.183816 0.481723
+v 0.412258 0.186322 0.372200
+v 0.499064 0.181281 0.194546
+v -0.620944 0.114196 0.205393
+v -0.593140 0.284606 0.465397
+v -0.709277 0.290352 0.368544
+v 0.331991 0.298730 0.507109
+v 0.493615 0.288780 0.390246
+v -0.734812 0.417559 0.373453
+v -0.646226 0.415905 0.465931
+v -0.729066 0.550971 0.370309
+v -0.639264 0.552231 0.455736
+v -0.673080 0.664328 0.361160
+v -0.558026 0.667664 0.441694
+v -0.439561 0.746649 0.398150
+v -0.578297 0.752380 0.341393
+v -0.155401 0.731894 0.425115
+v 0.122174 0.738819 0.364385
+v -0.153978 0.787998 0.355147
+v 0.020421 0.784120 0.294750
+v 0.247876 0.765792 0.197267
+v 0.125340 0.782897 0.176678
+v -0.043446 0.811227 0.241642
+v 0.558163 0.872424 0.220756
+v 0.437036 0.883494 0.223158
+v 0.541303 0.747924 0.346324
+v 0.444888 0.778752 0.336648
+v 0.655053 0.725896 0.185049
+v 0.619613 0.657536 0.329553
+v 0.434634 0.604480 0.474894
+v 0.651954 0.570723 0.295143
+v 0.592654 0.279119 0.202361
+v 0.492087 0.491998 0.473723
+v 0.589689 0.437199 0.355858
+v 0.663446 0.421948 0.194391
+v -0.155861 0.700443 0.460785
+v 0.331287 0.693852 0.414713
+v 0.344892 0.800254 0.215618
+v -0.486990 0.811865 0.308689
+v -0.342382 0.805703 0.342498
+v -0.165759 0.811153 0.318950
+v -0.007279 0.811872 0.124148
+v 0.680180 0.582133 0.178183
+v -0.147802 0.113848 0.440174
+v -0.169221 0.174452 0.508066
+v 0.130864 0.433447 0.588481
+v -0.104495 0.439097 0.610858
+v 0.093266 0.366667 0.590505
+v 0.142200 0.519030 0.580496
+v -0.094560 0.519980 0.600277
+v -0.242460 0.370634 0.605519
+v -0.113896 0.357436 0.602776
+v -0.265918 0.446467 0.610071
+v -0.254626 0.544929 0.595273
+v -0.090067 0.589948 0.572808
+v -0.213366 0.605555 0.572059
+v 0.111972 0.572725 0.569708
+v 0.222438 0.531256 0.560203
+v 0.203457 0.430689 0.573067
+v 0.121122 0.350371 0.581689
+v 0.149222 0.593774 0.552218
+v -0.122904 0.317184 0.591973
+v -0.277581 0.352002 0.594961
+v -0.338971 0.444969 0.595273
+v -0.327486 0.555991 0.579969
+v -0.245692 0.625262 0.558246
+v -0.096903 0.623164 0.552589
+v -0.063101 0.846215 0.095907
+v -0.099409 0.849232 0.200693
+v -0.190219 0.850345 0.274843
+v -0.347653 0.850426 0.294209
+v -0.441941 0.851190 0.267866
+v -0.508728 0.906708 0.236459
+v -0.250482 0.895646 0.299540
+v -0.129607 0.894230 0.249078
+v -0.052550 0.898204 0.162020
+v -0.032473 0.885844 0.027243
+v -0.569280 0.897848 0.105352
+v 0.243405 0.114359 0.383603
+v 0.120514 0.099939 0.374698
+v 0.251190 0.099294 0.301860
+v 0.354745 0.116324 0.256774
+v 0.309667 0.101577 0.169998
+v -0.610000 0.143030 0.326913
+v -0.429826 0.115093 0.374209
+v -0.469722 0.142133 0.407944
+v 0.195197 0.143549 0.453897
+v 0.364880 0.144595 0.353605
+v 0.331887 0.185722 0.442309
+v 0.441233 0.144447 0.186976
+v 0.466619 0.183928 0.282843
+v -0.730378 0.183468 0.225011
+v -0.685692 0.234107 0.360025
+v -0.548395 0.227093 0.457182
+v 0.268814 0.236917 0.501401
+v 0.455350 0.236628 0.385063
+v 0.413718 0.293444 0.467703
+v -0.791575 0.291894 0.236481
+v -0.766804 0.234678 0.231566
+v -0.725633 0.351268 0.373883
+v -0.625170 0.348109 0.466606
+v -0.815568 0.421140 0.243110
+v -0.806278 0.353877 0.239899
+v -0.652083 0.485511 0.461149
+v -0.738400 0.485629 0.373000
+v -0.806871 0.553477 0.248070
+v -0.816740 0.488988 0.246283
+v -0.607961 0.614452 0.450316
+v -0.707794 0.611190 0.367907
+v -0.755475 0.665218 0.246246
+v -0.786126 0.612650 0.248040
+v -0.629885 0.710490 0.351951
+v -0.668647 0.754648 0.244251
+v -0.714756 0.712009 0.244489
+v 0.068569 0.761307 0.332993
+v -0.153904 0.761240 0.389186
+v 0.206749 0.749933 0.288188
+v 0.186590 0.771776 0.189697
+v 0.083190 0.783549 0.242702
+v -0.019646 0.812562 0.183514
+v 0.558430 0.813140 0.285504
+v 0.496254 0.893466 0.224648
+v 0.444451 0.826048 0.289686
+v 0.486808 0.772324 0.345316
+v 0.643131 0.696573 0.258984
+v 0.617107 0.818990 0.209783
+v 0.587835 0.707123 0.341097
+v 0.584469 0.603879 0.391736
+v 0.505077 0.687809 0.406817
+v 0.541830 0.459931 0.422824
+v 0.638549 0.525711 0.320063
+v 0.639335 0.607290 0.311655
+v 0.549570 0.228131 0.199766
+v 0.551520 0.282115 0.295499
+v 0.112032 0.394137 0.590290
+v 0.530760 0.342815 0.385249
+v 0.397533 0.373377 0.501979
+v 0.631283 0.334630 0.201464
+v 0.142141 0.477244 0.584915
+v 0.300695 0.773155 0.205683
+v 0.215757 0.712928 0.393886
+v 0.342579 0.739026 0.308400
+v -0.392161 0.776454 0.370836
+v -0.527939 0.784766 0.328685
+v -0.297051 0.737336 0.420511
+v -0.252180 0.793136 0.358083
+v -0.579394 0.817173 0.232211
+v -0.623879 0.789177 0.242442
+v 0.403947 0.733170 0.365831
+v 0.386256 0.846334 0.221512
+v 0.631328 0.425522 0.278513
+v 0.680395 0.512550 0.183870
+v 0.666864 0.575891 0.246884
+v 0.467190 0.553544 0.480974
+v 0.670838 0.651042 0.179110
+v -0.460958 0.832499 0.286417
+v -0.509529 0.873143 0.206038
+v -0.252513 0.810708 0.333601
+v -0.179216 0.832039 0.290153
+v -0.340372 0.871164 0.290287
+v -0.051772 0.830067 0.104967
+v -0.387965 0.100502 0.336129
+v -0.493922 0.100272 0.273241
+v -0.141870 0.099360 0.395629
+v -0.277425 0.114278 0.421534
+v -0.550359 0.100176 0.190023
+v -0.156372 0.139546 0.478631
+v -0.327953 0.177996 0.488099
+v -0.188513 0.362745 0.605645
+v -0.108610 0.397903 0.608997
+v -0.183879 0.443219 0.613282
+v -0.255709 0.401514 0.609249
+v -0.099846 0.480417 0.607803
+v -0.171720 0.530256 0.601664
+v -0.267053 0.497018 0.606231
+v -0.090245 0.556280 0.588755
+v -0.159709 0.599690 0.573289
+v -0.232776 0.582326 0.582090
+v -0.441377 0.664765 0.474420
+v -0.329162 0.700487 0.455996
+v 0.391943 0.648054 0.457138
+v -0.555401 0.555242 0.497715
+v -0.512480 0.616595 0.487462
+v -0.560065 0.419561 0.512803
+v -0.570734 0.488091 0.505693
+v -0.465132 0.292250 0.515050
+v -0.525663 0.353329 0.516029
+v -0.175056 0.214015 0.531414
+v -0.357529 0.232580 0.514249
+v 0.248654 0.303579 0.528774
+v 0.375950 0.446801 0.517631
+v 0.323879 0.370560 0.526379
+v 0.363175 0.565451 0.506094
+v 0.380947 0.508613 0.514190
+v 0.250582 0.659138 0.472878
+v 0.130315 0.552283 0.575699
+v 0.295994 0.427331 0.540504
+v 0.177196 0.321054 0.553997
+v 0.310452 0.549518 0.521567
+v -0.144814 0.256447 0.559766
+v -0.356595 0.316265 0.556473
+v -0.450786 0.432372 0.554835
+v -0.442682 0.561306 0.540718
+v -0.326130 0.653459 0.518098
+v -0.122638 0.666664 0.503239
+v 0.168788 0.385536 0.577812
+v 0.222779 0.481633 0.566920
+v -0.209614 0.327638 0.593975
+v -0.317892 0.392914 0.596303
+v -0.342463 0.501711 0.589927
+v -0.177651 0.631379 0.554049
+v -0.293959 0.599275 0.567944
+v 0.199928 0.570804 0.554805
+v 0.072388 0.603598 0.552040
+v 0.056580 0.575512 0.570761
+v 0.060065 0.513551 0.590490
+v 0.052221 0.434715 0.598809
+v 0.039639 0.360409 0.595080
+v 0.051561 0.328988 0.585686
+v 0.135854 0.239342 0.526112
+v 0.090641 0.181577 0.505723
+v 0.032588 0.113870 0.439714
+v 0.262193 0.426871 0.556006
+v 0.285919 0.544558 0.538865
+v 0.150875 0.333422 0.567928
+v 0.188250 0.619909 0.528915
+v -0.133618 0.277844 0.577174
+v -0.398055 0.439520 0.575439
+v -0.318122 0.331212 0.576826
+v -0.389151 0.561492 0.560878
+v -0.285307 0.643375 0.538731
+v -0.108825 0.652576 0.528604
+v 0.387872 0.117814 0.103899
+v 0.513804 0.182081 0.111306
+v -0.759308 0.182734 0.106190
+v -0.643713 0.115264 0.101319
+v -0.826956 0.291294 0.107480
+v -0.856139 0.421370 0.115317
+v -0.847924 0.553455 0.124133
+v -0.796343 0.663979 0.129219
+v -0.716684 0.756332 0.127061
+v 0.252621 0.791542 0.112952
+v 0.143624 0.783676 0.105812
+v 0.554174 0.911379 0.151558
+v 0.431913 0.920224 0.149719
+v 0.655402 0.747902 0.111558
+v 0.610434 0.280454 0.113123
+v 0.326111 0.846875 0.125304
+v 0.672833 0.424839 0.109141
+v 0.688802 0.586841 0.104374
+v -0.005433 0.811205 0.062216
+v -0.095242 0.810708 0.287966
+v -0.083757 0.831824 0.211125
+v -0.055308 0.866678 0.092444
+v -0.063553 0.843783 0.037215
+v -0.099194 0.869399 0.199543
+v -0.074630 0.848157 0.151358
+v -0.191716 0.869829 0.271848
+v -0.137244 0.849789 0.242465
+v -0.350315 0.830667 0.312656
+v -0.261633 0.850641 0.293275
+v -0.428321 0.872394 0.265835
+v -0.544673 0.836613 0.215529
+v -0.521903 0.853592 0.204029
+v 0.191772 0.099701 0.345627
+v 0.290093 0.100524 0.239684
+v 0.289966 0.143787 0.416404
+v 0.414341 0.144313 0.271351
+v -0.682653 0.142141 0.216559
+v 0.371724 0.237510 0.458917
+v -0.491935 0.709503 0.423484
+v 0.145648 0.766133 0.270075
+v 0.498916 0.836109 0.289872
+v 0.611227 0.761114 0.272671
+v 0.550601 0.650337 0.405186
+v 0.613029 0.558348 0.362984
+v 0.512017 0.231727 0.290783
+v 0.458464 0.354359 0.463143
+v 0.586945 0.336906 0.294149
+v 0.273062 0.738878 0.303291
+v -0.271279 0.767275 0.388845
+v 0.396428 0.780687 0.298339
+v 0.661326 0.513373 0.258257
+v 0.451368 0.712869 0.396934
+v 0.658464 0.634174 0.250094
+v -0.258474 0.831357 0.308770
+v -0.121451 0.832039 0.255855
+v -0.254849 0.099383 0.379992
+v -0.300780 0.141043 0.457664
+v -0.184480 0.400625 0.612347
+v -0.179876 0.487616 0.610605
+v -0.162348 0.567727 0.588674
+v 0.322767 0.620154 0.492051
+v 0.139368 0.678282 0.459206
+v 0.248684 0.370434 0.548511
+v 0.315250 0.488313 0.531740
+v -0.264258 0.275071 0.556637
+v -0.232554 0.670170 0.508845
+v 0.101563 0.644547 0.500444
+v 0.057574 0.547064 0.582557
+v 0.058419 0.475145 0.595984
+v 0.043791 0.395768 0.598409
+v 0.059761 0.142111 0.475917
+v 0.009262 0.099627 0.395747
+v 0.285177 0.485533 0.547769
+v 0.216936 0.374949 0.562842
+v 0.254638 0.591765 0.531910
+v 0.068028 0.301029 0.572111
+v -0.370326 0.381125 0.577227
+v -0.237499 0.294563 0.576722
+v -0.406137 0.501599 0.570286
+v -0.348580 0.611842 0.549200
+v -0.204387 0.657863 0.531414
+v 0.091835 0.631068 0.528225
+v 0.317926 0.101666 0.099777
+v 0.452866 0.145017 0.108326
+v -0.707816 0.142259 0.105137
+v -0.799368 0.233648 0.106420
+v -0.843683 0.353670 0.110661
+v -0.858037 0.489262 0.120255
+v -0.827112 0.612146 0.126921
+v -0.760168 0.714278 0.133949
+v 0.202604 0.779924 0.112248
+v 0.493845 0.929811 0.153182
+v 0.608729 0.856046 0.142238
+v 0.564851 0.228762 0.112211
+v 0.643531 0.342467 0.111677
+v 0.291309 0.810871 0.114450
+v 0.373177 0.893021 0.145323
+v 0.690093 0.516317 0.106220
+v 0.676547 0.661837 0.104878
+v -0.057155 0.830356 0.044281
+v -0.570919 0.100917 0.095929
+v -0.060372 0.830742 0.161330
+v -0.054597 0.861214 0.032603
+v -0.071205 0.868962 0.150149
+v -0.138867 0.869385 0.240596
+v -0.259809 0.870964 0.289590
+v 0.116043 1.227810 -0.086848
+v 0.112981 1.226920 -0.096968
+v 0.109770 1.223660 -0.106888
+v 0.106656 1.218200 -0.115860
+v 0.103765 1.210780 -0.123445
+v 0.101237 1.201750 -0.129302
+v 0.099183 1.191560 -0.133142
+v 0.097722 1.180670 -0.134803
+v 0.096877 1.169600 -0.134188
+v 0.096706 1.158870 -0.131341
+v 0.097233 1.148990 -0.126358
+v 0.098582 1.140210 -0.118907
+v 0.107227 1.129140 -0.087189
+v 0.111023 1.279440 -0.084156
+v 0.104921 1.277670 -0.104464
+v 0.098597 1.271220 -0.124230
+v 0.092465 1.260370 -0.142114
+v 0.086808 1.245610 -0.157276
+v 0.081863 1.227640 -0.168946
+v 0.077896 1.207330 -0.176635
+v 0.074931 1.185630 -0.179719
+v 0.073344 1.163630 -0.178451
+v 0.073085 1.142330 -0.172735
+v 0.074182 1.122730 -0.162733
+v 0.076999 1.105340 -0.147860
+v 0.082775 1.090330 -0.124727
+v 0.093578 1.080800 -0.085424
+v 0.099561 1.329980 -0.079626
+v 0.090493 1.327370 -0.109810
+v 0.081114 1.317810 -0.139103
+v 0.072032 1.301730 -0.165602
+v 0.063661 1.279880 -0.188075
+v 0.056402 1.253280 -0.205468
+v 0.050582 1.223180 -0.216961
+v 0.046482 1.190990 -0.222025
+v 0.044228 1.158240 -0.220238
+v 0.043976 1.126500 -0.211845
+v 0.045629 1.097460 -0.196875
+v 0.049707 1.072110 -0.173891
+v 0.074249 1.033830 -0.082177
+v 0.082048 1.378790 -0.073369
+v 0.070141 1.375370 -0.112983
+v 0.057855 1.362840 -0.151374
+v 0.045956 1.341760 -0.186110
+v 0.034983 1.313130 -0.215559
+v 0.025463 1.278270 -0.238351
+v 0.017841 1.238830 -0.253417
+v 0.012465 1.196640 -0.260060
+v 0.009589 1.153670 -0.257954
+v 0.009322 1.112020 -0.247018
+v 0.011568 1.074110 -0.226977
+v 0.017196 1.040940 -0.196809
+v 0.049099 0.990215 -0.077476
+v 0.058745 1.425160 -0.065480
+v 0.044176 1.420980 -0.113954
+v 0.029162 1.405660 -0.160879
+v 0.014608 1.379890 -0.203341
+v 0.001196 1.344890 -0.239337
+v -0.010437 1.302290 -0.267200
+v -0.019757 1.254070 -0.285617
+v -0.026326 1.202490 -0.293728
+v -0.029840 1.149980 -0.291163
+v -0.030130 1.098970 -0.278025
+v -0.027223 1.052480 -0.253810
+v -0.020039 1.013150 -0.214907
+v 0.019034 0.951527 -0.070885
+v 0.029985 1.468400 -0.056071
+v 0.012969 1.463530 -0.112694
+v -0.004558 1.445640 -0.167478
+v -0.021544 1.415560 -0.217035
+v -0.037203 1.374700 -0.259059
+v -0.050786 1.324970 -0.291585
+v -0.061663 1.268690 -0.313079
+v -0.069336 1.208480 -0.322555
+v -0.073429 1.147180 -0.319552
+v -0.073770 1.087640 -0.304219
+v -0.070256 1.033350 -0.275927
+v -0.061596 0.988146 -0.229217
+v -0.013106 0.915316 -0.059222
+v -0.003809 1.507890 -0.045284
+v -0.023019 1.502390 -0.109231
+v -0.042808 1.482200 -0.171066
+v -0.061981 1.448250 -0.227007
+v -0.079650 1.402130 -0.274444
+v -0.094982 1.346000 -0.311152
+v -0.107268 1.282460 -0.335419
+v -0.115920 1.214500 -0.346110
+v -0.120547 1.145310 -0.342722
+v -0.120932 1.078100 -0.325417
+v -0.116995 1.016280 -0.294492
+v -0.107461 0.964739 -0.241724
+v -0.042141 1.543050 -0.033265
+v -0.063271 1.537000 -0.103604
+v -0.085032 1.514810 -0.171593
+v -0.106111 1.477490 -0.233102
+v -0.125537 1.426770 -0.285254
+v -0.142397 1.365050 -0.325617
+v -0.155905 1.295190 -0.352301
+v -0.165418 1.220480 -0.364053
+v -0.170504 1.144390 -0.360331
+v -0.170919 1.070500 -0.341306
+v -0.166589 1.002580 -0.307400
+v -0.157233 0.947279 -0.257910
+v -0.084454 1.573380 -0.020208
+v -0.107201 1.566870 -0.095908
+v -0.130608 1.542990 -0.169057
+v -0.153288 1.502830 -0.235229
+v -0.174189 1.448270 -0.291348
+v -0.192332 1.381860 -0.334774
+v -0.206856 1.306700 -0.363481
+v -0.217103 1.226310 -0.376123
+v -0.222575 1.144450 -0.372119
+v -0.223019 1.064950 -0.351648
+v -0.218259 0.992209 -0.314888
+v -0.208250 0.933985 -0.265962
+v -0.130133 1.598420 -0.006284
+v -0.154163 1.591540 -0.086247
+v -0.178875 1.566330 -0.163489
+v -0.202830 1.523920 -0.233369
+v -0.224903 1.466310 -0.292623
+v -0.244054 1.396180 -0.338481
+v -0.259394 1.316820 -0.368790
+v -0.270211 1.231930 -0.382151
+v -0.275987 1.145490 -0.377917
+v -0.276461 1.061530 -0.356297
+v -0.271561 0.984202 -0.318069
+v -0.262026 0.923353 -0.267949
+v -0.178512 1.617810 0.008292
+v -0.203468 1.610670 -0.074755
+v -0.229136 1.584490 -0.154970
+v -0.254011 1.540450 -0.227534
+v -0.276929 1.480630 -0.289072
+v -0.296814 1.407800 -0.336686
+v -0.312747 1.325390 -0.368168
+v -0.323972 1.237240 -0.382032
+v -0.330378 1.147320 -0.377776
+v -0.331394 1.060040 -0.355511
+v -0.325863 0.979694 -0.315919
+v -0.319650 0.913737 -0.262299
+v -0.280317 1.627610 -0.054322
+v -0.306652 1.600740 -0.136627
+v -0.332180 1.555560 -0.211096
+v -0.355698 1.494160 -0.274236
+v -0.376109 1.419430 -0.323103
+v -0.392458 1.334870 -0.355408
+v -0.403779 1.244400 -0.368931
+v -0.410778 1.151820 -0.363645
+v -0.413685 1.062510 -0.343441
+v -0.405714 0.980124 -0.300964
+v -0.394830 0.908428 -0.245869
+v -0.254700 1.634940 0.030935
+v -0.332647 1.639700 0.053801
+v -0.358181 1.632410 -0.031167
+v -0.384420 1.605640 -0.113176
+v -0.409852 1.560620 -0.187370
+v -0.455605 1.501640 -0.241539
+v -0.469596 1.435120 -0.285313
+v -0.487731 1.361430 -0.309839
+v -0.481273 1.251120 -0.341698
+v -0.495294 1.162890 -0.342469
+v -0.499179 1.071130 -0.324260
+v -0.486893 0.988206 -0.278254
+v -0.472243 0.916280 -0.221780
+v -0.384532 1.634550 0.068822
+v -0.409503 1.627420 -0.014292
+v -0.435171 1.601230 -0.094507
+v -0.477967 1.551730 -0.173906
+v -0.531913 1.293920 -0.304605
+v -0.574434 1.201870 -0.303530
+v -0.577555 1.088810 -0.291971
+v -0.565010 1.003490 -0.248901
+v -0.544406 0.934897 -0.186510
+v -0.435401 1.623220 0.083413
+v -0.459453 1.616360 0.003354
+v -0.504154 1.581240 -0.099519
+v -0.484521 1.605890 0.097345
+v -0.507305 1.599390 0.021512
+v -0.543182 1.576140 -0.062314
+v -0.638033 1.195650 -0.253861
+v -0.639101 1.100600 -0.247226
+v -0.629010 1.022190 -0.209027
+v -0.607679 0.960876 -0.147059
+v -0.531171 1.582800 0.110416
+v -0.552347 1.576750 0.039921
+v -0.581900 1.558720 -0.028075
+v -0.672969 1.195730 -0.210355
+v -0.677537 1.112500 -0.200049
+v -0.673733 1.039200 -0.171459
+v -0.650430 0.981407 -0.115222
+v -0.574671 1.554290 0.122450
+v -0.593940 1.548790 0.058316
+v -0.621915 1.529820 -0.001035
+v -0.696977 1.198480 -0.165973
+v -0.708076 1.122690 -0.156490
+v -0.704413 1.060390 -0.128968
+v -0.685447 1.007250 -0.081991
+v -0.614382 1.520770 0.133260
+v -0.631457 1.515900 0.076422
+v -0.657800 1.494270 0.020437
+v -0.729933 1.135600 -0.101899
+v -0.726093 1.091230 -0.072301
+v -0.714208 1.026920 0.059369
+v -0.661158 1.463140 0.145560
+v -0.676306 1.456560 0.088737
+v -0.689370 1.446410 0.047313
+v -0.711249 1.402380 0.038720
+v -0.729303 1.354320 0.032537
+v -0.739550 1.298720 -0.009562
+v -0.738222 1.240510 -0.068876
+v -0.699913 1.401690 0.155436
+v -0.718760 1.387490 0.096907
+v 0.113759 1.176340 -0.087782
+v -0.633644 1.322440 -0.197795
+v -0.587861 1.447770 -0.194836
+v -0.670122 1.297640 -0.168798
+v -0.706155 1.388550 -0.058347
+v -0.634756 1.483830 -0.094952
+v -0.628195 1.331840 -0.174640
+v -0.653848 1.313230 -0.150306
+v -0.588061 1.417700 -0.168434
+v -0.601659 1.370600 -0.184575
+v -0.673103 1.313110 -0.121873
+v -0.598997 1.446270 -0.132097
+v -0.686945 1.336970 -0.096301
+v -0.626779 1.445730 -0.100134
+v -0.685944 1.377830 -0.079419
+v -0.661373 1.419720 -0.080323
+v -0.635846 1.334490 -0.177798
+v -0.661373 1.315230 -0.154970
+v -0.596002 1.423930 -0.173936
+v -0.680977 1.314720 -0.124801
+v -0.695345 1.338850 -0.096486
+v -0.633399 1.452410 -0.101906
+v -0.694233 1.382730 -0.077617
+v -0.668358 1.426310 -0.079953
+v -0.703049 1.281940 -0.127300
+v -0.725233 1.329830 -0.067081
+v -0.708876 1.397010 -0.038848
+v -0.573232 1.381680 -0.244749
+v -0.623546 1.311570 -0.227541
+v -0.673926 1.462790 -0.044134
+v -0.626852 1.515750 -0.081636
+v -0.585355 1.521520 -0.147897
+v -0.560087 1.473270 -0.216990
+v -0.674163 1.282030 -0.185732
+v -0.617689 1.387380 -0.169517
+v -0.601192 1.420660 -0.159826
+v -0.641066 1.356200 -0.164201
+v -0.659460 1.333900 -0.147222
+v -0.672903 1.329390 -0.128657
+v -0.682793 1.350090 -0.114955
+v -0.677633 1.383550 -0.107993
+v -0.656517 1.416850 -0.109157
+v -0.628610 1.439670 -0.117343
+v -0.606115 1.441970 -0.136175
+v -0.609733 1.373500 -0.188824
+v -0.608999 1.453300 -0.136701
+v -0.654233 1.320970 -0.149091
+v -0.641940 1.320520 -0.163667
+v -0.630381 1.339840 -0.171452
+v -0.605626 1.375760 -0.180038
+v -0.592072 1.394850 -0.180623
+v -0.591790 1.418150 -0.165439
+v -0.614122 1.348870 -0.181943
+v -0.672154 1.320190 -0.124453
+v -0.664087 1.310470 -0.136130
+v -0.590411 1.435720 -0.151678
+v -0.601096 1.444640 -0.133409
+v -0.685151 1.343060 -0.103248
+v -0.680821 1.321890 -0.108290
+v -0.611490 1.449740 -0.114258
+v -0.627831 1.443630 -0.105976
+v -0.682882 1.380590 -0.089658
+v -0.689548 1.356240 -0.086166
+v -0.643898 1.435340 -0.088650
+v -0.660150 1.418920 -0.090614
+v -0.675801 1.399480 -0.077291
+v -0.625229 1.433430 -0.132497
+v -0.610674 1.439460 -0.138533
+v -0.648184 1.411260 -0.131519
+v -0.668854 1.381870 -0.129695
+v -0.673889 1.336510 -0.131986
+v -0.667994 1.346350 -0.141328
+v -0.655909 1.372420 -0.149847
+v -0.634504 1.401090 -0.152998
+v -0.614471 1.425870 -0.149150
+v -0.695049 1.297050 -0.125735
+v -0.721177 1.363200 -0.047730
+v -0.715801 1.333930 -0.082043
+v -0.592932 1.342010 -0.238217
+v -0.599546 1.375790 -0.212023
+v -0.650801 1.493590 -0.059719
+v -0.673192 1.443950 -0.062766
+v -0.569459 1.504090 -0.185398
+v -0.605959 1.487290 -0.145628
+v -0.561303 1.429470 -0.237928
+v -0.652135 1.292830 -0.209902
+v -0.689562 1.277830 -0.157988
+v -0.692988 1.429880 -0.036957
+v -0.605470 1.526100 -0.111863
+v -0.717477 1.300250 -0.095671
+v -0.729533 1.316260 -0.043348
+v -0.713592 1.253400 -0.126099
+v -0.708958 1.402880 -0.008005
+v -0.604142 1.284850 -0.253476
+v -0.533566 1.373680 -0.280990
+v -0.601785 1.544540 -0.056561
+v -0.668521 1.481520 -0.015463
+v -0.504599 1.493210 -0.234570
+v -0.544777 1.551900 -0.140957
+v -0.668083 1.256100 -0.196379
+v -0.607657 1.404450 -0.166648
+v -0.629418 1.371000 -0.168738
+v -0.651157 1.343610 -0.156497
+v -0.666044 1.328320 -0.138036
+v -0.679094 1.337280 -0.120538
+v -0.682460 1.366220 -0.110736
+v -0.668721 1.400780 -0.107355
+v -0.642593 1.430150 -0.112634
+v -0.616205 1.444010 -0.124393
+v -0.600361 1.433450 -0.149402
+v -0.622167 1.351300 -0.185205
+v -0.600235 1.442530 -0.157061
+v -0.599835 1.399270 -0.185190
+v -0.649258 1.322800 -0.167923
+v -0.671776 1.312360 -0.140179
+v -0.689044 1.323350 -0.109928
+v -0.697696 1.359520 -0.085068
+v -0.683794 1.405730 -0.075586
+v -0.649874 1.442120 -0.089287
+v -0.619957 1.456110 -0.117513
+v -0.643364 1.328660 -0.161250
+v -0.596128 1.397500 -0.175893
+v -0.617458 1.355830 -0.178214
+v -0.663694 1.317910 -0.136738
+v -0.593318 1.434570 -0.150788
+v -0.679657 1.328940 -0.113154
+v -0.613218 1.447720 -0.117684
+v -0.686738 1.360940 -0.095144
+v -0.644194 1.433740 -0.096968
+v -0.673615 1.400390 -0.087849
+v -0.617844 1.440050 -0.131407
+v -0.636091 1.423650 -0.132305
+v -0.659438 1.396930 -0.130347
+v -0.674912 1.366730 -0.129257
+v -0.669492 1.337140 -0.138169
+v -0.663464 1.358610 -0.145969
+v -0.645685 1.386720 -0.152167
+v -0.623546 1.414470 -0.151878
+v -0.608028 1.433410 -0.146881
+v -0.707571 1.310990 -0.103129
+v -0.714393 1.360840 -0.066659
+v -0.615272 1.345310 -0.206551
+v -0.652209 1.467150 -0.076386
+v -0.595052 1.473310 -0.173142
+v -0.588328 1.412040 -0.208056
+v -0.653699 1.307060 -0.186303
+v -0.683045 1.293870 -0.147556
+v -0.691838 1.417040 -0.056301
+v -0.619527 1.490980 -0.118158
+v -0.726026 1.275660 -0.084757
+v -0.723965 1.360970 -0.017050
+v -0.565247 1.326140 -0.276949
+v -0.636929 1.516850 -0.030878
+v -0.520220 1.530090 -0.189305
+v -0.511249 1.442260 -0.268356
+v -0.639865 1.265080 -0.225154
+v -0.692202 1.252380 -0.165350
+v -0.690541 1.443170 -0.006929
+v -0.572187 1.556610 -0.095612
+v -0.677410 1.352820 -0.128723
+v -0.677907 1.342830 -0.127419
+v -0.716609 1.361110 0.159358
+v -0.723201 1.059690 0.152641
+v -0.740417 1.171570 0.161597
+v -0.752065 1.065250 0.162769
+v -0.798634 1.073080 0.176552
+v -0.913867 1.094570 0.211013
+v -0.883001 1.061640 0.200804
+v -0.792991 1.142950 0.176596
+v -0.835950 1.070640 0.187273
+v -0.841132 1.134610 0.189875
+v -0.853967 1.064880 0.192404
+v -0.743568 1.358830 0.167173
+v -0.789611 1.343050 0.180244
+v -0.829529 1.331260 0.191618
+v -0.849244 1.342480 0.197638
+v -0.869900 1.366830 0.204237
+v -0.904947 1.394330 0.215114
+v -0.937133 1.394470 0.224507
+v -0.949967 1.339680 0.226969
+v -0.954994 1.380230 0.229393
+v -0.920955 1.305360 0.217701
+v -0.883638 1.268320 0.205942
+v -0.856599 1.249600 0.197608
+v -0.814537 1.230620 0.184885
+v -0.748654 1.158130 0.163999
+v -0.750456 1.191620 0.164370
+v -0.773477 1.069010 0.169108
+v -0.904702 1.070670 0.207351
+v -0.768324 1.149440 0.169597
+v -0.818660 1.069770 0.182320
+v -0.818267 1.137090 0.183173
+v -0.875994 1.131720 0.200641
+v -0.728517 1.364630 0.164526
+v -0.766211 1.351680 0.173616
+v -0.810749 1.335600 0.186235
+v -0.841259 1.334780 0.195125
+v -0.857941 1.352220 0.200404
+v -0.955972 1.360050 0.229201
+v -0.937963 1.322090 0.223054
+v -0.901255 1.286040 0.211495
+v -0.870197 1.256940 0.201745
+v -0.837714 1.241690 0.191907
+v -0.778207 1.215290 0.173927
+v -0.909181 1.111960 0.210087
+v -0.725032 1.082230 0.020948
+v -0.752258 1.283800 0.027554
+v -0.730089 1.347230 0.116266
+v -0.749870 1.208080 -0.062003
+v -0.759205 1.175270 0.094261
+v -0.749633 1.165530 0.097389
+v -0.781707 1.271100 0.044585
+v -0.764373 1.098760 -0.033851
+v -0.758359 1.081570 0.044555
+v -0.817666 1.276210 0.059139
+v -0.766804 1.140450 -0.071975
+v -0.766345 1.202050 -0.060772
+v -0.823917 1.199360 -0.032813
+v -0.807256 1.087850 0.071291
+v -0.835075 1.176850 -0.024687
+v -0.829974 1.152770 -0.017673
+v -0.924261 1.110420 0.148022
+v -0.903783 1.106340 0.091577
+v -0.954037 1.294020 0.075814
+v -0.960984 1.367890 0.174832
+v -0.823405 1.195790 0.092659
+v -0.808413 1.153690 0.096537
+v -0.852625 1.245870 0.018108
+v -0.848272 1.292100 0.074805
+v -0.857451 1.178060 0.035258
+v -0.868232 1.214140 0.095276
+v -0.859097 1.188860 -0.003252
+v -0.871990 1.317760 0.091606
+v -0.887516 1.344160 0.111099
+v -0.901003 1.213410 0.057886
+v -0.942515 1.269910 0.096262
+v -0.899564 1.236240 0.102795
+v -0.927753 1.268380 0.118772
+v -0.908128 1.225360 0.029371
+v -0.848317 1.088560 0.096381
+v -0.860135 1.147460 0.027673
+v -0.865792 1.081620 0.110609
+v -0.907371 1.250970 0.025367
+v -0.730430 1.068750 0.103143
+v -0.750849 1.163660 0.090413
+v -0.759716 1.160370 0.099236
+v -0.746763 1.172260 0.133023
+v -0.766107 1.274950 0.038134
+v -0.758715 1.073380 0.109986
+v -0.742945 1.317870 0.068385
+v -0.776673 1.309200 0.079721
+v -0.799509 1.273830 0.051265
+v -0.764239 1.173640 -0.074970
+v -0.796098 1.203600 -0.048864
+v -0.795468 1.143050 -0.059066
+v -0.783479 1.084710 0.058005
+v -0.801169 1.081170 0.136233
+v -0.813351 1.306110 0.090620
+v -0.834830 1.161490 -0.026288
+v -0.830011 1.140260 -0.035037
+v -0.917610 1.086720 0.141260
+v -0.921266 1.098890 0.180081
+v -0.938845 1.316450 0.074546
+v -0.961799 1.340370 0.114887
+v -0.819698 1.177060 0.065760
+v -0.789833 1.186590 0.093363
+v -0.782737 1.157730 0.097916
+v -0.816235 1.160260 0.053727
+v -0.798693 1.144930 0.148934
+v -0.835372 1.280100 0.067851
+v -0.835372 1.168960 0.030972
+v -0.862686 1.193130 0.066583
+v -0.847701 1.204770 0.093734
+v -0.857155 1.178140 0.010983
+v -0.828336 1.160650 0.002435
+v -0.840376 1.183100 -0.012490
+v -0.824806 1.220500 0.115955
+v -0.871182 1.244050 0.120018
+v -0.880999 1.332290 0.102009
+v -0.922897 1.238180 0.072025
+v -0.934241 1.268200 0.108934
+v -0.915661 1.253000 0.110513
+v -0.899394 1.222500 0.079002
+v -0.904806 1.214590 0.040559
+v -0.934426 1.253580 0.045556
+v -0.950537 1.278610 0.084207
+v -0.866067 1.335980 0.115288
+v -0.885707 1.358520 0.129256
+v -0.898207 1.264650 0.126372
+v -0.929117 1.288170 0.139117
+v -0.950404 1.303440 0.132941
+v -0.829299 1.090110 0.083621
+v -0.856702 1.133760 0.020926
+v -0.839642 1.149270 0.007343
+v -0.832265 1.151220 0.103417
+v -0.857510 1.151290 0.073130
+v -0.841511 1.079940 0.146242
+v -0.846693 1.138300 0.160203
+v -0.886597 1.139500 0.056433
+v -0.881600 1.123140 0.052066
+v -0.876284 1.146160 0.084474
+v -0.911709 1.122570 0.101186
+v -0.892610 1.078960 0.127825
+v -0.844929 1.314250 0.102884
+v -0.884714 1.224120 0.097908
+v -0.884424 1.201480 0.010650
+v -0.859995 1.213230 -0.005602
+v -0.861099 1.304340 0.082605
+v -0.878345 1.188720 0.044251
+v -0.911968 1.334190 0.090101
+v -0.888191 1.282750 0.045667
+v -0.815976 1.228730 -0.008286
+v -0.769644 1.222050 -0.029432
+v -0.814612 1.109020 0.002969
+v -0.857392 1.107230 0.036363
+v -0.759894 1.341760 0.124407
+v -0.801251 1.328420 0.135803
+v -0.839442 1.324850 0.146398
+v -0.859460 1.340220 0.153694
+v -0.880213 1.363270 0.164059
+v -0.913103 1.385820 0.169330
+v -0.943976 1.383990 0.171755
+v -0.953962 1.327640 0.178798
+v -0.928436 1.300180 0.175358
+v -0.892928 1.269740 0.163569
+v -0.864413 1.249100 0.156051
+v -0.824754 1.230100 0.145108
+v -0.918960 1.124860 0.149156
+v -0.828751 1.156580 0.005445
+v -0.862805 1.151880 0.045000
+v -0.761644 1.165720 0.088314
+v -0.754185 1.158820 0.138516
+v -0.760488 1.194380 0.126624
+v -0.756543 1.314570 0.074183
+v -0.794986 1.176560 -0.067296
+v -0.790322 1.100080 -0.016598
+v -0.780128 1.076350 0.120211
+v -0.795268 1.307580 0.084778
+v -0.821877 1.171110 -0.046662
+v -0.913177 1.072670 0.176789
+v -0.943523 1.358040 0.113701
+v -0.791990 1.163970 0.077482
+v -0.774160 1.150640 0.143959
+v -0.841318 1.185710 0.063610
+v -0.833881 1.170340 0.003228
+v -0.850712 1.232250 0.118706
+v -0.918070 1.243320 0.090338
+v -0.929703 1.240470 0.055906
+v -0.875542 1.347820 0.123095
+v -0.913940 1.278080 0.132948
+v -0.940891 1.294640 0.138324
+v -0.836906 1.110700 0.020748
+v -0.836929 1.139090 -0.000968
+v -0.836402 1.155860 0.062298
+v -0.822292 1.081450 0.140185
+v -0.822626 1.141080 0.153812
+v -0.859475 1.074390 0.153597
+v -0.882578 1.134560 0.171125
+v -0.882163 1.140840 0.112463
+v -0.889763 1.066660 0.167455
+v -0.831168 1.306140 0.098160
+v -0.885233 1.253320 0.122287
+v -0.885596 1.227180 0.009901
+v -0.881236 1.205680 0.071432
+v -0.915424 1.365450 0.122591
+v -0.901959 1.307080 0.064655
+v -0.831835 1.233750 0.005660
+v -0.795957 1.227830 -0.019964
+v -0.743672 1.172950 -0.078803
+v -0.875631 1.100720 0.063758
+v -0.741744 1.347690 0.119588
+v -0.778467 1.335640 0.129456
+v -0.820565 1.323880 0.141586
+v -0.851416 1.331140 0.149897
+v -0.868728 1.351080 0.158513
+v -0.960880 1.347210 0.176366
+v -0.943716 1.312540 0.179443
+v -0.909752 1.284780 0.168885
+v -0.878886 1.258170 0.159581
+v -0.846456 1.240170 0.151551
+v -0.787980 1.215280 0.136567
+v -0.916357 1.115800 0.179377
+v -0.842971 1.155190 0.025011
+v -0.837426 1.206250 -0.017754
+v -0.885833 1.144510 0.069719
+v -0.880828 1.189760 0.022616
+v -0.924098 1.280850 0.044659
+v -0.909233 1.131980 0.108133
+v -0.958359 1.319120 0.124400
+v -0.888584 1.091040 0.092718
+v -0.756269 1.218570 -0.036572
+v -0.785266 1.204520 0.113783
+v -0.857325 1.324790 0.107807
+v -0.872109 1.262270 0.030335
+v -0.823865 1.157920 0.033997
+v -0.855323 1.146830 0.107629
+v -0.885351 1.139060 0.143610
+v -0.851809 1.141350 0.135766
+v -0.827372 1.146180 0.130324
+v -0.803171 1.148150 0.124815
+v -0.778096 1.152650 0.120804
+v -0.757054 1.158950 0.117089
+v -0.747512 1.169600 0.112522
+v -0.757440 1.187320 0.108711
+v -0.698430 1.418360 0.093082
+v -0.679679 1.431890 0.150231
+v -0.728213 1.199410 -0.109157
+v -0.716506 1.049860 -0.006840
+v -0.694560 1.007270 0.144700
+v -0.739950 1.142900 -0.069995
+v -0.731142 1.108500 -0.035630
+v -0.729748 1.362880 0.106843
+v -0.713295 1.377320 0.158765
+v -0.713963 1.041770 0.151099
+v -0.741581 1.331030 0.052763
+v -0.752109 1.288250 0.014001
+v -0.750730 1.221230 -0.062114
+v -0.742901 1.186160 -0.088383
+v -0.740083 1.141330 -0.079930
+v -0.734597 1.103470 -0.049183
+v -0.728198 1.069300 0.009679
+v -0.824554 1.132900 -0.021817
+v -0.795720 1.115210 -0.032642
+v -0.801607 1.123040 -0.041465
+v 0.101429 1.132540 -0.107089
+v -0.727842 1.053820 0.072581
+v 0.058308 1.049900 -0.140223
+v 0.028562 1.011430 -0.153198
+v -0.006908 0.979167 -0.163207
+v -0.046730 0.948688 -0.169057
+v -0.663361 0.952358 -0.001532
+v 0.118920 1.226450 -0.076727
+v 0.121529 1.222740 -0.066800
+v 0.123643 1.216850 -0.057806
+v 0.125340 1.209090 -0.050184
+v 0.126260 1.199780 -0.044290
+v 0.126460 1.189380 -0.040405
+v 0.125956 1.178400 -0.038714
+v 0.124777 1.167340 -0.039285
+v 0.122953 1.156740 -0.042080
+v 0.120595 1.147070 -0.047003
+v 0.117659 1.138620 -0.054336
+v 0.116777 1.276720 -0.063856
+v 0.121989 1.269340 -0.044090
+v 0.126304 1.257650 -0.026199
+v 0.129514 1.242190 -0.011030
+v 0.131465 1.223690 0.000715
+v 0.132072 1.203010 0.008485
+v 0.131242 1.181130 0.011881
+v 0.128996 1.159100 0.010739
+v 0.125437 1.137980 0.005119
+v 0.120803 1.118760 -0.004683
+v 0.115131 1.102010 -0.019067
+v 0.107279 1.086840 -0.043556
+v 0.108109 1.325960 -0.049450
+v 0.115843 1.315020 -0.020157
+v 0.122226 1.297690 0.006350
+v 0.126979 1.274790 0.028830
+v 0.129870 1.247380 0.046231
+v 0.130768 1.216740 0.057745
+v 0.129633 1.184310 0.062824
+v 0.126504 1.151600 0.061237
+v 0.121492 1.120180 0.053008
+v 0.114627 1.091680 0.038394
+v 0.106360 1.066940 0.017434
+v 0.093274 1.373520 -0.033762
+v 0.103401 1.359190 0.004629
+v 0.111772 1.336480 0.039373
+v 0.118000 1.306460 0.068830
+v 0.121796 1.270540 0.091636
+v 0.122968 1.230390 0.106717
+v 0.121477 1.187890 0.113375
+v 0.117385 1.145020 0.111299
+v 0.110883 1.103790 0.100578
+v 0.101881 1.066490 0.081286
+v 0.091138 1.034150 0.053949
+v 0.072476 1.418710 -0.017013
+v 0.084858 1.401190 0.029919
+v 0.095090 1.373440 0.072381
+v 0.102704 1.336740 0.108392
+v 0.107339 1.292830 0.136270
+v 0.108777 1.243750 0.154702
+v 0.106953 1.191790 0.162843
+v 0.101948 1.139400 0.160300
+v 0.093800 1.089160 0.146924
+v 0.082404 1.044100 0.122576
+v 0.068117 1.005500 0.086587
+v 0.046030 1.460870 0.000544
+v 0.060480 1.440420 0.055328
+v 0.072432 1.408020 0.104893
+v 0.081314 1.365190 0.146932
+v 0.086727 1.313930 0.179473
+v 0.088402 1.256640 0.200997
+v 0.086274 1.195990 0.210494
+v 0.080432 1.134820 0.207529
+v 0.070860 1.076250 0.191759
+v 0.057796 1.023530 0.163577
+v 0.041270 0.978226 0.122769
+v 0.014311 1.499390 0.018657
+v 0.030623 1.476310 0.080492
+v 0.044109 1.439740 0.136441
+v 0.054141 1.391390 0.183892
+v 0.060250 1.333530 0.220622
+v 0.062141 1.268860 0.244919
+v 0.059739 1.200400 0.255640
+v 0.053140 1.131360 0.252289
+v 0.042538 1.065090 0.234776
+v 0.028147 1.005140 0.203577
+v 0.008573 0.953878 0.159247
+v -0.022211 1.533710 0.037059
+v -0.004276 1.508320 0.105049
+v 0.010552 1.468120 0.166565
+v 0.021585 1.414960 0.218739
+v 0.028295 1.351340 0.259124
+v 0.030378 1.280240 0.285838
+v 0.027731 1.204970 0.297627
+v 0.020488 1.129060 0.293942
+v 0.008973 1.056060 0.274954
+v -0.006582 0.989955 0.241115
+v -0.026697 0.937269 0.197623
+v -0.063004 1.563320 0.055484
+v -0.043705 1.536020 0.128633
+v -0.027750 1.492750 0.194821
+v -0.015887 1.435560 0.250954
+v -0.008665 1.367110 0.294409
+v -0.006426 1.290610 0.323147
+v -0.009266 1.209630 0.335833
+v -0.017066 1.127960 0.331866
+v -0.029455 1.049410 0.311440
+v -0.046434 0.979398 0.274910
+v -0.066934 0.928958 0.234650
+v -0.107483 1.587800 0.073664
+v -0.087101 1.558960 0.150906
+v -0.070256 1.513280 0.220800
+v -0.057718 1.452890 0.280077
+v -0.050096 1.380610 0.325957
+v -0.047731 1.299830 0.356311
+v -0.050734 1.214310 0.369701
+v -0.058971 1.128070 0.365512
+v -0.072050 1.045130 0.343944
+v -0.090549 0.972925 0.305560
+v -0.112406 0.925807 0.269734
+v -0.154979 1.606780 0.091332
+v -0.133818 1.576840 0.171547
+v -0.116321 1.529400 0.244125
+v -0.103309 1.466690 0.305671
+v -0.095390 1.391630 0.353323
+v -0.092936 1.307750 0.384834
+v -0.096057 1.218950 0.398743
+v -0.104606 1.129390 0.394398
+v -0.118189 1.043260 0.372000
+v -0.140588 0.969514 0.333467
+v -0.166434 0.924725 0.299718
+v -0.230545 1.623620 0.116177
+v -0.208829 1.592890 0.198483
+v -0.190871 1.544210 0.272967
+v -0.177525 1.479860 0.336129
+v -0.169392 1.402840 0.385019
+v -0.166871 1.316750 0.417360
+v -0.170074 1.225630 0.431632
+v -0.178853 1.133730 0.427169
+v -0.192791 1.045350 0.404185
+v -0.212106 0.970107 0.362628
+v -0.234200 0.927149 0.324103
+v -0.308573 1.628430 0.138754
+v -0.286938 1.597810 0.220763
+v -0.269055 1.549310 0.294973
+v -0.279242 1.487490 0.362635
+v -0.268335 1.418960 0.404148
+v -0.271308 1.344030 0.431558
+v -0.248332 1.231900 0.453060
+v -0.257073 1.140330 0.448611
+v -0.270960 1.052270 0.425709
+v -0.289770 0.977766 0.383173
+v -0.310100 0.931798 0.337501
+v -0.360984 1.623520 0.151936
+v -0.339816 1.593580 0.232144
+v -0.333826 1.540150 0.319877
+v -0.312043 1.277970 0.451888
+v -0.412721 1.612610 0.163458
+v -0.395527 1.572520 0.272589
+v -0.341114 1.180810 0.467154
+v -0.352740 1.069580 0.447195
+v -0.368836 0.993018 0.394702
+v -0.389366 0.942630 0.337738
+v -0.463034 1.595830 0.173163
+v -0.448502 1.568540 0.262038
+v -0.425748 1.175210 0.466924
+v -0.429789 1.085910 0.452964
+v -0.445010 1.009510 0.406187
+v -0.472910 0.948665 0.334268
+v -0.511197 1.573450 0.180904
+v -0.499772 1.552130 0.253275
+v -0.482059 1.177080 0.457612
+v -0.487168 1.099610 0.440908
+v -0.503568 1.025090 0.400270
+v -0.534827 0.967468 0.329174
+v -0.556498 1.545780 0.186568
+v -0.548409 1.523920 0.250769
+v -0.527864 1.182090 0.432581
+v -0.536228 1.113510 0.412941
+v -0.552413 1.046560 0.380407
+v -0.587364 0.994700 0.318202
+v -0.598286 1.513230 0.190083
+v -0.590656 1.488880 0.250487
+v -0.586371 1.126470 0.372162
+v -0.600213 1.083580 0.343455
+v -0.667364 1.012150 0.232055
+v -0.643461 1.453930 0.201264
+v -0.632939 1.441710 0.242324
+v -0.646093 1.395630 0.258976
+v -0.660209 1.342400 0.272240
+v -0.648265 1.281370 0.310498
+v -0.614092 1.213440 0.360211
+v -0.684454 1.382920 0.215291
+v -0.454530 1.308090 0.415454
+v -0.416524 1.434010 0.392100
+v -0.501010 1.284330 0.409145
+v -0.590129 1.379570 0.337931
+v -0.509314 1.473760 0.334765
+v -0.462433 1.318540 0.392708
+v -0.497303 1.300610 0.384737
+v -0.431271 1.405110 0.368678
+v -0.434601 1.357180 0.387740
+v -0.529147 1.301380 0.370872
+v -0.459668 1.435090 0.345197
+v -0.554430 1.326360 0.357653
+v -0.500284 1.435570 0.333201
+v -0.562208 1.367840 0.344582
+v -0.540432 1.410010 0.334001
+v -0.467260 1.320910 0.399847
+v -0.501181 1.302450 0.393182
+v -0.434927 1.410990 0.377857
+v -0.534219 1.302910 0.377879
+v -0.561362 1.328070 0.362509
+v -0.504828 1.442080 0.338546
+v -0.570422 1.372900 0.348170
+v -0.546430 1.416530 0.337723
+v -0.550842 1.269610 0.390439
+v -0.600643 1.318070 0.352485
+v -0.603282 1.388730 0.324200
+v -0.378104 1.365990 0.423736
+v -0.429500 1.296200 0.436504
+v -0.570126 1.454490 0.312322
+v -0.509410 1.506320 0.320693
+v -0.438723 1.509750 0.354420
+v -0.380876 1.458890 0.396926
+v -0.495108 1.268020 0.426850
+v -0.456050 1.374390 0.384211
+v -0.446923 1.408280 0.368618
+v -0.478930 1.343160 0.391277
+v -0.503776 1.321340 0.386198
+v -0.525225 1.317500 0.377278
+v -0.540743 1.338690 0.371666
+v -0.539713 1.372480 0.364482
+v -0.520865 1.405960 0.355540
+v -0.492647 1.428750 0.348407
+v -0.463531 1.430520 0.352278
+v -0.439086 1.359780 0.395785
+v -0.465533 1.441790 0.354761
+v -0.498356 1.308460 0.384441
+v -0.479983 1.307600 0.390142
+v -0.466052 1.326560 0.391566
+v -0.440339 1.362490 0.386168
+v -0.428387 1.381700 0.380111
+v -0.436017 1.405640 0.368188
+v -0.446634 1.335350 0.391929
+v -0.526931 1.308510 0.373023
+v -0.513763 1.298450 0.378198
+v -0.442029 1.423820 0.356615
+v -0.460758 1.433370 0.347362
+v -0.549106 1.332140 0.362776
+v -0.542990 1.310780 0.363859
+v -0.479753 1.439170 0.337041
+v -0.498059 1.433210 0.338591
+v -0.554037 1.370250 0.351736
+v -0.561837 1.345990 0.351328
+v -0.521021 1.425470 0.332281
+v -0.533870 1.408780 0.341972
+v -0.554482 1.389730 0.338331
+v -0.481733 1.421900 0.359091
+v -0.466141 1.427850 0.356607
+v -0.501892 1.399520 0.369649
+v -0.520658 1.369970 0.377961
+v -0.524194 1.324470 0.380845
+v -0.514089 1.333980 0.385931
+v -0.499030 1.359830 0.387555
+v -0.478930 1.388610 0.379940
+v -0.463798 1.413770 0.366995
+v -0.545637 1.284860 0.385583
+v -0.608235 1.353050 0.335811
+v -0.585718 1.323820 0.360552
+v -0.398159 1.326270 0.428845
+v -0.417955 1.361190 0.410087
+v -0.541663 1.484830 0.314153
+v -0.559539 1.434770 0.326654
+v -0.405373 1.490920 0.376693
+v -0.457748 1.475390 0.362109
+v -0.371201 1.414210 0.413319
+v -0.463145 1.278040 0.436823
+v -0.523171 1.264770 0.409857
+v -0.591168 1.421330 0.315495
+v -0.474993 1.515620 0.335084
+v -0.582159 1.289070 0.373334
+v -0.619319 1.302800 0.335996
+v -0.559843 1.242140 0.398068
+v -0.619357 1.395120 0.298517
+v -0.400547 1.271590 0.448752
+v -0.325262 1.356930 0.432641
+v -0.501396 1.536480 0.287343
+v -0.580699 1.474570 0.286001
+v -0.324395 1.478740 0.382750
+v -0.407865 1.540900 0.328062
+v -0.485485 1.238910 0.442502
+v -0.448917 1.391710 0.377145
+v -0.466541 1.357910 0.389245
+v -0.491683 1.330760 0.389875
+v -0.514356 1.316090 0.381690
+v -0.534767 1.325690 0.373890
+v -0.542530 1.354980 0.368633
+v -0.532321 1.389830 0.359892
+v -0.507090 1.419260 0.351558
+v -0.478345 1.432940 0.347866
+v -0.451675 1.421520 0.359951
+v -0.451713 1.337540 0.399113
+v -0.447331 1.430250 0.366728
+v -0.432421 1.385830 0.388326
+v -0.483979 1.309670 0.398120
+v -0.518085 1.300090 0.386079
+v -0.548980 1.312140 0.369842
+v -0.569414 1.349260 0.355317
+v -0.562052 1.395950 0.341460
+v -0.525633 1.432140 0.336329
+v -0.485055 1.445280 0.344611
+v -0.482504 1.315670 0.389341
+v -0.434311 1.384510 0.378428
+v -0.451446 1.342460 0.390580
+v -0.513088 1.305730 0.378872
+v -0.444980 1.422670 0.357379
+v -0.539312 1.317670 0.367632
+v -0.479390 1.436980 0.340771
+v -0.554586 1.350330 0.357586
+v -0.516817 1.423510 0.339384
+v -0.546942 1.390210 0.346087
+v -0.476009 1.428670 0.354487
+v -0.491112 1.412010 0.364341
+v -0.512183 1.385110 0.374098
+v -0.526196 1.354790 0.380192
+v -0.517129 1.324880 0.383810
+v -0.507646 1.346090 0.387822
+v -0.488984 1.374140 0.384633
+v -0.470130 1.402150 0.373683
+v -0.459490 1.421480 0.361946
+v -0.568747 1.300030 0.373653
+v -0.592710 1.351220 0.348148
+v -0.434222 1.330630 0.413875
+v -0.534219 1.457680 0.327803
+v -0.433933 1.460370 0.378806
+v -0.410259 1.397750 0.401938
+v -0.477811 1.293120 0.415937
+v -0.523542 1.281200 0.397631
+v -0.579468 1.407520 0.330391
+v -0.483905 1.480090 0.346435
+v -0.594660 1.261010 0.368574
+v -0.627690 1.349180 0.311499
+v -0.353370 1.309170 0.447544
+v -0.545184 1.509490 0.283414
+v -0.361436 1.517340 0.354628
+v -0.312473 1.426300 0.412578
+v -0.444825 1.247500 0.450680
+v -0.522230 1.237670 0.420800
+v -0.603461 1.437350 0.290309
+v -0.455294 1.547230 0.304826
+v -0.528762 1.340880 0.380496
+v -0.529993 1.330950 0.379288
+v -0.654323 1.075280 0.269171
+v -0.679487 1.268330 0.285074
+v -0.705081 1.343190 0.202732
+v -0.626749 1.196160 0.357438
+v -0.726589 1.172780 0.230980
+v -0.722444 1.164420 0.223254
+v -0.713889 1.257950 0.283703
+v -0.655901 1.090290 0.335959
+v -0.693603 1.076370 0.266398
+v -0.750515 1.267570 0.292103
+v -0.635342 1.131200 0.369642
+v -0.642148 1.192060 0.364696
+v -0.705985 1.189580 0.371681
+v -0.749121 1.083190 0.270453
+v -0.719998 1.167420 0.369872
+v -0.719612 1.143910 0.360381
+v -0.888094 1.105780 0.269527
+v -0.841169 1.101310 0.306072
+v -0.872695 1.287490 0.354465
+v -0.930867 1.365470 0.278016
+v -0.775760 1.189130 0.266205
+v -0.762215 1.149810 0.253972
+v -0.756780 1.238170 0.346450
+v -0.783012 1.286860 0.298368
+v -0.770845 1.170970 0.331740
+v -0.812906 1.208220 0.288656
+v -0.751464 1.180210 0.365460
+v -0.811720 1.312930 0.298079
+v -0.834949 1.339940 0.291154
+v -0.819364 1.206850 0.337567
+v -0.874282 1.264440 0.330020
+v -0.842007 1.231620 0.299962
+v -0.873956 1.264050 0.303062
+v -0.809881 1.217470 0.365927
+v -0.797217 1.084460 0.271440
+v -0.768458 1.137370 0.337746
+v -0.819683 1.077920 0.268548
+v -0.806767 1.242890 0.369998
+v -0.701714 1.066450 0.201493
+v -0.720220 1.162410 0.229037
+v -0.728888 1.158840 0.226383
+v -0.733892 1.171410 0.190068
+v -0.697533 1.258950 0.281175
+v -0.729185 1.071010 0.211147
+v -0.691965 1.304840 0.248144
+v -0.726997 1.298970 0.254795
+v -0.731742 1.262940 0.288100
+v -0.631531 1.163580 0.373542
+v -0.673688 1.193590 0.370517
+v -0.667935 1.132860 0.375877
+v -0.721970 1.079770 0.268719
+v -0.778994 1.079390 0.212215
+v -0.763201 1.298800 0.264826
+v -0.719026 1.152250 0.370598
+v -0.710463 1.130660 0.374483
+v -0.879791 1.083690 0.270824
+v -0.903568 1.097870 0.241197
+v -0.858919 1.310030 0.348340
+v -0.899668 1.335380 0.327744
+v -0.753807 1.172610 0.286335
+v -0.742589 1.182730 0.248974
+v -0.745081 1.155550 0.239173
+v -0.747186 1.152010 0.292771
+v -0.782730 1.144140 0.202702
+v -0.769377 1.273370 0.295447
+v -0.749877 1.161900 0.323036
+v -0.792280 1.186100 0.308741
+v -0.797455 1.195900 0.277520
+v -0.757618 1.170150 0.351966
+v -0.728947 1.152680 0.342906
+v -0.730867 1.174450 0.362917
+v -0.788617 1.221500 0.251110
+v -0.827305 1.240790 0.270743
+v -0.824725 1.327770 0.294795
+v -0.845114 1.231930 0.338509
+v -0.874141 1.263380 0.314821
+v -0.859520 1.248490 0.302847
+v -0.829247 1.216870 0.319314
+v -0.813232 1.207240 0.354257
+v -0.840399 1.246030 0.367655
+v -0.874445 1.272500 0.344856
+v -0.819223 1.332220 0.275740
+v -0.843016 1.355090 0.275510
+v -0.853181 1.261040 0.280604
+v -0.885803 1.284690 0.287521
+v -0.900231 1.299410 0.304819
+v -0.774300 1.085700 0.272025
+v -0.763149 1.126250 0.341430
+v -0.741210 1.141130 0.344307
+v -0.785236 1.147520 0.260741
+v -0.792235 1.148610 0.297308
+v -0.818400 1.078090 0.225419
+v -0.828432 1.134770 0.217471
+v -0.806945 1.131980 0.327484
+v -0.801006 1.116670 0.328137
+v -0.814167 1.144450 0.297768
+v -0.852891 1.116530 0.301957
+v -0.851586 1.075670 0.268363
+v -0.795016 1.310240 0.273879
+v -0.827149 1.219010 0.295647
+v -0.780128 1.193110 0.367914
+v -0.750649 1.204450 0.368974
+v -0.797863 1.299260 0.299221
+v -0.793229 1.181870 0.335803
+v -0.844402 1.328770 0.321553
+v -0.801110 1.275760 0.343959
+v -0.712569 1.219890 0.347844
+v -0.662834 1.209580 0.339154
+v -0.718315 1.101290 0.332852
+v -0.772358 1.100400 0.327655
+v -0.734782 1.338840 0.211310
+v -0.776228 1.324640 0.222276
+v -0.813655 1.322770 0.234746
+v -0.834260 1.338200 0.240033
+v -0.857036 1.361410 0.243458
+v -0.887308 1.383750 0.257679
+v -0.914667 1.381630 0.272159
+v -0.927583 1.325520 0.269171
+v -0.904562 1.298270 0.257152
+v -0.868669 1.267800 0.246669
+v -0.840844 1.247210 0.236771
+v -0.801888 1.228340 0.223988
+v -0.884224 1.119800 0.266420
+v -0.730971 1.148720 0.340422
+v -0.780365 1.142570 0.324652
+v -0.726559 1.163660 0.236348
+v -0.740565 1.157380 0.188756
+v -0.743546 1.193710 0.203332
+v -0.706786 1.302520 0.249797
+v -0.663234 1.165590 0.384240
+v -0.687427 1.091820 0.335877
+v -0.752695 1.074150 0.214172
+v -0.745162 1.299230 0.260103
+v -0.697229 1.160440 0.380771
+v -0.895331 1.071240 0.237920
+v -0.883401 1.353200 0.319677
+v -0.741388 1.159500 0.260645
+v -0.759612 1.149480 0.194391
+v -0.771416 1.181140 0.300044
+v -0.733922 1.162310 0.345642
+v -0.809955 1.231210 0.261556
+v -0.850815 1.237930 0.320715
+v -0.842148 1.233440 0.355836
+v -0.831264 1.344270 0.274776
+v -0.869811 1.274540 0.284118
+v -0.895220 1.290980 0.294795
+v -0.746652 1.103460 0.329938
+v -0.734634 1.130870 0.349460
+v -0.768228 1.148880 0.295521
+v -0.798915 1.079560 0.220252
+v -0.806122 1.140730 0.210487
+v -0.837573 1.072620 0.228645
+v -0.864999 1.131950 0.228837
+v -0.833651 1.136980 0.276845
+v -0.870649 1.065130 0.232930
+v -0.781729 1.300270 0.268719
+v -0.840191 1.249700 0.276585
+v -0.780395 1.218740 0.370279
+v -0.809955 1.199530 0.315310
+v -0.864391 1.361350 0.297404
+v -0.822626 1.300710 0.336418
+v -0.733188 1.226300 0.345204
+v -0.689993 1.215980 0.345597
+v -0.611201 1.162880 0.364563
+v -0.802549 1.094850 0.314116
+v -0.716743 1.343560 0.207640
+v -0.753384 1.332980 0.216337
+v -0.795527 1.320540 0.227740
+v -0.825547 1.329060 0.238513
+v -0.844528 1.349140 0.241434
+v -0.931861 1.344880 0.275777
+v -0.919487 1.310590 0.262461
+v -0.885515 1.282830 0.251888
+v -0.854827 1.256240 0.241983
+v -0.823397 1.238330 0.230520
+v -0.766708 1.214560 0.211288
+v -0.898690 1.114570 0.239848
+v -0.752791 1.144710 0.331310
+v -0.725418 1.197120 0.366595
+v -0.813907 1.138760 0.315584
+v -0.783679 1.181960 0.355399
+v -0.830864 1.273360 0.364037
+v -0.854308 1.125410 0.295180
+v -0.902145 1.314600 0.316971
+v -0.829166 1.086270 0.296277
+v -0.647835 1.205210 0.337531
+v -0.752428 1.208610 0.232937
+v -0.807976 1.320830 0.276852
+v -0.779572 1.254840 0.347339
+v -0.741796 1.150130 0.314161
+v -0.809021 1.146380 0.267177
+v -0.851483 1.133120 0.252630
+v -0.820676 1.140190 0.240982
+v -0.796802 1.143580 0.233746
+v -0.773173 1.146690 0.226413
+v -0.752124 1.151610 0.217041
+v -0.735880 1.158370 0.210027
+v -0.727523 1.168610 0.209560
+v -0.734137 1.185810 0.218902
+v -0.664762 1.416040 0.208641
+v -0.583546 1.187430 0.382809
+v -0.636573 1.031640 0.296181
+v -0.612017 1.134640 0.352759
+v -0.626030 1.100790 0.318602
+v -0.699438 1.358910 0.211621
+v -0.682393 1.316700 0.262038
+v -0.674378 1.271360 0.295202
+v -0.628091 1.206870 0.359907
+v -0.605767 1.176530 0.373935
+v -0.607761 1.132470 0.361998
+v -0.620921 1.095560 0.331525
+v -0.652417 1.059350 0.281842
+v -0.713073 1.123940 0.360092
+v -0.683164 1.106170 0.352945
+v -0.683445 1.113310 0.363747
+v 0.113552 1.131330 -0.066333
+v -0.684684 1.048050 0.225375
+v 0.094504 1.044010 -0.019541
+v 0.075605 1.004920 0.006275
+v 0.050686 0.972087 0.034694
+v 0.018849 0.939531 0.063855
+v -0.568080 0.933547 0.233279
+v -0.619223 0.966682 0.233508
+v -0.594081 1.405660 -0.196623
+v -0.604639 1.374640 -0.200420
+v -0.618763 1.348310 -0.195882
+v -0.634845 1.328570 -0.187674
+v -0.591931 1.435850 -0.184382
+v -0.651653 1.315030 -0.176961
+v -0.597648 1.457920 -0.165098
+v -0.665740 1.306540 -0.161776
+v -0.607479 1.470300 -0.141165
+v -0.677426 1.303220 -0.143856
+v -0.619742 1.473540 -0.117839
+v -0.687983 1.305810 -0.125261
+v -0.634081 1.468120 -0.098429
+v -0.698230 1.317360 -0.106896
+v -0.651046 1.454640 -0.082837
+v -0.706207 1.336360 -0.089984
+v -0.670790 1.435100 -0.071374
+v -0.706489 1.359760 -0.077083
+v -0.687916 1.411090 -0.066273
+v -0.700306 1.385290 -0.068787
+v -0.460899 1.314510 0.407647
+v -0.442964 1.334080 0.406498
+v -0.428521 1.360480 0.402939
+v -0.421344 1.391790 0.395132
+v -0.480895 1.301390 0.407032
+v -0.425725 1.422500 0.384974
+v -0.501107 1.293390 0.401160
+v -0.440636 1.445310 0.372763
+v -0.520814 1.290650 0.391855
+v -0.461640 1.458590 0.358431
+v -0.539920 1.293840 0.381720
+v -0.484484 1.462680 0.345523
+v -0.558478 1.306200 0.371918
+v -0.507075 1.457920 0.336656
+v -0.572832 1.326390 0.362405
+v -0.529926 1.444920 0.332066
+v -0.580113 1.350020 0.352811
+v -0.552984 1.425650 0.332185
+v -0.579772 1.375690 0.343714
+v -0.570771 1.401720 0.335929
+v -0.013818 0.689774 0.459569
+v -0.014789 0.650285 0.502728
+v -0.010964 0.639090 0.528700
+v -0.045989 0.099768 0.400908
+v -0.045662 0.114122 0.445415
+v -0.042793 0.140176 0.483510
+v -0.036239 0.175846 0.512515
+v -0.025518 0.216054 0.533934
+v -0.028180 0.259316 0.560077
+v -0.030196 0.283271 0.575284
+v -0.033214 0.318267 0.589081
+v -0.034549 0.356754 0.599047
+v -0.031420 0.396169 0.604303
+v -0.026356 0.436332 0.605215
+v -0.021514 0.476250 0.602798
+v -0.018074 0.514352 0.596459
+v -0.016428 0.549177 0.586553
+v -0.015449 0.581214 0.571999
+v -0.012461 0.612169 0.552299
+v -0.022026 0.728765 0.412252
+v -0.040999 0.758378 0.374454
+v -0.061870 0.785203 0.334350
+v -0.564936 0.874619 0.100800
+v -0.577629 0.856098 0.103506
+v -0.599983 0.840135 0.107406
+v -0.634852 0.820057 0.115406
+v -0.675801 0.791089 0.122650
+v 0.101652 1.082620 -0.062158
+v 0.110734 1.129650 -0.075763
+v 0.004814 0.918178 0.005030
+v 0.086200 1.037050 -0.047389
+v 0.064513 0.994990 -0.029506
+v 0.037318 0.956969 -0.009858
+v 0.601092 0.282359 -0.079278
+v 0.088499 1.083950 -0.104182
+v 0.104380 1.130060 -0.096753
+v -0.027386 0.925778 -0.109750
+v 0.067205 1.038920 -0.109765
+v 0.040121 0.996658 -0.113213
+v 0.008061 0.958341 -0.113643
+v -0.643527 0.948043 0.125052
+v -0.600369 0.914033 0.110824
+v 0.115390 0.101510 -0.199241
+v 0.054986 0.101436 -0.218177
+v 0.159268 0.101577 -0.163800
+v 0.193300 0.101651 -0.116357
+v 0.225381 0.101681 -0.044015
+v -0.327190 0.101303 -0.185309
+v -0.402660 0.101244 -0.136679
+v -0.142263 0.101377 -0.230462
+v -0.227994 0.101333 -0.217028
+v -0.027016 0.101370 -0.231159
+v -0.444157 0.101281 -0.075697
+v -0.068209 0.101370 -0.236438
+v -0.465807 0.101555 -0.008041
+v 0.227316 0.101651 0.087988
+v -0.465392 0.101481 0.083562
+v 0.065737 0.101422 0.300845
+v 0.125993 0.101510 0.282717
+v 0.184966 0.101644 0.256115
+v 0.216988 0.101659 0.205282
+v 0.216462 0.101607 0.142928
+v -0.406619 0.101362 0.219510
+v -0.329614 0.101496 0.271618
+v -0.229625 0.101599 0.318320
+v -0.138408 0.101637 0.336470
+v -0.021166 0.101488 0.324430
+v -0.447679 0.101347 0.155147
+v -0.060432 0.101488 0.325260
+v -0.125907 0.101733 0.038053
+v -0.591887 0.918044 -0.089524
+v 0.371383 0.889529 0.029771
+v 0.318022 0.100858 0.019710
+v 0.251531 0.101622 0.022068
+v 0.489670 0.943801 0.034331
+v 0.427435 0.927505 0.032448
+v 0.452458 0.145410 0.019814
+v 0.388006 0.116880 0.019443
+v 0.515643 0.182037 0.021067
+v 0.568395 0.229229 0.020429
+v 0.613273 0.281670 0.020007
+v 0.646830 0.341592 0.019280
+v 0.677244 0.426760 0.018895
+v 0.692985 0.520358 0.019836
+v 0.690782 0.590674 0.019858
+v 0.680128 0.665870 0.019636
+v 0.663149 0.754782 0.021186
+v 0.616254 0.866775 0.029586
+v 0.554464 0.924777 0.033871
+v 0.328632 0.835116 0.019525
+v 0.291664 0.803716 0.016796
+v 0.250219 0.786923 0.018220
+v 0.197310 0.779019 0.019443
+v 0.138211 0.783245 0.018998
+v -0.005996 0.810967 0.011139
+v -0.056880 0.830274 0.007276
+v -0.065095 0.844391 0.009671
+v -0.057251 0.862104 0.003317
+v -0.037381 0.885303 -0.011089
+v -0.004424 0.915093 -0.028201
+v 0.028213 0.951602 -0.040472
+v 0.056677 0.991208 -0.052742
+v 0.080350 1.034650 -0.063404
+v 0.097982 1.081280 -0.072242
+v 0.109333 1.129310 -0.080279
+v 0.267479 0.607542 -0.436698
+v 0.201774 0.638423 -0.425992
+v 0.084280 0.274619 -0.482347
+v -0.149477 0.257121 -0.483904
+v -0.030945 0.258960 -0.484757
+v -0.426645 0.365214 -0.480598
+v -0.477344 0.496380 -0.471827
+v -0.405551 0.622549 -0.452372
+v 0.204769 0.631994 0.503046
+v 0.276280 0.601306 0.511217
+v 0.083361 0.281588 0.557549
+v -0.416947 0.370493 0.556696
+v -0.461069 0.497648 0.549594
+v -0.396610 0.615616 0.529449
+v 0.282701 0.615994 -0.426785
+v 0.210960 0.645792 -0.417131
+v 0.106093 0.655757 -0.412676
+v -0.015805 0.668139 -0.412357
+v 0.206037 0.306107 -0.470640
+v 0.279498 0.364747 -0.466355
+v 0.320076 0.433195 -0.457562
+v 0.333289 0.501288 -0.448568
+v 0.098419 0.261273 -0.471530
+v 0.321367 0.564888 -0.438329
+v -0.401118 0.302660 -0.465139
+v -0.299349 0.259004 -0.465236
+v -0.164046 0.239305 -0.470581
+v -0.031234 0.240639 -0.472805
+v -0.468217 0.358170 -0.465762
+v -0.507757 0.422756 -0.463478
+v -0.520569 0.493051 -0.457651
+v -0.501277 0.563990 -0.447582
+v -0.450638 0.625551 -0.435645
+v -0.137088 0.687742 -0.411082
+v -0.264851 0.692584 -0.412994
+v -0.371171 0.669377 -0.423456
+v -0.015383 0.662289 0.487276
+v 0.111349 0.654890 0.486957
+v 0.217418 0.640098 0.492903
+v 0.292399 0.607943 0.502557
+v 0.337448 0.492984 0.523954
+v 0.322894 0.431438 0.531362
+v 0.278334 0.368602 0.538620
+v 0.203917 0.312343 0.542831
+v 0.100503 0.265855 0.545085
+v 0.328298 0.553737 0.514331
+v -0.027290 0.241781 0.548289
+v -0.158893 0.239164 0.547310
+v -0.295034 0.260458 0.540829
+v -0.396795 0.305618 0.538806
+v -0.462530 0.362100 0.537901
+v -0.500128 0.425536 0.535439
+v -0.511390 0.492547 0.529597
+v -0.493210 0.558341 0.520848
+v -0.446241 0.616202 0.510127
+v -0.371431 0.657944 0.498316
+v -0.267320 0.680261 0.487847
+v -0.136339 0.678667 0.486371
+vt 0.877975 0.397658
+vt 0.873951 0.397664
+vt 0.900103 0.397679
+vt 0.870831 0.397671
+vt 0.867889 0.397674
+vt 0.931273 0.397662
+vt 0.929288 0.397637
+vt 0.865490 0.397668
+vt 0.867711 0.397671
+vt 0.868707 0.397667
+vt 0.866606 0.398924
+vt 0.871384 0.397619
+vt 0.874160 0.398826
+vt 0.878048 0.397558
+vt 0.866060 0.397625
+vt 0.860570 0.399008
+vt 0.856321 0.399060
+vt 0.862375 0.397683
+vt 0.860024 0.397676
+vt 0.853776 0.399148
+vt 0.932361 0.401286
+vt 0.944998 0.401429
+vt 0.936097 0.404570
+vt 0.948825 0.405179
+vt 0.939922 0.398832
+vt 0.928618 0.398828
+vt 0.862287 0.401557
+vt 0.870712 0.401462
+vt 0.855482 0.401642
+vt 0.858360 0.405349
+vt 0.851030 0.405417
+vt 0.867250 0.404976
+vt 0.851008 0.401767
+vt 0.848050 0.401779
+vt 0.846214 0.405443
+vt 0.842460 0.405195
+vt 0.951482 0.401365
+vt 0.945825 0.398800
+vt 0.955814 0.405156
+vt 0.939709 0.408955
+vt 0.951716 0.409790
+vt 0.943792 0.414167
+vt 0.953940 0.414936
+vt 0.854643 0.410095
+vt 0.863781 0.409841
+vt 0.847079 0.410021
+vt 0.850892 0.415208
+vt 0.843668 0.414845
+vt 0.858319 0.415536
+vt 0.959154 0.409842
+vt 0.961426 0.415077
+vt 0.955396 0.420505
+vt 0.946551 0.419988
+vt 0.948394 0.426244
+vt 0.956245 0.426562
+vt 0.962774 0.420748
+vt 0.963626 0.426902
+vt 0.947821 0.439136
+vt 0.949156 0.432796
+vt 0.955188 0.438695
+vt 0.956416 0.432776
+vt 0.963377 0.433055
+vt 0.962074 0.438861
+vt 0.939469 0.450342
+vt 0.944673 0.445101
+vt 0.949616 0.449137
+vt 0.952905 0.444195
+vt 0.959891 0.444199
+vt 0.957210 0.449055
+vt 0.928445 0.457630
+vt 0.933041 0.454385
+vt 0.940767 0.456994
+vt 0.945196 0.453364
+vt 0.954246 0.453493
+vt 0.950166 0.457401
+vt 0.877438 0.456074
+vt 0.890333 0.455013
+vt 0.881771 0.458050
+vt 0.891902 0.457826
+vt 0.885457 0.459788
+vt 0.893433 0.460126
+vt 0.902160 0.458193
+vt 0.902038 0.460510
+vt 0.902093 0.455956
+vt 0.875073 0.458463
+vt 0.870949 0.459563
+vt 0.869348 0.456995
+vt 0.865831 0.459946
+vt 0.876379 0.459992
+vt 0.880020 0.459819
+vt 0.890696 0.462723
+vt 0.889158 0.462546
+vt 0.893089 0.462870
+vt 0.245158 0.423975
+vt 0.250110 0.468112
+vt 0.148569 0.498875
+vt 0.150442 0.543685
+vt 0.595914 0.442419
+vt 0.701364 0.364720
+vt 0.595493 0.387203
+vt 0.704143 0.312403
+vt 0.757075 0.288915
+vt 0.749567 0.262372
+vt 0.302841 0.416173
+vt 0.302959 0.373132
+vt 0.224027 0.289653
+vt 0.235580 0.359702
+vt 0.134156 0.320572
+vt 0.143764 0.429085
+vt 0.298228 0.319321
+vt 0.288073 0.263566
+vt 0.342229 0.232442
+vt 0.353643 0.285267
+vt 0.355112 0.336459
+vt 0.409212 0.301844
+vt 0.416748 0.239090
+vt 0.412385 0.174765
+vt 0.371257 0.129527
+vt 0.316583 0.183819
+vt 0.312010 0.090946
+vt 0.278107 0.146712
+vt 0.258385 0.177707
+vt 0.272238 0.213152
+vt 0.842115 0.409869
+vt 0.837426 0.409453
+vt 0.838646 0.414482
+vt 0.833434 0.414204
+vt 0.345064 0.636086
+vt 0.339502 0.673242
+vt 0.283208 0.642063
+vt 0.260986 0.672117
+vt 0.331126 0.714598
+vt 0.241009 0.715530
+vt 0.847163 0.420560
+vt 0.840709 0.419570
+vt 0.839445 0.430603
+vt 0.835030 0.428579
+vt 0.843659 0.433436
+vt 0.852464 0.422383
+vt 0.835601 0.419062
+vt 0.830596 0.418889
+vt 0.831257 0.427506
+vt 0.827802 0.427141
+vt 0.326898 0.757163
+vt 0.328393 0.797118
+vt 0.233372 0.762934
+vt 0.238410 0.805793
+vt 0.866399 0.449489
+vt 0.869211 0.447456
+vt 0.876435 0.450731
+vt 0.878828 0.448367
+vt 0.861778 0.461251
+vt 0.863290 0.455790
+vt 0.869445 0.454182
+vt 0.798670 0.019963
+vt 0.720188 0.090411
+vt 0.815355 0.094326
+vt 0.722678 0.141004
+vt 0.586596 0.164171
+vt 0.591566 0.221736
+vt 0.919848 0.462088
+vt 0.924400 0.459418
+vt 0.932799 0.462552
+vt 0.936505 0.460001
+vt 0.914853 0.457033
+vt 0.912969 0.458780
+vt 0.911051 0.461039
+vt 0.945951 0.460521
+vt 0.941970 0.463070
+vt 0.896594 0.462992
+vt 0.902819 0.462828
+vt 0.775513 0.183818
+vt 0.714664 0.225413
+vt 0.594393 0.311859
+vt 0.240207 0.064107
+vt 0.220228 0.127313
+vt 0.137886 0.045291
+vt 0.134162 0.116701
+vt 0.209620 0.176711
+vt 0.132836 0.176022
+vt 0.803506 0.201004
+vt 0.853165 0.100422
+vt 0.393776 0.367451
+vt 0.346805 0.380154
+vt 0.214240 0.228892
+vt 0.133514 0.241035
+vt 0.939147 0.464980
+vt 0.930869 0.464578
+vt 0.937502 0.466649
+vt 0.929700 0.466322
+vt 0.911763 0.464455
+vt 0.904647 0.464520
+vt 0.910929 0.462701
+vt 0.906026 0.466148
+vt 0.912450 0.466086
+vt 0.920833 0.464349
+vt 0.920890 0.466118
+vt 0.899670 0.464526
+vt 0.896490 0.464450
+vt 0.897930 0.466107
+vt 0.901270 0.466162
+vt 0.924570 0.397524
+vt 0.934172 0.397517
+vt 0.901960 0.397532
+vt 0.912366 0.397543
+vt 0.902477 0.398810
+vt 0.914428 0.398827
+vt 0.887934 0.397535
+vt 0.886084 0.398748
+vt 0.939158 0.397548
+vt 0.916553 0.401268
+vt 0.903345 0.401161
+vt 0.904419 0.404260
+vt 0.918892 0.404595
+vt 0.883770 0.401252
+vt 0.880927 0.404722
+vt 0.600870 0.640918
+vt 0.595916 0.679782
+vt 0.517366 0.634965
+vt 0.510699 0.676291
+vt 0.593633 0.724091
+vt 0.505796 0.719121
+vt 0.905488 0.424961
+vt 0.912357 0.424967
+vt 0.905294 0.428912
+vt 0.913340 0.429138
+vt 0.911087 0.422154
+vt 0.905906 0.421495
+vt 0.587548 0.772055
+vt 0.501279 0.762876
+vt 0.497144 0.804816
+vt 0.579229 0.819401
+vt 0.904076 0.437412
+vt 0.904780 0.433190
+vt 0.911884 0.438770
+vt 0.913199 0.433998
+vt 0.570764 0.860428
+vt 0.494669 0.842655
+vt 0.495808 0.877438
+vt 0.568980 0.893979
+vt 0.903210 0.444063
+vt 0.903361 0.441071
+vt 0.908067 0.444769
+vt 0.909872 0.442542
+vt 0.918142 0.453706
+vt 0.902496 0.453271
+vt 0.928783 0.450398
+vt 0.935730 0.445715
+vt 0.939896 0.439756
+vt 0.941415 0.433124
+vt 0.940295 0.426582
+vt 0.937084 0.420446
+vt 0.931618 0.414911
+vt 0.921769 0.409549
+vt 0.905101 0.407893
+vt 0.876573 0.409920
+vt 0.866307 0.415598
+vt 0.859425 0.421874
+vt 0.854503 0.429219
+vt 0.853866 0.435555
+vt 0.855666 0.441340
+vt 0.846009 0.439206
+vt 0.849189 0.444410
+vt 0.859693 0.446280
+vt 0.853635 0.449316
+vt 0.859396 0.453185
+vt 0.333511 0.830512
+vt 0.255441 0.838224
+vt 0.335539 0.859065
+vt 0.275714 0.857682
+vt 0.889827 0.451740
+vt 0.862926 0.421743
+vt 0.859205 0.428005
+vt 0.137030 0.645509
+vt 0.099669 0.709478
+vt 0.114209 0.644610
+vt 0.072436 0.710311
+vt 0.217381 0.600094
+vt 0.193334 0.589335
+vt 0.869662 0.416377
+vt 0.857993 0.434235
+vt 0.859086 0.440054
+vt 0.083907 0.774978
+vt 0.093256 0.834193
+vt 0.056643 0.778851
+vt 0.067488 0.842396
+vt 0.916008 0.412068
+vt 0.903601 0.410265
+vt 0.543475 0.557405
+vt 0.557800 0.533271
+vt 0.655545 0.572596
+vt 0.685834 0.551732
+vt 0.910530 0.415402
+vt 0.913091 0.413541
+vt 0.918257 0.418305
+vt 0.921836 0.416936
+vt 0.925340 0.416062
+vt 0.879532 0.412275
+vt 0.318062 0.571086
+vt 0.304302 0.551225
+vt 0.931494 0.421140
+vt 0.923547 0.422692
+vt 0.927682 0.421785
+vt 0.926515 0.428111
+vt 0.931209 0.427509
+vt 0.935120 0.427050
+vt 0.936295 0.433481
+vt 0.927320 0.434037
+vt 0.932331 0.433786
+vt 0.925669 0.439922
+vt 0.930432 0.440034
+vt 0.934525 0.439972
+vt 0.912845 0.451737
+vt 0.922595 0.449614
+vt 0.907361 0.448978
+vt 0.915126 0.447877
+vt 0.909842 0.450414
+vt 0.918684 0.448912
+vt 0.515560 0.943140
+vt 0.618068 0.949096
+vt 0.529717 0.959537
+vt 0.647404 0.965201
+vt 0.901128 0.451294
+vt 0.929882 0.445604
+vt 0.921412 0.444811
+vt 0.925747 0.445329
+vt 0.862632 0.444730
+vt 0.194162 0.909528
+vt 0.176891 0.924466
+vt 0.127886 0.880246
+vt 0.105635 0.892789
+vt 0.298575 0.918513
+vt 0.289302 0.932026
+vt 0.410040 0.927663
+vt 0.412647 0.940987
+vt 0.152917 0.714025
+vt 0.188039 0.660899
+vt 0.251002 0.622891
+vt 0.148083 0.821618
+vt 0.139091 0.770988
+vt 0.624566 0.605374
+vt 0.529124 0.594060
+vt 0.907910 0.418325
+vt 0.914417 0.420366
+vt 0.333860 0.602351
+vt 0.918293 0.424021
+vt 0.920590 0.428770
+vt 0.920879 0.434155
+vt 0.919487 0.439514
+vt 0.911345 0.446349
+vt 0.904990 0.446728
+vt 0.503730 0.914675
+vt 0.590033 0.923870
+vt 0.916084 0.443813
+vt 0.174218 0.860862
+vt 0.232830 0.882474
+vt 0.318561 0.890705
+vt 0.412205 0.900441
+vt 0.414872 0.864697
+vt 0.414169 0.832004
+vt 0.413463 0.796679
+vt 0.414763 0.757000
+vt 0.418623 0.715141
+vt 0.424042 0.673740
+vt 0.428561 0.633258
+vt 0.429426 0.594048
+vt 0.429051 0.559095
+vt 0.429259 0.535162
+vt 0.891421 0.410387
+vt 0.891335 0.407994
+vt 0.892300 0.404278
+vt 0.892667 0.401108
+vt 0.892803 0.398753
+vt 0.893101 0.397518
+vt 0.953337 0.401538
+vt 0.958091 0.405199
+vt 0.947500 0.399036
+vt 0.961860 0.409791
+vt 0.964499 0.415028
+vt 0.966120 0.420717
+vt 0.967305 0.426924
+vt 0.967388 0.433074
+vt 0.966427 0.438883
+vt 0.964510 0.444202
+vt 0.961861 0.448940
+vt 0.958944 0.453645
+vt 0.955428 0.457592
+vt 0.951844 0.460785
+vt 0.948132 0.463506
+vt 0.945008 0.465414
+vt 0.942800 0.466896
+vt 0.940897 0.397660
+vt 0.894646 0.464377
+vt 0.893748 0.464327
+vt 0.895819 0.465909
+vt 0.894732 0.465707
+vt 0.898046 0.468336
+vt 0.895591 0.467749
+vt 0.894211 0.467377
+vt 0.892528 0.469525
+vt 0.894053 0.470321
+vt 0.897020 0.472125
+vt 0.906461 0.468060
+vt 0.901761 0.468217
+vt 0.901883 0.471147
+vt 0.906501 0.470456
+vt 0.912592 0.467918
+vt 0.912253 0.470089
+vt 0.920674 0.470154
+vt 0.920682 0.467923
+vt 0.929876 0.470181
+vt 0.929300 0.468129
+vt 0.937020 0.468441
+vt 0.938213 0.470896
+vt 0.942275 0.468514
+vt 0.942757 0.470744
+vt 0.890006 0.449500
+vt 0.859393 0.397599
+vt 0.852976 0.399065
+vt 0.847065 0.401675
+vt 0.841270 0.405026
+vt 0.953466 0.401386
+vt 0.958188 0.405089
+vt 0.947588 0.398916
+vt 0.961862 0.409747
+vt 0.964391 0.415022
+vt 0.965926 0.420729
+vt 0.967068 0.426923
+vt 0.967242 0.433135
+vt 0.966314 0.439008
+vt 0.964406 0.444378
+vt 0.961584 0.449120
+vt 0.954279 0.457569
+vt 0.958267 0.453722
+vt 0.870463 0.459645
+vt 0.869977 0.459728
+vt 0.865611 0.460368
+vt 0.865391 0.460791
+vt 0.875883 0.460032
+vt 0.875386 0.460071
+vt 0.889107 0.462568
+vt 0.889055 0.462590
+vt 0.044392 0.517280
+vt 0.043999 0.560029
+vt 0.579657 0.457140
+vt 0.472222 0.467543
+vt 0.576570 0.399149
+vt 0.470583 0.402538
+vt 0.055194 0.327681
+vt 0.048040 0.442488
+vt 0.836433 0.409343
+vt 0.832578 0.414030
+vt 0.832317 0.414141
+vt 0.829542 0.419704
+vt 0.829240 0.419624
+vt 0.456894 0.171519
+vt 0.545104 0.178866
+vt 0.459246 0.232082
+vt 0.554780 0.242428
+vt 0.861810 0.461905
+vt 0.861843 0.462560
+vt 0.950530 0.460750
+vt 0.946775 0.463400
+vt 0.468306 0.320445
+vt 0.572701 0.325840
+vt 0.057144 0.041836
+vt 0.056343 0.115320
+vt 0.056323 0.176259
+vt 0.056512 0.244825
+vt 0.943577 0.465237
+vt 0.941527 0.466697
+vt 0.893773 0.464335
+vt 0.893798 0.464342
+vt 0.894526 0.465626
+vt 0.894385 0.465571
+vt 0.940912 0.397604
+vt 0.893807 0.467247
+vt 0.893563 0.467166
+vt 0.891985 0.469369
+vt 0.891535 0.469419
+vt 0.940762 0.470517
+vt 0.940364 0.468392
+vt 0.866236 0.398834
+vt 0.873711 0.398813
+vt 0.870971 0.397493
+vt 0.877506 0.397514
+vt 0.860032 0.398872
+vt 0.865522 0.397455
+vt 0.856026 0.399014
+vt 0.861954 0.397568
+vt 0.853853 0.399094
+vt 0.860159 0.397664
+vt 0.948546 0.405179
+vt 0.944496 0.401457
+vt 0.935431 0.404681
+vt 0.931632 0.401375
+vt 0.939488 0.398861
+vt 0.927974 0.398901
+vt 0.870657 0.401504
+vt 0.861966 0.401526
+vt 0.855096 0.401600
+vt 0.850751 0.405417
+vt 0.858121 0.405363
+vt 0.867731 0.405188
+vt 0.850561 0.401575
+vt 0.848094 0.401586
+vt 0.842791 0.404957
+vt 0.845766 0.405198
+vt 0.945500 0.398818
+vt 0.951158 0.401375
+vt 0.955535 0.405156
+vt 0.953600 0.414936
+vt 0.951437 0.409790
+vt 0.942950 0.414410
+vt 0.938847 0.409148
+vt 0.863906 0.410047
+vt 0.854469 0.410101
+vt 0.846800 0.410021
+vt 0.843291 0.414792
+vt 0.850618 0.415218
+vt 0.858112 0.415702
+vt 0.958875 0.409842
+vt 0.961147 0.415077
+vt 0.955100 0.420509
+vt 0.945887 0.420220
+vt 0.955942 0.426574
+vt 0.947818 0.426423
+vt 0.962495 0.420748
+vt 0.963347 0.426902
+vt 0.947179 0.438896
+vt 0.955415 0.438781
+vt 0.948355 0.432791
+vt 0.956270 0.432802
+vt 0.963455 0.433109
+vt 0.962550 0.439010
+vt 0.939730 0.449457
+vt 0.950281 0.449152
+vt 0.944309 0.444589
+vt 0.953464 0.444290
+vt 0.960647 0.444424
+vt 0.957836 0.449233
+vt 0.928866 0.456683
+vt 0.941589 0.457208
+vt 0.933669 0.453285
+vt 0.946319 0.453375
+vt 0.954103 0.453514
+vt 0.949874 0.457416
+vt 0.877353 0.455968
+vt 0.882269 0.458025
+vt 0.890577 0.455047
+vt 0.892317 0.457757
+vt 0.886684 0.460112
+vt 0.894230 0.460211
+vt 0.902677 0.460467
+vt 0.902670 0.458019
+vt 0.902808 0.455334
+vt 0.865825 0.458435
+vt 0.871446 0.458982
+vt 0.869597 0.456984
+vt 0.875200 0.458466
+vt 0.877063 0.460000
+vt 0.880928 0.460060
+vt 0.890358 0.462714
+vt 0.889224 0.462652
+vt 0.892541 0.462593
+vt 0.253473 0.420340
+vt 0.198325 0.471074
+vt 0.257191 0.464291
+vt 0.201641 0.514357
+vt 0.644363 0.422582
+vt 0.643061 0.366324
+vt 0.703499 0.366966
+vt 0.703370 0.314326
+vt 0.745969 0.267070
+vt 0.753820 0.293297
+vt 0.305266 0.371658
+vt 0.304409 0.414784
+vt 0.230885 0.286168
+vt 0.167912 0.306331
+vt 0.242536 0.354324
+vt 0.188981 0.401339
+vt 0.300813 0.317790
+vt 0.290984 0.262458
+vt 0.343946 0.231017
+vt 0.355397 0.284532
+vt 0.356783 0.335152
+vt 0.414764 0.292810
+vt 0.419941 0.236001
+vt 0.413771 0.173263
+vt 0.370421 0.125545
+vt 0.313385 0.086563
+vt 0.319456 0.180460
+vt 0.282902 0.142155
+vt 0.261679 0.175104
+vt 0.275739 0.211488
+vt 0.841603 0.409572
+vt 0.838159 0.409243
+vt 0.834209 0.413908
+vt 0.837981 0.414182
+vt 0.352720 0.639241
+vt 0.294558 0.645659
+vt 0.348213 0.675519
+vt 0.274212 0.673847
+vt 0.253789 0.714174
+vt 0.339072 0.715472
+vt 0.846514 0.420792
+vt 0.839885 0.419736
+vt 0.834481 0.428371
+vt 0.838869 0.430451
+vt 0.843430 0.433385
+vt 0.852102 0.422532
+vt 0.834732 0.419195
+vt 0.830666 0.418987
+vt 0.827717 0.426976
+vt 0.830662 0.427303
+vt 0.241492 0.801977
+vt 0.330569 0.796358
+vt 0.241562 0.759109
+vt 0.332348 0.756954
+vt 0.878346 0.448288
+vt 0.868619 0.446935
+vt 0.875777 0.450429
+vt 0.865577 0.448677
+vt 0.860982 0.459109
+vt 0.863516 0.455972
+vt 0.868771 0.453598
+vt 0.797949 0.022112
+vt 0.816816 0.093012
+vt 0.715991 0.092651
+vt 0.720595 0.147032
+vt 0.636546 0.210325
+vt 0.627672 0.148444
+vt 0.919955 0.462087
+vt 0.933215 0.462650
+vt 0.924520 0.459410
+vt 0.936971 0.460171
+vt 0.915797 0.455832
+vt 0.913434 0.458571
+vt 0.911682 0.460937
+vt 0.945768 0.460575
+vt 0.941689 0.463136
+vt 0.897291 0.462545
+vt 0.903757 0.462585
+vt 0.772502 0.189202
+vt 0.711355 0.231084
+vt 0.641706 0.289174
+vt 0.247515 0.059560
+vt 0.175872 0.043369
+vt 0.230266 0.122298
+vt 0.166908 0.113803
+vt 0.162066 0.173298
+vt 0.220575 0.173418
+vt 0.800674 0.205793
+vt 0.855305 0.094200
+vt 0.399643 0.347472
+vt 0.348371 0.378515
+vt 0.223314 0.226619
+vt 0.162858 0.235961
+vt 0.938505 0.464915
+vt 0.930828 0.464538
+vt 0.929084 0.466249
+vt 0.936418 0.466469
+vt 0.904992 0.464496
+vt 0.911714 0.462545
+vt 0.912260 0.464434
+vt 0.906001 0.466171
+vt 0.912549 0.466198
+vt 0.920438 0.466178
+vt 0.920682 0.464371
+vt 0.899694 0.464496
+vt 0.896238 0.464476
+vt 0.897673 0.466070
+vt 0.901142 0.466120
+vt 0.933851 0.397545
+vt 0.924135 0.397565
+vt 0.901567 0.397461
+vt 0.902111 0.398787
+vt 0.911927 0.397463
+vt 0.913998 0.398826
+vt 0.885568 0.398789
+vt 0.887707 0.397486
+vt 0.939027 0.397536
+vt 0.902897 0.401138
+vt 0.916139 0.401275
+vt 0.904075 0.404332
+vt 0.918631 0.404656
+vt 0.883077 0.401373
+vt 0.880245 0.404983
+vt 0.600135 0.641640
+vt 0.519216 0.636196
+vt 0.595756 0.680499
+vt 0.513483 0.677706
+vt 0.509018 0.719971
+vt 0.595108 0.724198
+vt 0.912942 0.429219
+vt 0.912006 0.425106
+vt 0.905419 0.428922
+vt 0.905474 0.425025
+vt 0.910791 0.422281
+vt 0.905844 0.421559
+vt 0.590769 0.769750
+vt 0.503982 0.762367
+vt 0.498249 0.802950
+vt 0.581926 0.813498
+vt 0.904305 0.436885
+vt 0.911907 0.438227
+vt 0.905052 0.432984
+vt 0.913047 0.433844
+vt 0.571758 0.851938
+vt 0.493564 0.840197
+vt 0.493370 0.874734
+vt 0.568900 0.884732
+vt 0.903203 0.443238
+vt 0.908124 0.443775
+vt 0.903445 0.440313
+vt 0.909903 0.441649
+vt 0.902850 0.452456
+vt 0.918742 0.452460
+vt 0.929033 0.449192
+vt 0.935553 0.444785
+vt 0.939489 0.439171
+vt 0.940895 0.433027
+vt 0.939916 0.426757
+vt 0.936762 0.420698
+vt 0.931211 0.415109
+vt 0.904610 0.407951
+vt 0.921344 0.409650
+vt 0.876099 0.410269
+vt 0.865754 0.416146
+vt 0.858856 0.422274
+vt 0.854080 0.429250
+vt 0.848700 0.443676
+vt 0.855252 0.440105
+vt 0.845714 0.439016
+vt 0.853622 0.434905
+vt 0.852614 0.447663
+vt 0.858958 0.445110
+vt 0.858176 0.451853
+vt 0.254385 0.836095
+vt 0.333265 0.830744
+vt 0.334347 0.859927
+vt 0.274272 0.857068
+vt 0.889824 0.451480
+vt 0.858947 0.427844
+vt 0.863033 0.422095
+vt 0.160452 0.654158
+vt 0.126019 0.649524
+vt 0.111374 0.707428
+vt 0.074711 0.707898
+vt 0.232092 0.611555
+vt 0.203545 0.598866
+vt 0.869857 0.416948
+vt 0.858451 0.439034
+vt 0.857612 0.433475
+vt 0.086444 0.767612
+vt 0.053832 0.770468
+vt 0.085639 0.828171
+vt 0.059040 0.833261
+vt 0.915613 0.412200
+vt 0.903128 0.410252
+vt 0.540600 0.554531
+vt 0.653253 0.571690
+vt 0.552741 0.532581
+vt 0.682275 0.551691
+vt 0.910336 0.415321
+vt 0.917730 0.418674
+vt 0.912790 0.413538
+vt 0.921258 0.417307
+vt 0.924944 0.416332
+vt 0.879341 0.412694
+vt 0.321928 0.578317
+vt 0.305300 0.558373
+vt 0.930972 0.421500
+vt 0.922517 0.423241
+vt 0.926792 0.422268
+vt 0.925060 0.428584
+vt 0.929895 0.427930
+vt 0.934420 0.427304
+vt 0.935453 0.433435
+vt 0.925801 0.434263
+vt 0.930838 0.433902
+vt 0.924243 0.439743
+vt 0.929152 0.439726
+vt 0.933786 0.439455
+vt 0.922618 0.448568
+vt 0.913071 0.450609
+vt 0.907300 0.448560
+vt 0.909883 0.449686
+vt 0.914720 0.447235
+vt 0.918464 0.448158
+vt 0.513713 0.938992
+vt 0.528694 0.953445
+vt 0.617347 0.944413
+vt 0.647892 0.957038
+vt 0.901060 0.450464
+vt 0.929479 0.444749
+vt 0.920523 0.444350
+vt 0.924927 0.444695
+vt 0.861743 0.443993
+vt 0.191556 0.905473
+vt 0.119561 0.876603
+vt 0.173642 0.917872
+vt 0.096090 0.886393
+vt 0.296111 0.916924
+vt 0.285563 0.930752
+vt 0.407594 0.925155
+vt 0.411737 0.936637
+vt 0.175067 0.711347
+vt 0.212659 0.665019
+vt 0.264355 0.628942
+vt 0.154483 0.814520
+vt 0.154113 0.763611
+vt 0.623018 0.605621
+vt 0.528987 0.594894
+vt 0.907779 0.418347
+vt 0.914012 0.420577
+vt 0.339791 0.607004
+vt 0.917709 0.424320
+vt 0.919641 0.429082
+vt 0.919962 0.434273
+vt 0.918588 0.439240
+vt 0.911088 0.445577
+vt 0.904848 0.446138
+vt 0.588358 0.917245
+vt 0.500784 0.908814
+vt 0.915514 0.443200
+vt 0.178893 0.855096
+vt 0.233880 0.878659
+vt 0.317203 0.888741
+vt 0.409219 0.897533
+vt 0.412456 0.865776
+vt 0.413517 0.832907
+vt 0.415303 0.797177
+vt 0.419030 0.758087
+vt 0.424284 0.717135
+vt 0.429773 0.675932
+vt 0.433166 0.635493
+vt 0.431719 0.596003
+vt 0.426262 0.535529
+vt 0.428450 0.560105
+vt 0.890897 0.408138
+vt 0.891059 0.410492
+vt 0.891880 0.404459
+vt 0.892481 0.401195
+vt 0.892744 0.398811
+vt 0.892774 0.397499
+vt 0.852987 0.399150
+vt 0.859402 0.397672
+vt 0.847027 0.401638
+vt 0.841440 0.405030
+vt 0.139391 0.505963
+vt 0.140772 0.546375
+vt 0.105327 0.324975
+vt 0.131454 0.436403
+vt 0.836758 0.409301
+vt 0.826856 0.427241
+vt 0.103269 0.042003
+vt 0.100779 0.113050
+vt 0.099208 0.173793
+vt 0.099637 0.242841
+vt 0.893304 0.464316
+vt 0.894343 0.465793
+vt 0.894093 0.464377
+vt 0.895401 0.465971
+vt 0.893628 0.467665
+vt 0.890751 0.471053
+vt 0.897653 0.467915
+vt 0.895086 0.467874
+vt 0.893376 0.470550
+vt 0.896513 0.470318
+vt 0.906138 0.467954
+vt 0.901291 0.467913
+vt 0.900442 0.470186
+vt 0.905385 0.470181
+vt 0.912382 0.468058
+vt 0.911527 0.470316
+vt 0.918635 0.470453
+vt 0.919770 0.468076
+vt 0.926488 0.470872
+vt 0.927835 0.468189
+vt 0.935282 0.468257
+vt 0.935209 0.471328
+vt 0.878197 0.500625
+vt 0.878935 0.505269
+vt 0.877915 0.500707
+vt 0.878376 0.505430
+vt 0.878490 0.500327
+vt 0.879515 0.504678
+vt 0.878776 0.499827
+vt 0.880077 0.503685
+vt 0.879041 0.499148
+vt 0.880596 0.502336
+vt 0.879273 0.498323
+vt 0.881050 0.500691
+vt 0.879461 0.497390
+vt 0.881414 0.498833
+vt 0.879595 0.496393
+vt 0.881685 0.496848
+vt 0.879673 0.495381
+vt 0.881831 0.494834
+vt 0.879688 0.494400
+vt 0.881855 0.492886
+vt 0.879641 0.493495
+vt 0.881754 0.491093
+vt 0.879517 0.492692
+vt 0.881496 0.489502
+vt 0.879256 0.491990
+vt 0.880966 0.488128
+vt 0.878724 0.491679
+vt 0.879976 0.487255
+vt 0.878985 0.491764
+vt 0.880441 0.487544
+vt 0.880259 0.509816
+vt 0.879427 0.510054
+vt 0.881118 0.508941
+vt 0.881951 0.507469
+vt 0.882719 0.505470
+vt 0.883385 0.503037
+vt 0.883918 0.500283
+vt 0.884295 0.497337
+vt 0.884501 0.494341
+vt 0.884524 0.491438
+vt 0.884373 0.488781
+vt 0.883999 0.486461
+vt 0.883210 0.484429
+vt 0.882394 0.483424
+vt 0.882124 0.514208
+vt 0.881033 0.514520
+vt 0.883251 0.513061
+vt 0.884343 0.511133
+vt 0.885349 0.508513
+vt 0.886222 0.505324
+vt 0.886921 0.501715
+vt 0.887414 0.497854
+vt 0.887677 0.493924
+vt 0.887702 0.490113
+vt 0.887496 0.486645
+vt 0.886980 0.483610
+vt 0.885938 0.480910
+vt 0.884878 0.479558
+vt 0.883170 0.518762
+vt 0.884506 0.518380
+vt 0.885883 0.516979
+vt 0.887217 0.514621
+vt 0.888447 0.511419
+vt 0.889514 0.507521
+vt 0.890369 0.503109
+vt 0.890971 0.498390
+vt 0.891293 0.493586
+vt 0.891320 0.488919
+vt 0.891054 0.484665
+vt 0.890395 0.481066
+vt 0.889190 0.477958
+vt 0.887817 0.476052
+vt 0.885807 0.522719
+vt 0.887367 0.522273
+vt 0.888975 0.520637
+vt 0.890533 0.517885
+vt 0.891968 0.514147
+vt 0.893214 0.509596
+vt 0.894212 0.504446
+vt 0.894915 0.498938
+vt 0.895291 0.493329
+vt 0.895322 0.487882
+vt 0.895000 0.482915
+vt 0.894205 0.478779
+vt 0.892842 0.475169
+vt 0.891068 0.473073
+vt 0.888906 0.526332
+vt 0.890668 0.525829
+vt 0.892483 0.523982
+vt 0.894241 0.520876
+vt 0.895861 0.516656
+vt 0.897267 0.511520
+vt 0.898394 0.505707
+vt 0.899187 0.499489
+vt 0.899612 0.493158
+vt 0.899646 0.487009
+vt 0.899286 0.481354
+vt 0.898411 0.476638
+vt 0.892421 0.529549
+vt 0.894359 0.528996
+vt 0.896354 0.526965
+vt 0.898288 0.523550
+vt 0.900069 0.518911
+vt 0.901615 0.513263
+vt 0.902854 0.506872
+vt 0.903726 0.500036
+vt 0.904193 0.493075
+vt 0.904231 0.486314
+vt 0.903834 0.480100
+vt 0.902975 0.475040
+vt 0.896302 0.532323
+vt 0.898388 0.531728
+vt 0.900534 0.529543
+vt 0.902614 0.525869
+vt 0.904531 0.520877
+vt 0.906194 0.514801
+vt 0.907527 0.507925
+vt 0.908466 0.500570
+vt 0.908967 0.493081
+vt 0.909009 0.485806
+vt 0.908572 0.479151
+vt 0.907654 0.473824
+vt 0.900491 0.534614
+vt 0.902694 0.533986
+vt 0.904960 0.531679
+vt 0.907157 0.527799
+vt 0.909181 0.522528
+vt 0.910937 0.516111
+vt 0.912345 0.508850
+vt 0.913336 0.501084
+vt 0.913866 0.493175
+vt 0.913909 0.485494
+vt 0.913460 0.478418
+vt 0.912585 0.472851
+vt 0.904927 0.536388
+vt 0.907215 0.535736
+vt 0.909569 0.533340
+vt 0.911850 0.529311
+vt 0.913952 0.523838
+vt 0.915776 0.517175
+vt 0.917237 0.509635
+vt 0.918266 0.501570
+vt 0.918854 0.493342
+vt 0.918947 0.485357
+vt 0.918440 0.478006
+vt 0.917870 0.471971
+vt 0.911914 0.537955
+vt 0.914263 0.537286
+vt 0.916678 0.534827
+vt 0.919019 0.530693
+vt 0.921175 0.525076
+vt 0.923047 0.518239
+vt 0.924546 0.510502
+vt 0.925585 0.502224
+vt 0.926227 0.493754
+vt 0.926493 0.485583
+vt 0.925762 0.478045
+vt 0.924764 0.471485
+vt 0.919062 0.538392
+vt 0.921403 0.537724
+vt 0.923810 0.535275
+vt 0.926142 0.531156
+vt 0.930338 0.525761
+vt 0.931620 0.519674
+vt 0.933283 0.512932
+vt 0.932692 0.502839
+vt 0.933977 0.494768
+vt 0.934333 0.486371
+vt 0.933206 0.478784
+vt 0.931863 0.472204
+vt 0.923820 0.537920
+vt 0.926110 0.537268
+vt 0.928464 0.534872
+vt 0.932388 0.530343
+vt 0.937335 0.506755
+vt 0.941235 0.498334
+vt 0.941521 0.487989
+vt 0.940371 0.480184
+vt 0.938481 0.473907
+vt 0.928485 0.536884
+vt 0.930691 0.536256
+vt 0.934789 0.533043
+vt 0.932989 0.535298
+vt 0.935078 0.534703
+vt 0.938368 0.532576
+vt 0.947164 0.489068
+vt 0.947066 0.497764
+vt 0.946239 0.481893
+vt 0.944283 0.476284
+vt 0.937267 0.533185
+vt 0.939209 0.532632
+vt 0.941919 0.530982
+vt 0.950271 0.497772
+vt 0.950689 0.490156
+vt 0.950340 0.483450
+vt 0.948203 0.478162
+vt 0.941256 0.530577
+vt 0.943023 0.530074
+vt 0.945589 0.528338
+vt 0.953490 0.491089
+vt 0.952472 0.498024
+vt 0.953154 0.485389
+vt 0.951415 0.480527
+vt 0.944898 0.527510
+vt 0.946464 0.527065
+vt 0.948879 0.525086
+vt 0.955336 0.498108
+vt 0.955494 0.492270
+vt 0.955142 0.488211
+vt 0.954263 0.484425
+vt 0.949389 0.475505
+vt 0.954052 0.482326
+vt 0.947570 0.475109
+vt 0.943613 0.471998
+vt 0.945693 0.473082
+vt 0.949187 0.522238
+vt 0.950577 0.521636
+vt 0.951775 0.520708
+vt 0.950886 0.519379
+vt 0.952605 0.518141
+vt 0.952742 0.516616
+vt 0.954470 0.515316
+vt 0.878125 0.495997
+vt 0.699705 0.808301
+vt 0.767751 0.813767
+vt 0.710393 0.859091
+vt 0.770203 0.857847
+vt 0.670460 0.860010
+vt 0.652167 0.800156
+vt 0.732575 0.902022
+vt 0.706983 0.910753
+vt 0.776574 0.894725
+vt 0.709069 0.706824
+vt 0.772884 0.721001
+vt 0.699610 0.756109
+vt 0.769128 0.766819
+vt 0.650715 0.739559
+vt 0.665169 0.685443
+vt 0.747389 0.629544
+vt 0.781976 0.645207
+vt 0.726035 0.663915
+vt 0.777571 0.679447
+vt 0.692510 0.641696
+vt 0.725600 0.608173
+vt 0.800844 0.613722
+vt 0.782354 0.622030
+vt 0.800842 0.598447
+vt 0.770640 0.606739
+vt 0.761516 0.586356
+vt 0.801211 0.578120
+vt 0.843418 0.585771
+vt 0.884582 0.608278
+vt 0.833474 0.606525
+vt 0.861681 0.629270
+vt 0.821482 0.621769
+vt 0.826927 0.645008
+vt 0.841154 0.721344
+vt 0.835351 0.679466
+vt 0.901508 0.709495
+vt 0.885190 0.664947
+vt 0.920582 0.644640
+vt 0.946246 0.692573
+vt 0.840491 0.814502
+vt 0.842749 0.767471
+vt 0.905321 0.811872
+vt 0.908491 0.759591
+vt 0.957769 0.748636
+vt 0.953229 0.808266
+vt 0.832824 0.894364
+vt 0.836470 0.858222
+vt 0.876480 0.901039
+vt 0.893987 0.860506
+vt 0.934495 0.863509
+vt 0.904941 0.908789
+vt 0.808557 0.928096
+vt 0.833115 0.918674
+vt 0.811165 0.939260
+vt 0.850764 0.928355
+vt 0.866017 0.939324
+vt 0.815782 0.955298
+vt 0.767445 0.930389
+vt 0.781351 0.919619
+vt 0.756778 0.947206
+vt 0.943537 0.514246
+vt 0.944979 0.511457
+vt 0.944004 0.514141
+vt 0.945299 0.511731
+vt 0.946866 0.510467
+vt 0.945612 0.512005
+vt 0.946775 0.509925
+vt 0.944125 0.524447
+vt 0.943125 0.523168
+vt 0.944265 0.522893
+vt 0.943363 0.521760
+vt 0.943213 0.518650
+vt 0.943600 0.520352
+vt 0.942839 0.519741
+vt 0.942465 0.520831
+vt 0.942508 0.517563
+vt 0.943036 0.516979
+vt 0.944472 0.514036
+vt 0.943564 0.516394
+vt 0.948503 0.507957
+vt 0.948316 0.508687
+vt 0.946664 0.509365
+vt 0.949207 0.508705
+vt 0.948096 0.509397
+vt 0.949608 0.507910
+vt 0.950009 0.507096
+vt 0.951195 0.506751
+vt 0.950679 0.507606
+vt 0.950161 0.508443
+vt 0.951005 0.508659
+vt 0.951647 0.507843
+vt 0.951744 0.509448
+vt 0.952587 0.508900
+vt 0.952323 0.510866
+vt 0.953318 0.510638
+vt 0.953444 0.508317
+vt 0.952296 0.507042
+vt 0.954069 0.512878
+vt 0.953344 0.512779
+vt 0.954198 0.510416
+vt 0.952538 0.512757
+vt 0.952220 0.514880
+vt 0.952777 0.515115
+vt 0.952001 0.518020
+vt 0.951641 0.517475
+vt 0.953314 0.515413
+vt 0.951263 0.516984
+vt 0.949848 0.518868
+vt 0.950071 0.519673
+vt 0.948367 0.522605
+vt 0.948260 0.521460
+vt 0.950291 0.520481
+vt 0.948152 0.520315
+vt 0.946642 0.521256
+vt 0.946704 0.522693
+vt 0.945370 0.524785
+vt 0.945389 0.523190
+vt 0.946766 0.524131
+vt 0.945409 0.521595
+vt 0.944404 0.521338
+vt 0.678283 0.626368
+vt 0.715348 0.591757
+vt 0.648199 0.671421
+vt 0.648584 0.866073
+vt 0.628808 0.796368
+vt 0.694447 0.921544
+vt 0.627873 0.728326
+vt 0.756739 0.569509
+vt 0.801790 0.560389
+vt 0.753465 0.960832
+vt 0.819304 0.969461
+vt 0.896919 0.591012
+vt 0.849419 0.567507
+vt 0.878442 0.952645
+vt 0.925282 0.919811
+vt 0.970524 0.680324
+vt 0.939853 0.628765
+vt 0.962143 0.870229
+vt 0.978626 0.806509
+vt 0.983346 0.741752
+vt 0.947425 0.509189
+vt 0.948517 0.508521
+vt 0.946164 0.510225
+vt 0.942852 0.515989
+vt 0.943731 0.513771
+vt 0.942484 0.518080
+vt 0.944874 0.511783
+vt 0.949456 0.508269
+vt 0.950283 0.508511
+vt 0.942699 0.519730
+vt 0.943487 0.520694
+vt 0.950990 0.509314
+vt 0.951552 0.510694
+vt 0.944633 0.521012
+vt 0.946034 0.520645
+vt 0.951791 0.512457
+vt 0.951461 0.514432
+vt 0.947604 0.519694
+vt 0.949207 0.518265
+vt 0.950530 0.516413
+vt 0.954352 0.507334
+vt 0.955064 0.510040
+vt 0.953029 0.505660
+vt 0.954691 0.513093
+vt 0.953563 0.516187
+vt 0.945738 0.508370
+vt 0.942931 0.511155
+vt 0.941124 0.514785
+vt 0.948237 0.525023
+vt 0.946042 0.527051
+vt 0.950358 0.522206
+vt 0.939919 0.523165
+vt 0.940778 0.525984
+vt 0.942236 0.527579
+vt 0.940031 0.519157
+vt 0.950380 0.505668
+vt 0.948360 0.506655
+vt 0.951792 0.505283
+vt 0.952106 0.519194
+vt 0.944080 0.527997
+vt 0.955136 0.505085
+vt 0.955458 0.508799
+vt 0.953996 0.503048
+vt 0.954947 0.512890
+vt 0.953571 0.516724
+vt 0.940392 0.509704
+vt 0.943959 0.505926
+vt 0.937487 0.514053
+vt 0.946965 0.527152
+vt 0.943743 0.529685
+vt 0.949863 0.523920
+vt 0.936263 0.528363
+vt 0.934831 0.524989
+vt 0.938515 0.530358
+vt 0.935440 0.520327
+vt 0.947235 0.504116
+vt 0.949823 0.503295
+vt 0.952034 0.502955
+vt 0.951882 0.520410
+vt 0.941028 0.530790
+vt 0.956376 0.507194
+vt 0.956255 0.501869
+vt 0.955436 0.512282
+vt 0.953780 0.516679
+vt 0.874321 0.260529
+vt 0.874570 0.258357
+vt 0.870642 0.262514
+vt 0.875205 0.262091
+vt 0.870232 0.260129
+vt 0.874180 0.274349
+vt 0.878088 0.275377
+vt 0.875282 0.284248
+vt 0.879128 0.284194
+vt 0.876451 0.268146
+vt 0.871760 0.267630
+vt 0.879196 0.270887
+vt 0.875496 0.265391
+vt 0.893101 0.284420
+vt 0.892287 0.271326
+vt 0.879947 0.284131
+vt 0.877309 0.242158
+vt 0.869772 0.241647
+vt 0.862255 0.219399
+vt 0.855575 0.218438
+vt 0.849440 0.210845
+vt 0.862432 0.239621
+vt 0.849901 0.240235
+vt 0.863194 0.266321
+vt 0.867315 0.244511
+vt 0.876212 0.265675
+vt 0.880423 0.283451
+vt 0.866718 0.283089
+vt 0.870498 0.254293
+vt 0.863904 0.253791
+vt 0.870234 0.270504
+vt 0.864805 0.270607
+vt 0.878645 0.270230
+vt 0.879852 0.254050
+vt 0.894641 0.243235
+vt 0.885819 0.242521
+vt 0.884908 0.221559
+vt 0.874780 0.219802
+vt 0.853855 0.205189
+vt 0.868038 0.204565
+vt 0.856654 0.209577
+vt 0.870975 0.210424
+vt 0.882303 0.208527
+vt 0.885027 0.212810
+vt 0.869343 0.207181
+vt 0.855354 0.205901
+vt 0.879862 0.246326
+vt 0.872817 0.221463
+vt 0.859409 0.218500
+vt 0.885826 0.225344
+vt 0.891813 0.248223
+vt 0.886742 0.266822
+vt 0.897889 0.269881
+vt 0.890434 0.283337
+vt 0.902191 0.283202
+vt 0.887267 0.269985
+vt 0.888486 0.253806
+vt 0.897865 0.269734
+vt 0.897003 0.253871
+vt 0.890889 0.214313
+vt 0.890567 0.213820
+vt 0.887336 0.211472
+vt 0.935827 0.245027
+vt 0.948386 0.259727
+vt 0.940502 0.247339
+vt 0.952135 0.261243
+vt 0.935892 0.257903
+vt 0.929477 0.246937
+vt 0.955141 0.271935
+vt 0.951237 0.271687
+vt 0.956107 0.282705
+vt 0.951697 0.282445
+vt 0.941535 0.282520
+vt 0.939971 0.271031
+vt 0.963745 0.266939
+vt 0.955791 0.248173
+vt 0.971434 0.266214
+vt 0.963770 0.246708
+vt 0.948554 0.235961
+vt 0.955224 0.234835
+vt 0.886107 0.251776
+vt 0.887306 0.257140
+vt 0.896396 0.245174
+vt 0.901578 0.253513
+vt 0.893935 0.234459
+vt 0.893306 0.241625
+vt 0.884880 0.259333
+vt 0.895682 0.256287
+vt 0.898524 0.274248
+vt 0.887363 0.275119
+vt 0.897217 0.265981
+vt 0.885952 0.267220
+vt 0.888292 0.284017
+vt 0.899782 0.283788
+vt 0.893557 0.224473
+vt 0.903384 0.244263
+vt 0.909834 0.245212
+vt 0.904117 0.226402
+vt 0.898452 0.232315
+vt 0.905360 0.242289
+vt 0.908475 0.231470
+vt 0.914901 0.241090
+vt 0.912117 0.251402
+vt 0.921110 0.249824
+vt 0.894105 0.223477
+vt 0.905103 0.223634
+vt 0.892209 0.224715
+vt 0.891627 0.223783
+vt 0.894785 0.217728
+vt 0.904032 0.218825
+vt 0.916745 0.259188
+vt 0.905292 0.260920
+vt 0.925679 0.257542
+vt 0.919312 0.270264
+vt 0.927598 0.269904
+vt 0.909165 0.270372
+vt 0.923018 0.231732
+vt 0.931448 0.236491
+vt 0.922229 0.248254
+vt 0.927474 0.250713
+vt 0.931475 0.252998
+vt 0.939132 0.243721
+vt 0.941395 0.236759
+vt 0.941774 0.243180
+vt 0.953028 0.242625
+vt 0.951181 0.247570
+vt 0.943442 0.249961
+vt 0.949719 0.251412
+vt 0.935522 0.249090
+vt 0.932269 0.241398
+vt 0.930136 0.234395
+vt 0.929446 0.228394
+vt 0.942151 0.230847
+vt 0.929372 0.224433
+vt 0.942787 0.227016
+vt 0.954848 0.237908
+vt 0.922857 0.256527
+vt 0.927955 0.258098
+vt 0.925166 0.269641
+vt 0.929776 0.270263
+vt 0.933130 0.259066
+vt 0.935431 0.270899
+vt 0.945703 0.257403
+vt 0.938091 0.256867
+vt 0.953024 0.257866
+vt 0.948714 0.269472
+vt 0.957575 0.269677
+vt 0.940805 0.269454
+vt 0.944953 0.253898
+vt 0.950208 0.269278
+vt 0.957956 0.256418
+vt 0.964661 0.269455
+vt 0.961307 0.253711
+vt 0.968960 0.268209
+vt 0.902892 0.249987
+vt 0.897745 0.228849
+vt 0.908596 0.231834
+vt 0.912735 0.252197
+vt 0.894850 0.221810
+vt 0.906240 0.226903
+vt 0.897122 0.224227
+vt 0.908607 0.228742
+vt 0.903082 0.242361
+vt 0.906809 0.256103
+vt 0.919494 0.264606
+vt 0.908311 0.265315
+vt 0.917237 0.255133
+vt 0.907454 0.269025
+vt 0.916487 0.269043
+vt 0.911531 0.283044
+vt 0.919593 0.282900
+vt 0.909419 0.273405
+vt 0.911476 0.283360
+vt 0.920571 0.273041
+vt 0.922157 0.283218
+vt 0.922114 0.255039
+vt 0.920062 0.238863
+vt 0.921052 0.234472
+vt 0.923775 0.235380
+vt 0.934895 0.263753
+vt 0.929369 0.253983
+vt 0.927988 0.282737
+vt 0.925160 0.269608
+vt 0.937386 0.272947
+vt 0.938515 0.283177
+vt 0.905634 0.254510
+vt 0.912156 0.254648
+vt 0.906903 0.269654
+vt 0.915624 0.269303
+vt 0.934262 0.269581
+vt 0.931993 0.256855
+vt 0.928513 0.249010
+vt 0.928512 0.223212
+vt 0.917128 0.220406
+vt 0.916725 0.220767
+vt 0.904100 0.217973
+vt 0.914088 0.228391
+vt 0.916363 0.246441
+vt 0.918617 0.232270
+vt 0.923487 0.240784
+vt 0.938248 0.227770
+vt 0.892821 0.216321
+vt 0.844542 0.206031
+vt 0.874322 0.212263
+vt 0.864795 0.285932
+vt 0.870581 0.286403
+vt 0.877377 0.285736
+vt 0.887924 0.285532
+vt 0.898823 0.285311
+vt 0.908672 0.285115
+vt 0.917425 0.284957
+vt 0.921216 0.269224
+vt 0.922914 0.284906
+vt 0.926665 0.284912
+vt 0.930758 0.284928
+vt 0.936388 0.284960
+vt 0.952837 0.284936
+vt 0.967867 0.284724
+vt 0.971593 0.266723
+vt 0.976556 0.284338
+vt 0.976161 0.284497
+vt 0.973686 0.284223
+vt 0.968029 0.284169
+vt 0.960033 0.284154
+vt 0.950775 0.284138
+vt 0.942494 0.284121
+vt 0.936182 0.284123
+vt 0.929809 0.284158
+vt 0.920968 0.284223
+vt 0.910111 0.284292
+vt 0.952943 0.272204
+vt 0.950019 0.262148
+vt 0.953976 0.282879
+vt 0.900909 0.229617
+vt 0.889642 0.217102
+vt 0.912068 0.234085
+vt 0.913562 0.243735
+vt 0.925227 0.239763
+vt 0.923110 0.245509
+vt 0.916786 0.225007
+vt 0.940372 0.249840
+vt 0.963568 0.250138
+vt 0.888081 0.264217
+vt 0.918118 0.254988
+vt 0.953969 0.514386
+vt 0.955477 0.513065
+vt 0.954272 0.512902
+vt 0.955509 0.511632
+vt 0.956562 0.510151
+vt 0.957528 0.506237
+vt 0.957401 0.500104
+vt 0.957541 0.505830
+vt 0.957323 0.498901
+vt 0.956684 0.496896
+vt 0.956425 0.492794
+vt 0.955922 0.489330
+vt 0.956413 0.492938
+vt 0.955605 0.489791
+vt 0.954030 0.483685
+vt 0.955302 0.484788
+vt 0.952251 0.480529
+vt 0.844126 0.209259
+vt 0.844951 0.221286
+vt 0.955335 0.486204
+vt 0.955539 0.486154
+vt 0.954877 0.485325
+vt 0.956687 0.508947
+vt 0.956754 0.495687
+vt 0.955045 0.487387
+vt 0.886769 0.216308
+vt 0.872985 0.215718
+vt 0.877848 0.505182
+vt 0.877652 0.500582
+vt 0.877370 0.504506
+vt 0.877412 0.500243
+vt 0.876974 0.503437
+vt 0.877218 0.499703
+vt 0.876680 0.502023
+vt 0.877063 0.498994
+vt 0.876501 0.500330
+vt 0.876979 0.498142
+vt 0.876446 0.498438
+vt 0.876960 0.497191
+vt 0.876522 0.496436
+vt 0.877006 0.496186
+vt 0.876728 0.494420
+vt 0.877115 0.495174
+vt 0.877281 0.494204
+vt 0.877054 0.492488
+vt 0.877498 0.493320
+vt 0.877479 0.490730
+vt 0.877999 0.489197
+vt 0.877767 0.492546
+vt 0.878719 0.487808
+vt 0.878144 0.491879
+vt 0.879235 0.487423
+vt 0.878402 0.491725
+vt 0.878643 0.509686
+vt 0.877934 0.508686
+vt 0.877348 0.507101
+vt 0.876912 0.505005
+vt 0.876647 0.502497
+vt 0.876565 0.499694
+vt 0.876669 0.496727
+vt 0.876956 0.493734
+vt 0.877416 0.490859
+vt 0.878045 0.488251
+vt 0.878803 0.485988
+vt 0.879891 0.483890
+vt 0.880652 0.483253
+vt 0.880004 0.514038
+vt 0.879075 0.512727
+vt 0.878307 0.510649
+vt 0.877736 0.507903
+vt 0.877388 0.504616
+vt 0.877280 0.500942
+vt 0.877417 0.497054
+vt 0.877793 0.493132
+vt 0.878389 0.489360
+vt 0.879214 0.485947
+vt 0.880199 0.482989
+vt 0.881623 0.480314
+vt 0.882641 0.479405
+vt 0.881911 0.518172
+vt 0.880775 0.516569
+vt 0.879836 0.514030
+vt 0.879138 0.510673
+vt 0.878713 0.506655
+vt 0.878582 0.502165
+vt 0.878749 0.497412
+vt 0.879208 0.492618
+vt 0.879955 0.488021
+vt 0.881001 0.483899
+vt 0.882310 0.480367
+vt 0.883909 0.477310
+vt 0.885135 0.475926
+vt 0.884336 0.522030
+vt 0.883011 0.520159
+vt 0.881915 0.517195
+vt 0.881100 0.513276
+vt 0.880604 0.508586
+vt 0.880450 0.503344
+vt 0.880646 0.497795
+vt 0.881181 0.492199
+vt 0.882059 0.486840
+vt 0.883257 0.482017
+vt 0.884772 0.477872
+vt 0.886828 0.474331
+vt 0.888116 0.472377
+vt 0.887245 0.525554
+vt 0.885749 0.523442
+vt 0.884512 0.520096
+vt 0.883592 0.515673
+vt 0.883032 0.510379
+vt 0.882858 0.504462
+vt 0.883079 0.498199
+vt 0.883683 0.491882
+vt 0.884656 0.485819
+vt 0.885976 0.480334
+vt 0.887771 0.475644
+vt 0.888962 0.472096
+vt 0.890594 0.528694
+vt 0.888949 0.526372
+vt 0.887589 0.522693
+vt 0.886578 0.517830
+vt 0.885962 0.512009
+vt 0.885771 0.505504
+vt 0.886014 0.498617
+vt 0.886678 0.491672
+vt 0.887734 0.484992
+vt 0.889161 0.478945
+vt 0.891005 0.474124
+vt 0.894335 0.531403
+vt 0.892565 0.528905
+vt 0.891102 0.524947
+vt 0.890014 0.519714
+vt 0.889351 0.513452
+vt 0.889146 0.506453
+vt 0.889407 0.499043
+vt 0.890122 0.491571
+vt 0.891258 0.484384
+vt 0.892815 0.477978
+vt 0.894695 0.473363
+vt 0.898413 0.533643
+vt 0.896544 0.531004
+vt 0.894999 0.526825
+vt 0.893850 0.521300
+vt 0.893151 0.514687
+vt 0.892934 0.507295
+vt 0.893209 0.499472
+vt 0.893965 0.491581
+vt 0.895164 0.483992
+vt 0.896860 0.477387
+vt 0.898865 0.473076
+vt 0.902769 0.535380
+vt 0.900829 0.532640
+vt 0.899224 0.528300
+vt 0.898031 0.522562
+vt 0.897305 0.515695
+vt 0.897079 0.508020
+vt 0.897366 0.499895
+vt 0.898150 0.491701
+vt 0.899396 0.483821
+vt 0.901449 0.477074
+vt 0.903819 0.472976
+vt 0.909699 0.536920
+vt 0.907707 0.534109
+vt 0.906061 0.529655
+vt 0.904836 0.523767
+vt 0.904091 0.516721
+vt 0.903860 0.508844
+vt 0.904153 0.500507
+vt 0.904958 0.492099
+vt 0.906237 0.484012
+vt 0.908008 0.477129
+vt 0.910034 0.473198
+vt 0.916854 0.537360
+vt 0.914870 0.534559
+vt 0.913230 0.530122
+vt 0.914164 0.524465
+vt 0.913164 0.518196
+vt 0.913437 0.511340
+vt 0.911330 0.501080
+vt 0.912131 0.492703
+vt 0.913405 0.484646
+vt 0.915130 0.477830
+vt 0.916994 0.473623
+vt 0.921660 0.536911
+vt 0.919719 0.534172
+vt 0.919170 0.529284
+vt 0.917172 0.505296
+vt 0.919838 0.496406
+vt 0.920904 0.486229
+vt 0.922380 0.479225
+vt 0.924263 0.474614
+vt 0.926405 0.535913
+vt 0.924828 0.532245
+vt 0.931019 0.534378
+vt 0.929686 0.531880
+vt 0.927970 0.487724
+vt 0.927599 0.495894
+vt 0.929366 0.480734
+vt 0.931925 0.475167
+vt 0.935435 0.532330
+vt 0.934387 0.530379
+vt 0.933232 0.488977
+vt 0.932764 0.496065
+vt 0.934736 0.482159
+vt 0.937602 0.476887
+vt 0.939590 0.529799
+vt 0.938848 0.527798
+vt 0.937731 0.490249
+vt 0.936964 0.496523
+vt 0.939215 0.484123
+vt 0.942421 0.479379
+vt 0.943421 0.526821
+vt 0.942722 0.524593
+vt 0.942329 0.491435
+vt 0.942070 0.497013
+vt 0.943599 0.487511
+vt 0.946933 0.482758
+vt 0.945342 0.476815
+vt 0.949756 0.480976
+vt 0.940652 0.473783
+vt 0.947564 0.521395
+vt 0.946600 0.520277
+vt 0.949518 0.517928
+vt 0.951324 0.514898
+vt 0.699423 0.809253
+vt 0.710111 0.860044
+vt 0.767468 0.814720
+vt 0.769921 0.858800
+vt 0.669688 0.860514
+vt 0.651885 0.801109
+vt 0.732293 0.902975
+vt 0.706453 0.911484
+vt 0.776292 0.895678
+vt 0.708787 0.707777
+vt 0.699329 0.757062
+vt 0.772602 0.721954
+vt 0.768846 0.767772
+vt 0.650433 0.740512
+vt 0.664887 0.686395
+vt 0.747107 0.630497
+vt 0.725753 0.664868
+vt 0.781693 0.646160
+vt 0.777288 0.680400
+vt 0.692228 0.642648
+vt 0.725318 0.609127
+vt 0.770358 0.607692
+vt 0.782072 0.622983
+vt 0.800560 0.599400
+vt 0.800562 0.614675
+vt 0.761234 0.587309
+vt 0.800928 0.579073
+vt 0.861399 0.630223
+vt 0.884300 0.609231
+vt 0.833192 0.607478
+vt 0.843136 0.586725
+vt 0.821200 0.622722
+vt 0.826645 0.645961
+vt 0.884907 0.665900
+vt 0.835069 0.680418
+vt 0.901226 0.710448
+vt 0.840872 0.722297
+vt 0.945964 0.693526
+vt 0.920299 0.645594
+vt 0.908209 0.760544
+vt 0.842467 0.768423
+vt 0.905039 0.812825
+vt 0.840209 0.815454
+vt 0.952947 0.809219
+vt 0.957487 0.749588
+vt 0.893705 0.861459
+vt 0.836187 0.859175
+vt 0.876198 0.901993
+vt 0.832541 0.895317
+vt 0.904659 0.909742
+vt 0.934213 0.864462
+vt 0.850482 0.929308
+vt 0.832833 0.919627
+vt 0.810883 0.940213
+vt 0.808275 0.929049
+vt 0.815494 0.955807
+vt 0.865735 0.940277
+vt 0.767163 0.931342
+vt 0.781069 0.920572
+vt 0.757688 0.947696
+vt 0.926885 0.512910
+vt 0.927854 0.512845
+vt 0.928377 0.510114
+vt 0.929178 0.510430
+vt 0.931406 0.509225
+vt 0.930823 0.508639
+vt 0.929980 0.510746
+vt 0.930534 0.523359
+vt 0.930891 0.521821
+vt 0.928350 0.521984
+vt 0.928964 0.520607
+vt 0.928441 0.517467
+vt 0.927597 0.518520
+vt 0.929579 0.519229
+vt 0.926753 0.519573
+vt 0.926179 0.516255
+vt 0.927195 0.515710
+vt 0.928823 0.512781
+vt 0.928211 0.515165
+vt 0.932374 0.506682
+vt 0.930239 0.508053
+vt 0.932657 0.507439
+vt 0.934517 0.507536
+vt 0.934510 0.506707
+vt 0.932940 0.508196
+vt 0.934502 0.505878
+vt 0.936567 0.505592
+vt 0.936317 0.506456
+vt 0.936067 0.507320
+vt 0.937546 0.507578
+vt 0.938070 0.506748
+vt 0.938901 0.508422
+vt 0.940036 0.509880
+vt 0.939772 0.507879
+vt 0.941088 0.509726
+vt 0.940713 0.507314
+vt 0.938594 0.505926
+vt 0.942911 0.511998
+vt 0.942269 0.509491
+vt 0.941755 0.511888
+vt 0.940774 0.511818
+vt 0.940867 0.513981
+vt 0.941724 0.514237
+vt 0.941696 0.517149
+vt 0.942674 0.514592
+vt 0.940898 0.516619
+vt 0.940099 0.516090
+vt 0.938667 0.517973
+vt 0.939267 0.518807
+vt 0.937547 0.521739
+vt 0.939868 0.519641
+vt 0.937153 0.520570
+vt 0.936759 0.519402
+vt 0.934851 0.520311
+vt 0.935057 0.521760
+vt 0.932933 0.523788
+vt 0.935263 0.523209
+vt 0.932985 0.522196
+vt 0.933038 0.520604
+vt 0.931248 0.520284
+vt 0.715065 0.592710
+vt 0.678001 0.627321
+vt 0.647916 0.672373
+vt 0.628526 0.797321
+vt 0.648988 0.865355
+vt 0.693916 0.922274
+vt 0.627590 0.729278
+vt 0.801508 0.561342
+vt 0.756457 0.570463
+vt 0.753635 0.961316
+vt 0.820182 0.969676
+vt 0.896636 0.591965
+vt 0.849136 0.568460
+vt 0.878160 0.953598
+vt 0.925000 0.920764
+vt 0.970242 0.681277
+vt 0.939570 0.629718
+vt 0.961861 0.871181
+vt 0.978343 0.807461
+vt 0.983064 0.742705
+vt 0.932573 0.508007
+vt 0.934161 0.507367
+vt 0.930964 0.509008
+vt 0.927841 0.514787
+vt 0.928411 0.512543
+vt 0.928106 0.516928
+vt 0.929515 0.510546
+vt 0.935671 0.507169
+vt 0.937081 0.507438
+vt 0.929093 0.518640
+vt 0.930710 0.519671
+vt 0.938351 0.508298
+vt 0.939400 0.509723
+vt 0.932552 0.520045
+vt 0.934434 0.519715
+vt 0.940079 0.511519
+vt 0.940113 0.513519
+vt 0.936336 0.518792
+vt 0.938116 0.517377
+vt 0.939405 0.515521
+vt 0.943638 0.508965
+vt 0.941943 0.506311
+vt 0.939071 0.504531
+vt 0.943880 0.515430
+vt 0.944335 0.512165
+vt 0.927944 0.506963
+vt 0.925069 0.509715
+vt 0.923231 0.513349
+vt 0.935271 0.526189
+vt 0.938229 0.524222
+vt 0.940839 0.521447
+vt 0.923484 0.521848
+vt 0.925731 0.524779
+vt 0.928789 0.526502
+vt 0.922597 0.517761
+vt 0.933960 0.504385
+vt 0.931029 0.505302
+vt 0.936534 0.504088
+vt 0.942769 0.518413
+vt 0.932116 0.527039
+vt 0.945351 0.507568
+vt 0.943089 0.503744
+vt 0.939896 0.502018
+vt 0.946118 0.511811
+vt 0.945354 0.516015
+vt 0.925289 0.504712
+vt 0.920962 0.508150
+vt 0.918385 0.512520
+vt 0.938552 0.526478
+vt 0.934537 0.528948
+vt 0.941809 0.523283
+vt 0.921702 0.527196
+vt 0.918305 0.523665
+vt 0.925959 0.529352
+vt 0.917212 0.518867
+vt 0.933078 0.501722
+vt 0.929349 0.502508
+vt 0.936447 0.501609
+vt 0.943896 0.519878
+vt 0.930309 0.529931
+vt 0.948005 0.505607
+vt 0.944871 0.499392
+vt 0.947806 0.516062
+vt 0.949100 0.511191
+vt 0.879346 0.309871
+vt 0.875833 0.306046
+vt 0.875654 0.308144
+vt 0.879007 0.306407
+vt 0.876295 0.294136
+vt 0.878977 0.293033
+vt 0.876175 0.301094
+vt 0.879812 0.300399
+vt 0.882198 0.297454
+vt 0.880254 0.303450
+vt 0.893175 0.297687
+vt 0.859181 0.350621
+vt 0.872916 0.327332
+vt 0.865817 0.349629
+vt 0.880252 0.326495
+vt 0.865713 0.330422
+vt 0.852822 0.359205
+vt 0.877097 0.301439
+vt 0.864055 0.301093
+vt 0.869257 0.322943
+vt 0.852819 0.327816
+vt 0.872679 0.316230
+vt 0.866118 0.317194
+vt 0.865658 0.301152
+vt 0.871305 0.301562
+vt 0.879516 0.300927
+vt 0.881998 0.315802
+vt 0.878301 0.348969
+vt 0.888479 0.326114
+vt 0.888262 0.347411
+vt 0.897047 0.325511
+vt 0.857019 0.363938
+vt 0.860381 0.359999
+vt 0.872015 0.364197
+vt 0.874655 0.358691
+vt 0.888629 0.355800
+vt 0.886097 0.359632
+vt 0.872907 0.361011
+vt 0.858128 0.362290
+vt 0.881706 0.320824
+vt 0.875903 0.346079
+vt 0.862423 0.349295
+vt 0.893556 0.318638
+vt 0.888714 0.341972
+vt 0.887564 0.300041
+vt 0.898554 0.296744
+vt 0.888147 0.300674
+vt 0.898715 0.300283
+vt 0.890480 0.315686
+vt 0.898830 0.315390
+vt 0.894383 0.353795
+vt 0.894065 0.354129
+vt 0.890921 0.356254
+vt 0.952900 0.304272
+vt 0.949521 0.305533
+vt 0.942171 0.318343
+vt 0.937705 0.320863
+vt 0.937122 0.307590
+vt 0.931259 0.318903
+vt 0.955735 0.293528
+vt 0.951772 0.293299
+vt 0.940544 0.294181
+vt 0.964624 0.302435
+vt 0.972337 0.302695
+vt 0.957594 0.320997
+vt 0.965634 0.321964
+vt 0.957663 0.333351
+vt 0.950951 0.332759
+vt 0.904388 0.314564
+vt 0.887892 0.312338
+vt 0.897685 0.323309
+vt 0.888936 0.316243
+vt 0.878641 0.308129
+vt 0.895710 0.326063
+vt 0.896265 0.333541
+vt 0.887647 0.308911
+vt 0.896958 0.311973
+vt 0.897963 0.301935
+vt 0.887703 0.301029
+vt 0.898883 0.293285
+vt 0.887878 0.292930
+vt 0.896731 0.344468
+vt 0.905567 0.324686
+vt 0.906991 0.342485
+vt 0.911791 0.324251
+vt 0.900908 0.335596
+vt 0.911043 0.336295
+vt 0.907054 0.325968
+vt 0.917143 0.326675
+vt 0.923285 0.318080
+vt 0.915184 0.316034
+vt 0.908088 0.344187
+vt 0.897103 0.344536
+vt 0.895142 0.343144
+vt 0.894607 0.344154
+vt 0.898106 0.350443
+vt 0.907260 0.349181
+vt 0.907871 0.308374
+vt 0.918398 0.309598
+vt 0.927049 0.310818
+vt 0.928305 0.298442
+vt 0.920004 0.298182
+vt 0.909927 0.298243
+vt 0.929162 0.318872
+vt 0.933826 0.332572
+vt 0.924036 0.321254
+vt 0.925629 0.337191
+vt 0.941158 0.325550
+vt 0.933051 0.316658
+vt 0.943727 0.330971
+vt 0.955075 0.325269
+vt 0.943791 0.324629
+vt 0.952983 0.320359
+vt 0.951332 0.316568
+vt 0.945126 0.317960
+vt 0.937248 0.318798
+vt 0.934373 0.326359
+vt 0.932584 0.333273
+vt 0.932192 0.339300
+vt 0.932318 0.343421
+vt 0.944776 0.336887
+vt 0.945607 0.340893
+vt 0.957129 0.330060
+vt 0.924262 0.313255
+vt 0.925922 0.300164
+vt 0.929283 0.311724
+vt 0.930502 0.299579
+vt 0.936126 0.298969
+vt 0.934410 0.310774
+vt 0.939442 0.311396
+vt 0.947027 0.310849
+vt 0.954323 0.310334
+vt 0.958291 0.298597
+vt 0.949440 0.298819
+vt 0.941532 0.298832
+vt 0.946483 0.315703
+vt 0.950981 0.300512
+vt 0.969751 0.300159
+vt 0.965388 0.298807
+vt 0.962811 0.314479
+vt 0.959326 0.311738
+vt 0.904541 0.316598
+vt 0.900452 0.338160
+vt 0.914268 0.314088
+vt 0.911146 0.334819
+vt 0.910827 0.338483
+vt 0.909045 0.340214
+vt 0.900043 0.343367
+vt 0.897918 0.345703
+vt 0.905082 0.324826
+vt 0.907714 0.311836
+vt 0.909052 0.301920
+vt 0.918749 0.311513
+vt 0.920236 0.301846
+vt 0.908155 0.297332
+vt 0.917180 0.297035
+vt 0.909931 0.293440
+vt 0.920410 0.293444
+vt 0.923496 0.310879
+vt 0.922253 0.327378
+vt 0.923469 0.332076
+vt 0.925923 0.331262
+vt 0.930583 0.312153
+vt 0.934974 0.302503
+vt 0.925817 0.296140
+vt 0.937580 0.293425
+vt 0.916398 0.300537
+vt 0.913653 0.315103
+vt 0.907706 0.300101
+vt 0.907277 0.314776
+vt 0.934984 0.298714
+vt 0.933344 0.311407
+vt 0.930312 0.318902
+vt 0.931529 0.345055
+vt 0.920283 0.347817
+vt 0.919853 0.347075
+vt 0.907379 0.350402
+vt 0.916863 0.340467
+vt 0.918259 0.323025
+vt 0.921166 0.335346
+vt 0.925584 0.327014
+vt 0.941044 0.340685
+vt 0.896265 0.352186
+vt 0.847118 0.363087
+vt 0.877923 0.355511
+vt 0.921992 0.300554
+vt 0.972463 0.301867
+vt 0.953466 0.293583
+vt 0.950828 0.303658
+vt 0.892951 0.350759
+vt 0.903260 0.337983
+vt 0.915596 0.322974
+vt 0.914170 0.333036
+vt 0.927312 0.326701
+vt 0.925040 0.320904
+vt 0.919699 0.342662
+vt 0.941871 0.316003
+vt 0.965254 0.318221
+vt 0.889956 0.306147
+vt 0.919598 0.314755
+vt 0.953215 0.511264
+vt 0.952698 0.512701
+vt 0.951135 0.508840
+vt 0.950868 0.504414
+vt 0.946032 0.497811
+vt 0.950400 0.504692
+vt 0.946155 0.498790
+vt 0.944108 0.496015
+vt 0.945966 0.489086
+vt 0.945497 0.488606
+vt 0.944681 0.492183
+vt 0.944291 0.491984
+vt 0.951345 0.484260
+vt 0.845891 0.359181
+vt 0.847322 0.346695
+vt 0.948386 0.485293
+vt 0.952907 0.485943
+vt 0.952013 0.507755
+vt 0.944606 0.494766
+vt 0.948560 0.486751
+vt 0.890112 0.351330
+vt 0.876360 0.352039
+vt 0.889967 0.448966
+vt 0.878531 0.491695
+vt 0.879572 0.487300
+vt 0.881189 0.483034
+vt 0.883359 0.479059
+vt 0.885970 0.475436
+vt 0.826451 0.427416
+vt 0.889758 0.472115
+vt 0.881748 0.482959
+vt 0.884054 0.478968
+vt 0.886811 0.475429
+vt 0.942835 0.472365
+vt 0.883515 0.397651
+vt 0.925482 0.397634
+vt 0.918562 0.397639
+vt 0.909465 0.397642
+vt 0.901603 0.397646
+vt 0.891034 0.397645
+vt 0.894812 0.397645
+vt 0.931235 0.397655
+vt 0.877003 0.397658
+vt 0.882529 0.397650
+vt 0.871595 0.397671
+vt 0.868659 0.397671
+vt 0.925845 0.397644
+vt 0.918784 0.397657
+vt 0.909614 0.397666
+vt 0.901249 0.397670
+vt 0.890498 0.397656
+vt 0.929610 0.397643
+vt 0.894099 0.397656
+vt 0.484696 0.402538
+vt 0.483058 0.467543
+vt 0.026409 0.041836
+vt 0.027211 0.115320
+vt 0.027231 0.176259
+vt 0.027041 0.244825
+vt 0.028360 0.327681
+vt 0.035514 0.442488
+vt 0.039161 0.517280
+vt 0.039555 0.560029
+vt 0.486973 0.320445
+vt 0.496033 0.232082
+vt 0.498386 0.171519
+vt 0.166927 0.932027
+vt 0.089131 0.901460
+vt 0.412839 0.954955
+vt 0.280654 0.942250
+vt 0.172265 0.583534
+vt 0.092604 0.643694
+vt 0.048602 0.713913
+vt 0.034269 0.783775
+vt 0.288973 0.537530
+vt 0.047197 0.849027
+vt 0.429574 0.516361
+vt 0.573602 0.514995
+vt 0.720327 0.535205
+vt 0.544365 0.975068
+vt 0.682918 0.980037
+vt 0.412382 0.948959
+vt 0.274953 0.941360
+vt 0.159929 0.926191
+vt 0.078614 0.893197
+vt 0.045546 0.712112
+vt 0.029763 0.775258
+vt 0.174568 0.589926
+vt 0.093871 0.647648
+vt 0.286715 0.542233
+vt 0.039682 0.837587
+vt 0.425296 0.517534
+vt 0.568016 0.514851
+vt 0.715649 0.536695
+vt 0.543555 0.965760
+vt 0.685594 0.967391
+usemtl blinn3
+s off
+f 1988/1 1990/2 2015/3
+f 1990/2 1991/4 2015/3
+f 1991/4 1992/5 2015/3
+f 2000/6 1998/7 2015/3
+f 1992/5 2019/8 2015/3
+f 2019/8 2001/9 2015/3
+f 2001/9 2007/10 2015/3
+s 1
+f 90/11 244/12 7/13
+f 7/13 244/12 91/14
+f 244/12 90/11 92/15
+f 92/15 90/11 6/16
+f 93/17 245/18 6/16
+f 6/16 245/18 92/15
+f 245/18 93/17 300/19
+f 300/19 93/17 219/20
+f 96/21 94/22 8/23
+f 8/23 94/22 9/24
+f 94/22 96/21 10/25
+f 10/25 96/21 95/26
+f 246/27 90/11 97/28
+f 97/28 90/11 7/13
+f 90/11 246/27 6/16
+f 6/16 246/27 98/29
+f 246/27 99/30 98/29
+f 98/29 99/30 12/31
+f 99/30 246/27 11/32
+f 11/32 246/27 97/28
+f 247/33 93/17 98/29
+f 98/29 93/17 6/16
+f 93/17 247/33 219/20
+f 219/20 247/33 301/34
+f 247/33 100/35 301/34
+f 301/34 100/35 220/36
+f 100/35 247/33 12/31
+f 12/31 247/33 98/29
+f 248/37 94/22 13/38
+f 13/38 94/22 10/25
+f 94/22 248/37 9/24
+f 9/24 248/37 101/39
+f 103/40 102/41 14/42
+f 14/42 102/41 15/43
+f 102/41 103/40 9/24
+f 9/24 103/40 8/23
+f 249/44 99/30 104/45
+f 104/45 99/30 11/32
+f 99/30 249/44 12/31
+f 12/31 249/44 105/46
+f 249/44 106/47 105/46
+f 105/46 106/47 17/48
+f 106/47 249/44 16/49
+f 16/49 249/44 104/45
+f 108/50 102/41 101/39
+f 101/39 102/41 9/24
+f 102/41 108/50 15/43
+f 15/43 108/50 107/51
+f 109/52 110/53 15/43
+f 15/43 110/53 14/42
+f 110/53 109/52 19/54
+f 19/54 109/52 18/55
+f 112/56 109/52 107/51
+f 107/51 109/52 15/43
+f 109/52 112/56 18/55
+f 18/55 112/56 111/57
+f 21/58 113/59 20/60
+f 20/60 113/59 114/61
+f 114/61 113/59 18/55
+f 18/55 113/59 19/54
+f 116/62 114/61 111/57
+f 111/57 114/61 18/55
+f 116/62 115/63 114/61
+f 114/61 115/63 20/60
+f 23/64 117/65 22/66
+f 22/66 117/65 118/67
+f 117/65 21/58 118/67
+f 118/67 21/58 20/60
+f 115/63 120/68 20/60
+f 20/60 120/68 118/67
+f 120/68 119/69 118/67
+f 118/67 119/69 22/66
+f 24/70 250/71 25/72
+f 25/72 250/71 121/73
+f 250/71 23/64 121/73
+f 121/73 23/64 22/66
+f 119/69 123/74 22/66
+f 22/66 123/74 121/73
+f 123/74 122/75 121/73
+f 121/73 122/75 25/72
+f 27/76 340/77 124/78
+f 124/78 340/77 341/79
+f 124/78 341/79 29/80
+f 29/80 341/79 342/81
+f 341/79 125/82 342/81
+f 342/81 125/82 28/83
+f 125/82 341/79 26/84
+f 26/84 341/79 340/77
+f 251/85 308/86 126/87
+f 126/87 308/86 30/88
+f 308/86 251/85 228/89
+f 228/89 251/85 127/90
+f 251/85 124/78 127/90
+f 127/90 124/78 29/80
+f 124/78 251/85 27/76
+f 27/76 251/85 126/87
+f 127/90 128/91 228/89
+f 228/89 128/91 46/92
+f 127/90 29/80 128/91
+f 128/91 29/80 31/93
+f 129/94 252/95 32/96
+f 32/96 252/95 309/97
+f 309/98 252/99 229/100
+f 229/100 252/99 130/101
+f 252/99 131/102 130/101
+f 130/101 131/102 34/103
+f 252/95 129/94 131/104
+f 131/104 129/94 33/105
+f 132/106 253/107 35/108
+f 35/108 253/107 310/109
+f 253/107 129/94 310/109
+f 310/109 129/94 32/96
+f 129/94 253/107 33/105
+f 33/105 253/107 133/110
+f 253/107 132/106 133/110
+f 133/110 132/106 36/111
+f 134/112 254/113 36/111
+f 36/111 254/113 133/110
+f 254/113 135/114 133/110
+f 133/110 135/114 33/105
+f 135/114 254/113 37/115
+f 37/115 254/113 157/116
+f 254/113 134/112 157/116
+f 157/116 134/112 39/117
+f 136/118 255/119 40/120
+f 40/120 255/119 137/121
+f 137/121 255/119 38/122
+f 38/122 255/119 138/123
+f 255/119 134/112 138/123
+f 138/123 134/112 36/111
+f 255/119 136/118 134/112
+f 134/112 136/118 39/117
+f 256/124 100/35 105/46
+f 105/46 100/35 12/31
+f 100/35 256/124 220/36
+f 220/36 256/124 311/125
+f 256/124 139/126 311/125
+f 311/125 139/126 1979/127
+f 139/126 256/124 17/48
+f 17/48 256/124 105/46
+f 205/128 286/129 60/130
+f 60/130 286/129 140/131
+f 286/129 204/132 140/131
+f 140/131 204/132 58/133
+f 106/47 257/134 17/48
+f 17/48 257/134 141/135
+f 257/134 136/136 141/135
+f 141/135 136/136 40/137
+f 136/136 257/134 39/138
+f 39/138 257/134 142/139
+f 257/134 106/47 142/139
+f 142/139 106/47 16/49
+f 139/126 258/140 1979/127
+f 1979/127 258/140 143/141
+f 258/140 155/142 143/141
+f 143/141 155/142 230/143
+f 155/142 258/140 40/137
+f 40/137 258/140 141/135
+f 258/140 139/126 141/135
+f 141/135 139/126 17/48
+f 285/144 203/145 144/146
+f 144/146 203/145 61/147
+f 204/132 285/144 58/133
+f 58/133 285/144 144/146
+f 57/148 2066/149 145/150
+f 145/150 2066/149 2067/151
+f 312/152 259/153 30/88
+f 30/88 259/153 126/87
+f 259/153 146/154 126/87
+f 126/87 146/154 27/76
+f 146/155 259/156 41/157
+f 41/157 259/156 147/158
+f 259/156 312/159 147/158
+f 147/158 312/159 42/160
+f 44/161 148/162 43/163
+f 43/163 148/162 149/164
+f 148/162 24/70 149/164
+f 149/164 24/70 25/72
+f 150/165 260/166 26/84
+f 26/84 260/166 125/82
+f 260/166 151/167 125/82
+f 125/82 151/167 28/83
+f 151/167 260/166 44/161
+f 44/161 260/166 148/162
+f 260/166 150/165 148/162
+f 148/162 150/165 24/70
+f 122/75 153/168 25/72
+f 25/72 153/168 149/164
+f 153/168 152/169 149/164
+f 149/164 152/169 43/163
+f 232/170 31/93 342/81
+f 342/81 31/93 29/80
+f 342/81 28/83 232/170
+f 232/170 28/83 45/171
+f 154/172 261/173 34/103
+f 34/103 261/173 130/101
+f 261/173 313/174 130/101
+f 130/101 313/174 229/100
+f 313/174 261/173 42/160
+f 42/160 261/173 147/158
+f 261/173 154/172 147/158
+f 147/158 154/172 41/157
+f 155/175 262/176 230/177
+f 230/177 262/176 314/178
+f 262/176 156/179 314/178
+f 314/178 156/179 231/180
+f 156/179 262/176 38/122
+f 38/122 262/176 137/121
+f 262/176 155/175 137/121
+f 137/121 155/175 40/120
+f 263/181 182/182 154/172
+f 154/172 182/182 41/157
+f 182/183 263/184 37/115
+f 37/115 263/184 135/114
+f 263/184 131/104 135/114
+f 135/114 131/104 33/105
+f 131/102 263/181 34/103
+f 34/103 263/181 154/172
+f 138/123 264/185 38/122
+f 38/122 264/185 156/179
+f 156/179 264/185 231/180
+f 231/180 264/185 315/186
+f 264/185 132/106 315/186
+f 315/186 132/106 35/108
+f 132/106 264/185 36/111
+f 36/111 264/185 138/123
+f 152/169 242/187 43/163
+f 43/163 242/187 158/188
+f 242/187 243/189 158/188
+f 158/188 243/189 84/190
+f 265/191 161/192 160/193
+f 160/193 161/192 45/171
+f 161/192 265/191 82/194
+f 82/194 265/191 240/195
+f 265/191 239/196 240/195
+f 240/195 239/196 83/197
+f 239/196 265/191 44/161
+f 44/161 265/191 160/193
+f 83/197 239/196 84/190
+f 84/190 239/196 158/188
+f 158/188 239/196 43/163
+f 43/163 239/196 44/161
+f 161/192 266/198 45/171
+f 45/171 266/198 232/170
+f 266/198 233/199 232/170
+f 232/170 233/199 31/93
+f 233/199 266/198 81/200
+f 81/200 266/198 238/201
+f 266/198 161/192 238/201
+f 238/201 161/192 82/194
+f 164/202 165/203 95/26
+f 95/26 165/203 10/25
+f 166/204 267/205 47/206
+f 47/206 267/205 167/207
+f 267/205 164/202 167/207
+f 167/207 164/202 95/26
+f 289/208 208/209 91/14
+f 91/14 208/209 7/13
+f 165/203 168/210 10/25
+f 10/25 168/210 13/38
+f 268/211 169/212 167/207
+f 167/207 169/212 47/206
+f 169/212 268/211 48/213
+f 48/213 268/211 170/214
+f 268/211 96/21 170/214
+f 170/214 96/21 8/23
+f 96/21 268/211 95/26
+f 95/26 268/211 167/207
+f 208/209 288/215 7/13
+f 7/13 288/215 97/28
+f 288/215 207/216 97/28
+f 97/28 207/216 11/32
+f 171/217 269/218 64/219
+f 64/219 269/218 172/220
+f 269/218 173/221 172/220
+f 172/220 173/221 59/222
+f 269/223 174/224 173/225
+f 173/225 174/224 65/226
+f 174/224 269/223 63/227
+f 63/227 269/223 171/228
+f 173/221 270/229 59/222
+f 59/222 270/229 175/230
+f 175/230 270/229 62/231
+f 62/231 270/229 176/232
+f 176/233 270/234 66/235
+f 66/235 270/234 177/236
+f 270/234 173/225 177/236
+f 177/236 173/225 65/226
+f 271/237 178/238 176/232
+f 176/232 178/238 62/231
+f 178/238 271/237 67/239
+f 67/239 271/237 179/240
+f 179/241 271/242 68/243
+f 68/243 271/242 180/244
+f 271/242 176/233 180/244
+f 180/244 176/233 66/235
+f 272/245 150/165 181/246
+f 181/246 150/165 26/84
+f 150/165 272/245 24/70
+f 24/70 272/245 250/71
+f 272/245 49/247 250/71
+f 250/71 49/247 23/64
+f 151/167 160/193 28/83
+f 28/83 160/193 45/171
+f 160/193 151/167 44/161
+f 49/247 273/248 23/64
+f 23/64 273/248 117/65
+f 273/248 50/249 117/65
+f 117/65 50/249 21/58
+f 50/249 274/250 21/58
+f 21/58 274/250 113/59
+f 113/59 274/250 19/54
+f 19/54 274/250 51/251
+f 275/252 110/53 51/251
+f 51/251 110/53 19/54
+f 110/53 275/252 14/42
+f 14/42 275/252 52/253
+f 276/254 53/255 170/214
+f 170/214 53/255 48/213
+f 276/254 103/40 52/253
+f 52/253 103/40 14/42
+f 103/40 276/254 8/23
+f 8/23 276/254 170/214
+f 207/216 287/256 11/32
+f 11/32 287/256 104/45
+f 287/256 54/257 104/45
+f 104/45 54/257 16/49
+f 54/257 277/258 16/49
+f 16/49 277/258 142/139
+f 277/258 55/259 142/139
+f 142/139 55/259 39/138
+f 278/260 56/261 157/262
+f 157/262 56/261 37/263
+f 55/259 278/260 39/138
+f 39/138 278/260 157/262
+f 183/264 182/265 56/261
+f 56/261 182/265 37/263
+f 182/265 183/264 41/266
+f 41/266 183/264 57/148
+f 284/267 184/268 203/145
+f 203/145 184/268 61/147
+f 202/269 69/270 284/267
+f 284/267 69/270 184/268
+f 145/150 146/154 57/148
+f 57/148 146/154 41/266
+f 322/271 340/77 145/150
+f 340/77 27/76 145/150
+f 145/150 27/76 146/154
+f 2070/272 2071/273 277/258
+f 277/258 2071/273 55/259
+f 291/274 209/275 279/276
+f 279/276 209/275 185/277
+f 211/278 291/274 186/279
+f 186/279 291/274 279/276
+f 2069/280 2070/272 54/257
+f 54/257 2070/272 277/258
+f 2072/281 2074/282 278/260
+f 278/260 2074/282 56/261
+f 290/283 210/284 280/285
+f 280/285 210/284 187/286
+f 209/275 290/283 185/277
+f 185/277 290/283 280/285
+f 2071/273 2072/281 55/259
+f 55/259 2072/281 278/260
+f 276/254 2076/287 53/255
+f 53/255 2076/287 2077/288
+f 213/289 2054/290 295/291
+f 295/291 2054/290 281/292
+f 295/293 281/294 215/295
+f 215/295 281/294 188/296
+f 2076/287 276/254 2075/297
+f 2075/297 276/254 52/253
+f 2073/298 2069/280 287/256
+f 287/256 2069/280 54/257
+f 293/299 211/278 2053/300
+f 2053/300 211/278 186/279
+f 275/252 2079/301 52/253
+f 52/253 2079/301 2075/297
+f 215/295 188/296 294/302
+f 294/302 188/296 2056/303
+f 294/302 2056/303 214/304
+f 214/304 2056/303 189/305
+f 2079/301 275/252 2080/306
+f 2080/306 275/252 51/251
+f 274/250 2081/307 51/251
+f 51/251 2081/307 2080/306
+f 214/304 189/305 296/308
+f 296/308 189/305 2057/309
+f 216/310 296/308 190/311
+f 190/311 296/308 2057/309
+f 2082/312 2081/307 50/249
+f 50/249 2081/307 274/250
+f 2085/313 2086/314 272/245
+f 272/245 2086/314 49/247
+f 298/315 217/316 282/317
+f 282/317 217/316 191/318
+f 218/319 298/320 192/321
+f 192/321 298/320 282/322
+f 2084/323 2085/313 181/246
+f 181/246 2085/313 272/245
+f 2083/324 2082/312 273/248
+f 273/248 2082/312 50/249
+f 297/325 216/310 2058/326
+f 2058/326 216/310 190/311
+f 217/316 297/325 191/318
+f 191/318 297/325 2058/326
+f 2086/314 2083/324 49/247
+f 49/247 2083/324 273/248
+f 183/264 2065/327 57/148
+f 57/148 2065/327 2066/149
+f 212/328 2052/329 292/330
+f 292/330 2052/329 2051/331
+f 292/330 2051/331 210/284
+f 210/284 2051/331 187/286
+f 2065/327 183/264 2074/282
+f 2074/282 183/264 56/261
+f 299/332 283/333 212/328
+f 212/328 283/333 2052/329
+f 324/334 323/335 299/332
+f 299/332 323/335 283/333
+f 71/336 193/337 58/133
+f 58/133 193/337 140/131
+f 60/130 140/131 72/338
+f 72/338 140/131 193/337
+f 70/339 194/340 61/147
+f 61/147 194/340 144/146
+f 58/133 144/146 71/336
+f 71/336 144/146 194/340
+f 195/341 171/217 74/342
+f 74/342 171/217 64/219
+f 171/228 195/343 63/227
+f 63/227 195/343 75/344
+f 72/338 206/345 60/130
+f 60/130 206/345 205/128
+f 63/227 75/344 174/224
+f 174/224 75/344 196/346
+f 174/224 196/346 65/226
+f 65/226 196/346 76/347
+f 65/226 76/347 177/236
+f 177/236 76/347 197/348
+f 66/235 177/236 77/349
+f 77/349 177/236 197/348
+f 78/350 198/351 68/243
+f 68/243 198/351 179/241
+f 67/239 179/240 79/352
+f 79/352 179/240 198/353
+f 77/349 199/354 66/235
+f 66/235 199/354 180/244
+f 68/243 180/244 78/350
+f 78/350 180/244 199/354
+f 200/355 184/268 73/356
+f 73/356 184/268 69/270
+f 70/339 61/147 200/355
+f 200/355 61/147 184/268
+f 73/356 69/270 201/357
+f 201/357 69/270 202/269
+f 201/357 202/269 339/358
+f 339/358 202/269 338/359
+f 338/359 67/239 339/358
+f 339/358 67/239 79/352
+f 284/267 337/360 202/269
+f 202/269 337/360 338/359
+f 178/238 337/360 62/231
+f 62/231 337/360 336/361
+f 335/362 336/361 285/144
+f 285/144 336/361 203/145
+f 335/362 334/363 175/230
+f 175/230 334/363 59/222
+f 333/364 334/363 286/129
+f 286/129 334/363 204/132
+f 333/364 332/365 172/220
+f 172/220 332/365 64/219
+f 332/365 205/128 331/366
+f 331/366 205/128 206/345
+f 332/365 331/366 64/219
+f 64/219 331/366 74/342
+f 330/367 293/299 2055/368
+f 2055/368 293/299 2053/300
+f 2078/369 329/370 2077/288
+f 2077/288 329/370 53/255
+f 329/370 328/371 53/255
+f 53/255 328/371 48/213
+f 327/372 328/371 288/215
+f 288/215 328/371 207/216
+f 169/212 327/372 47/206
+f 47/206 327/372 326/373
+f 325/374 326/373 289/208
+f 289/208 326/373 208/209
+f 290/283 209/275 194/340
+f 194/340 209/275 71/336
+f 210/284 290/283 70/339
+f 70/339 290/283 194/340
+f 211/278 293/299 72/338
+f 72/338 293/299 206/345
+f 209/275 291/274 71/336
+f 71/336 291/274 193/337
+f 291/274 211/278 193/337
+f 193/337 211/278 72/338
+f 200/355 292/330 70/339
+f 70/339 292/330 210/284
+f 292/330 200/355 212/328
+f 212/328 200/355 73/356
+f 293/299 330/367 206/345
+f 206/345 330/367 331/366
+f 2054/290 213/289 2055/368
+f 2055/368 213/289 330/367
+f 196/346 294/302 76/347
+f 76/347 294/302 214/304
+f 294/302 196/346 215/295
+f 215/295 196/346 75/344
+f 295/291 195/341 213/289
+f 213/289 195/341 74/342
+f 195/343 295/293 75/344
+f 75/344 295/293 215/295
+f 296/308 197/348 214/304
+f 214/304 197/348 76/347
+f 296/308 216/310 197/348
+f 197/348 216/310 77/349
+f 216/310 297/325 77/349
+f 77/349 297/325 199/354
+f 297/325 217/316 199/354
+f 199/354 217/316 78/350
+f 324/334 218/319 323/335
+f 323/335 218/319 192/321
+f 218/319 324/334 79/352
+f 79/352 324/334 339/358
+f 217/316 298/315 78/350
+f 78/350 298/315 198/351
+f 298/320 218/319 198/353
+f 198/353 218/319 79/352
+f 201/357 299/332 73/356
+f 73/356 299/332 212/328
+f 248/37 302/375 101/39
+f 101/39 302/375 221/376
+f 302/375 248/37 222/377
+f 222/377 248/37 13/38
+f 108/50 303/378 107/51
+f 107/51 303/378 223/379
+f 303/378 108/50 221/376
+f 221/376 108/50 101/39
+f 112/56 304/380 111/57
+f 111/57 304/380 224/381
+f 304/380 112/56 223/379
+f 223/379 112/56 107/51
+f 305/382 225/383 116/62
+f 116/62 225/383 115/63
+f 224/381 305/382 111/57
+f 111/57 305/382 116/62
+f 306/384 226/385 120/68
+f 120/68 226/385 119/69
+f 225/383 306/384 115/63
+f 115/63 306/384 120/68
+f 307/386 227/387 123/74
+f 123/74 227/387 122/75
+f 226/385 307/386 119/69
+f 119/69 307/386 123/74
+f 347/388 346/389 153/168
+f 153/168 346/389 152/169
+f 153/168 122/75 347/388
+f 347/388 122/75 227/387
+f 345/390 344/391 242/187
+f 242/187 344/391 243/189
+f 242/187 152/169 345/390
+f 345/390 152/169 346/389
+f 168/210 316/392 13/38
+f 13/38 316/392 222/377
+f 317/393 128/91 233/199
+f 233/199 128/91 31/93
+f 128/91 317/393 46/92
+f 46/92 317/393 163/394
+f 317/393 236/395 163/394
+f 163/394 236/395 80/396
+f 317/393 233/199 236/395
+f 236/395 233/199 81/200
+f 235/397 318/398 81/200
+f 81/200 318/398 236/395
+f 318/398 234/399 236/395
+f 236/395 234/399 80/396
+f 234/399 318/398 4/400
+f 4/400 318/398 88/401
+f 318/398 235/397 88/401
+f 88/401 235/397 3/402
+f 237/403 319/404 82/194
+f 82/194 319/404 238/201
+f 319/404 235/397 238/201
+f 238/201 235/397 81/200
+f 235/397 319/404 3/402
+f 3/402 319/404 87/405
+f 319/404 237/403 87/405
+f 87/405 237/403 2/406
+f 320/407 237/403 240/195
+f 240/195 237/403 82/194
+f 237/403 320/407 2/406
+f 2/406 320/407 86/408
+f 86/408 320/407 1/409
+f 1/409 320/407 162/410
+f 320/407 240/195 162/410
+f 162/410 240/195 83/197
+f 1/409 162/410 5/411
+f 5/411 162/410 241/412
+f 162/410 83/197 241/412
+f 241/412 83/197 84/190
+f 243/189 159/413 84/190
+f 84/190 159/413 241/412
+f 241/412 159/413 5/411
+f 5/411 159/413 85/414
+f 344/391 343/415 243/189
+f 243/189 343/415 159/413
+f 343/415 89/416 159/413
+f 159/413 89/416 85/414
+f 145/150 2067/151 322/271
+f 322/271 2067/151 2068/417
+f 322/271 181/246 340/77
+f 340/77 181/246 26/84
+f 2068/417 2084/323 322/271
+f 322/271 2084/323 181/246
+f 326/373 325/374 47/206
+f 47/206 325/374 166/204
+f 326/373 327/372 208/209
+f 208/209 327/372 288/215
+f 48/213 328/371 169/212
+f 169/212 328/371 327/372
+f 328/371 329/370 207/216
+f 207/216 329/370 287/256
+f 287/256 329/370 2073/298
+f 2073/298 329/370 2078/369
+f 331/366 330/367 74/342
+f 74/342 330/367 213/289
+f 332/365 333/364 205/128
+f 205/128 333/364 286/129
+f 334/363 333/364 59/222
+f 59/222 333/364 172/220
+f 334/363 335/362 204/132
+f 204/132 335/362 285/144
+f 62/231 336/361 175/230
+f 175/230 336/361 335/362
+f 203/145 336/361 284/267
+f 284/267 336/361 337/360
+f 67/239 338/359 178/238
+f 178/238 338/359 337/360
+f 339/358 324/334 201/357
+f 201/357 324/334 299/332
+f 300/19 219/20 2018/418
+f 2018/418 219/20 2023/419
+f 219/20 301/34 2023/419
+f 2023/419 301/34 2022/420
+f 301/34 220/36 2022/420
+f 2022/420 220/36 2024/421
+f 302/375 675/422 221/376
+f 221/376 675/422 593/423
+f 222/377 594/424 302/375
+f 302/375 594/424 675/422
+f 303/378 676/425 223/379
+f 223/379 676/425 595/426
+f 303/378 221/376 676/425
+f 676/425 221/376 593/423
+f 304/380 677/427 224/381
+f 224/381 677/427 596/428
+f 223/379 595/426 304/380
+f 304/380 595/426 677/427
+f 305/382 678/429 225/383
+f 225/383 678/429 597/430
+f 224/381 596/428 305/382
+f 305/382 596/428 678/429
+f 306/384 679/431 226/385
+f 226/385 679/431 598/432
+f 225/383 597/430 306/384
+f 306/384 597/430 679/431
+f 227/387 307/386 599/433
+f 599/433 307/386 680/434
+f 226/385 598/432 307/386
+f 307/386 598/432 680/434
+f 2038/435 681/436 2037/437
+f 2037/437 681/436 600/438
+f 2039/439 601/440 2038/435
+f 2038/435 601/440 681/436
+f 2040/441 609/442 2039/439
+f 2039/439 609/442 601/440
+f 32/96 309/97 2034/443
+f 2034/443 309/97 2020/444
+f 682/445 2020/446 603/447
+f 603/447 2020/446 2021/448
+f 35/108 310/109 2032/449
+f 2032/449 310/109 2033/450
+f 310/109 32/96 2033/450
+f 2033/450 32/96 2034/443
+f 220/36 311/125 2024/421
+f 2024/421 311/125 2025/451
+f 605/452 2026/453 685/454
+f 685/454 2026/453 2027/455
+f 2036/456 686/457 2035/458
+f 2035/458 686/457 606/459
+f 2037/437 600/438 2036/460
+f 2036/460 600/438 686/461
+f 347/388 1972/462 346/389
+f 346/389 1972/462 1971/463
+f 227/387 599/433 347/388
+f 347/388 599/433 1972/462
+f 2017/464 687/465 2021/448
+f 2021/448 687/465 603/447
+f 2035/458 606/459 2017/464
+f 2017/464 606/459 687/465
+f 230/177 314/178 2028/466
+f 2028/466 314/178 2029/467
+f 314/178 231/180 2029/467
+f 2029/467 231/180 2030/468
+f 231/180 315/186 2030/468
+f 2030/468 315/186 2031/469
+f 315/186 35/108 2031/469
+f 2031/469 35/108 2032/449
+f 345/390 1970/470 344/391
+f 344/391 1970/470 1969/471
+f 346/389 1971/463 345/390
+f 345/390 1971/463 1970/470
+f 2041/472 690/473 2040/441
+f 2040/441 690/473 609/442
+f 2042/474 613/475 2041/472
+f 2041/472 613/475 690/473
+f 316/392 691/476 222/377
+f 222/377 691/476 594/424
+f 2043/477 693/478 2042/474
+f 2042/474 693/478 613/475
+f 2044/479 435/480 2043/477
+f 2043/477 435/480 693/478
+f 89/416 343/415 436/481
+f 436/481 343/415 1968/482
+f 343/415 344/391 1968/482
+f 1968/482 344/391 1969/471
+f 437/483 354/484 623/485
+f 623/485 354/484 438/486
+f 353/487 437/483 439/488
+f 439/488 437/483 623/485
+f 440/489 353/487 624/490
+f 624/490 353/487 439/488
+f 355/491 440/489 441/492
+f 441/492 440/489 624/490
+f 357/493 442/494 356/495
+f 356/495 442/494 444/496
+f 442/494 358/497 444/496
+f 444/496 358/497 443/498
+f 354/484 437/483 445/499
+f 445/499 437/483 625/500
+f 437/483 353/487 625/500
+f 625/500 353/487 446/501
+f 360/502 447/503 446/501
+f 446/501 447/503 625/500
+f 447/503 359/504 625/500
+f 625/500 359/504 445/499
+f 353/487 440/489 446/501
+f 446/501 440/489 626/505
+f 440/489 355/491 626/505
+f 626/505 355/491 448/506
+f 361/507 449/508 448/506
+f 448/506 449/508 626/505
+f 449/508 360/502 626/505
+f 626/505 360/502 446/501
+f 358/497 442/494 362/509
+f 362/509 442/494 627/510
+f 442/494 357/493 627/510
+f 627/510 357/493 450/511
+f 364/512 451/513 363/514
+f 363/514 451/513 452/515
+f 451/513 357/493 452/515
+f 452/515 357/493 356/495
+f 359/504 447/503 453/516
+f 453/516 447/503 628/517
+f 447/503 360/502 628/517
+f 628/517 360/502 454/518
+f 366/519 455/520 454/518
+f 454/518 455/520 628/517
+f 455/520 365/521 628/517
+f 628/517 365/521 453/516
+f 357/493 451/513 450/511
+f 450/511 451/513 457/522
+f 451/513 364/512 457/522
+f 457/522 364/512 456/523
+f 458/524 364/512 459/525
+f 459/525 364/512 363/514
+f 367/526 458/524 368/527
+f 368/527 458/524 459/525
+f 364/512 458/524 456/523
+f 456/523 458/524 461/528
+f 458/524 367/526 461/528
+f 461/528 367/526 460/529
+f 370/530 369/531 462/532
+f 462/532 369/531 463/533
+f 463/533 367/526 462/532
+f 462/532 367/526 368/527
+f 367/526 463/533 460/529
+f 460/529 463/533 465/534
+f 369/531 464/535 463/533
+f 463/533 464/535 465/534
+f 372/536 371/537 466/538
+f 466/538 371/537 467/539
+f 369/531 370/530 467/539
+f 467/539 370/530 466/538
+f 464/535 369/531 469/540
+f 469/540 369/531 467/539
+f 371/537 468/541 467/539
+f 467/539 468/541 469/540
+f 373/542 374/543 629/544
+f 629/544 374/543 470/545
+f 371/537 372/536 470/545
+f 470/545 372/536 629/544
+f 468/541 371/537 472/546
+f 472/546 371/537 470/545
+f 374/543 471/547 470/545
+f 470/545 471/547 472/546
+f 376/548 473/549 1965/550
+f 1965/550 473/549 1966/551
+f 473/549 378/552 1966/551
+f 1966/551 378/552 1967/553
+f 377/554 474/555 1967/553
+f 1967/553 474/555 1966/551
+f 474/555 375/556 1966/551
+f 1966/551 375/556 1965/550
+f 379/557 476/558 475/559
+f 475/559 476/558 630/560
+f 476/558 380/561 630/560
+f 630/560 380/561 477/562
+f 378/552 473/549 477/562
+f 477/562 473/549 630/560
+f 473/549 376/548 630/560
+f 630/560 376/548 475/559
+f 477/562 380/561 478/563
+f 478/563 380/561 400/564
+f 477/562 478/563 378/552
+f 378/552 478/563 381/565
+f 479/566 382/567 631/568
+f 631/568 382/567 480/569
+f 480/570 383/571 631/572
+f 631/572 383/571 481/573
+f 385/574 482/575 481/573
+f 481/573 482/575 631/572
+f 384/576 479/566 482/577
+f 482/577 479/566 631/568
+f 483/578 386/579 632/580
+f 632/580 386/579 484/581
+f 382/567 479/566 484/581
+f 484/581 479/566 632/580
+f 479/566 384/576 632/580
+f 632/580 384/576 485/582
+f 387/583 483/578 485/582
+f 485/582 483/578 632/580
+f 486/584 387/583 633/585
+f 633/585 387/583 485/582
+f 384/576 487/586 485/582
+f 485/582 487/586 633/585
+f 487/586 388/587 633/585
+f 633/585 388/587 512/588
+f 391/589 486/584 512/588
+f 512/588 486/584 633/585
+f 488/590 392/591 634/592
+f 634/592 392/591 489/593
+f 489/593 389/594 634/592
+f 634/592 389/594 490/595
+f 387/583 486/584 490/595
+f 490/595 486/584 634/592
+f 391/589 488/590 486/584
+f 486/584 488/590 634/592
+f 360/502 449/508 454/518
+f 454/518 449/508 635/596
+f 449/508 361/507 635/596
+f 635/596 361/507 491/597
+f 390/598 492/599 491/597
+f 491/597 492/599 635/596
+f 492/599 366/519 635/596
+f 635/596 366/519 454/518
+f 576/600 406/601 660/602
+f 660/602 406/601 493/603
+f 404/604 575/605 493/603
+f 493/603 575/605 660/602
+f 455/520 366/519 636/606
+f 636/606 366/519 494/607
+f 392/608 488/609 494/607
+f 494/607 488/609 636/606
+f 488/609 391/610 636/606
+f 636/606 391/610 495/611
+f 365/521 455/520 495/611
+f 495/611 455/520 636/606
+f 492/599 390/598 637/612
+f 637/612 390/598 496/613
+f 393/614 509/615 496/613
+f 496/613 509/615 637/612
+f 509/615 392/608 637/612
+f 637/612 392/608 494/607
+f 366/519 492/599 494/607
+f 494/607 492/599 637/612
+f 407/616 574/617 497/618
+f 497/618 574/617 659/619
+f 575/605 404/604 659/619
+f 659/619 404/604 497/618
+f 2088/620 2089/621 652/622
+f 652/622 2089/621 553/623
+f 498/624 379/557 638/625
+f 638/625 379/557 475/559
+f 376/548 499/626 475/559
+f 475/559 499/626 638/625
+f 499/627 395/628 638/629
+f 638/629 395/628 500/630
+f 396/631 498/632 500/630
+f 500/630 498/632 638/629
+f 398/633 397/634 501/635
+f 501/635 397/634 502/636
+f 374/543 373/542 502/636
+f 502/636 373/542 501/635
+f 503/637 375/556 639/638
+f 639/638 375/556 474/555
+f 377/554 504/639 474/555
+f 474/555 504/639 639/638
+f 504/639 398/633 639/638
+f 639/638 398/633 501/635
+f 373/542 503/637 501/635
+f 501/635 503/637 639/638
+f 471/547 374/543 506/640
+f 506/640 374/543 502/636
+f 397/634 505/641 502/636
+f 502/636 505/641 506/640
+f 610/642 1967/553 381/565
+f 381/565 1967/553 378/552
+f 399/643 377/554 610/642
+f 610/642 377/554 1967/553
+f 507/644 385/574 640/645
+f 640/645 385/574 481/573
+f 383/571 508/646 481/573
+f 481/573 508/646 640/645
+f 508/646 396/631 640/645
+f 640/645 396/631 500/630
+f 395/628 507/644 500/630
+f 500/630 507/644 640/645
+f 509/647 393/648 641/649
+f 641/649 393/648 510/650
+f 401/651 511/652 510/650
+f 510/650 511/652 641/649
+f 511/652 389/594 641/649
+f 641/649 389/594 489/593
+f 392/591 509/647 489/593
+f 489/593 509/647 641/649
+f 507/644 395/628 642/653
+f 642/653 395/628 539/654
+f 388/587 487/586 539/655
+f 539/655 487/586 642/656
+f 384/576 482/577 487/586
+f 487/586 482/577 642/656
+f 482/575 385/574 642/653
+f 642/653 385/574 507/644
+f 389/594 511/652 490/595
+f 490/595 511/652 643/657
+f 511/652 401/651 643/657
+f 643/657 401/651 513/658
+f 386/579 483/578 513/658
+f 513/658 483/578 643/657
+f 483/578 387/583 643/657
+f 643/657 387/583 490/595
+f 505/641 397/634 621/659
+f 621/659 397/634 514/660
+f 430/661 622/662 514/660
+f 514/660 622/662 621/659
+f 399/643 517/663 516/664
+f 516/664 517/663 644/665
+f 517/663 428/666 644/665
+f 644/665 428/666 619/667
+f 619/667 429/668 644/665
+f 644/665 429/668 618/669
+f 618/669 398/633 644/665
+f 644/665 398/633 516/664
+f 429/668 430/661 618/669
+f 618/669 430/661 514/660
+f 397/634 398/633 514/660
+f 514/660 398/633 618/669
+f 517/663 399/643 645/670
+f 645/670 399/643 610/642
+f 381/565 611/671 610/642
+f 610/642 611/671 645/670
+f 611/671 427/672 645/670
+f 645/670 427/672 617/673
+f 428/666 517/663 617/673
+f 617/673 517/663 645/670
+f 358/497 521/674 443/498
+f 443/498 521/674 520/675
+f 522/676 402/677 646/678
+f 646/678 402/677 523/679
+f 443/498 520/675 523/679
+f 523/679 520/675 646/678
+f 354/484 580/680 438/486
+f 438/486 580/680 662/681
+f 521/674 358/497 524/682
+f 524/682 358/497 362/509
+f 402/677 525/683 523/679
+f 523/679 525/683 647/684
+f 525/683 403/685 647/684
+f 647/684 403/685 526/686
+f 356/495 444/496 526/686
+f 526/686 444/496 647/684
+f 444/496 443/498 647/684
+f 647/684 443/498 523/679
+f 580/680 354/484 661/687
+f 661/687 354/484 445/499
+f 359/504 579/688 445/499
+f 445/499 579/688 661/687
+f 527/689 410/690 648/691
+f 648/691 410/690 528/692
+f 405/693 529/694 528/692
+f 528/692 529/694 648/691
+f 411/695 530/696 529/697
+f 529/697 530/696 648/698
+f 530/696 409/699 648/698
+f 648/698 409/699 527/700
+f 529/694 405/693 649/701
+f 649/701 405/693 531/702
+f 408/703 532/704 531/702
+f 531/702 532/704 649/701
+f 532/705 412/706 649/707
+f 649/707 412/706 533/708
+f 533/708 411/695 649/707
+f 649/707 411/695 529/697
+f 532/704 408/703 650/709
+f 650/709 408/703 534/710
+f 534/710 413/711 650/709
+f 650/709 413/711 535/712
+f 535/713 414/714 650/715
+f 650/715 414/714 536/716
+f 412/706 532/705 536/716
+f 536/716 532/705 650/715
+f 375/556 503/637 394/717
+f 394/717 503/637 538/718
+f 503/637 373/542 538/718
+f 538/718 373/542 629/544
+f 372/536 537/719 629/544
+f 629/544 537/719 538/718
+f 377/554 399/643 504/639
+f 504/639 399/643 516/664
+f 516/664 398/633 504/639
+f 537/719 372/536 541/720
+f 541/720 372/536 466/538
+f 370/530 540/721 466/538
+f 466/538 540/721 541/720
+f 540/721 370/530 543/722
+f 543/722 370/530 462/532
+f 462/532 368/527 543/722
+f 543/722 368/527 542/723
+f 368/527 459/525 542/723
+f 542/723 459/525 545/724
+f 459/525 363/514 545/724
+f 545/724 363/514 544/725
+f 403/685 546/726 526/686
+f 526/686 546/726 547/727
+f 363/514 452/515 544/725
+f 544/725 452/515 547/727
+f 452/515 356/495 547/727
+f 547/727 356/495 526/686
+f 579/688 359/504 578/728
+f 578/728 359/504 453/516
+f 365/521 548/729 453/516
+f 453/516 548/729 578/728
+f 548/729 365/521 550/730
+f 550/730 365/521 495/611
+f 391/610 549/731 495/611
+f 495/611 549/731 550/730
+f 388/732 551/733 512/734
+f 512/734 551/733 552/735
+f 549/731 391/610 552/735
+f 552/735 391/610 512/734
+f 388/732 539/736 551/733
+f 551/733 539/736 651/737
+f 539/736 395/738 651/737
+f 651/737 395/738 553/623
+f 407/616 554/739 574/617
+f 574/617 554/739 658/740
+f 573/741 658/740 415/742
+f 415/742 658/740 554/739
+f 395/738 499/626 553/623
+f 553/623 499/626 652/622
+f 1946/743 652/622 1965/550
+f 652/622 499/626 1965/550
+f 499/626 376/548 1965/550
+f 549/731 2092/744 550/730
+f 550/730 2092/744 2093/745
+f 664/746 653/747 581/748
+f 581/748 653/747 555/749
+f 583/750 556/751 664/746
+f 664/746 556/751 653/747
+f 2094/752 548/729 2093/745
+f 2093/745 548/729 550/730
+f 551/733 2096/753 552/735
+f 552/735 2096/753 2091/754
+f 663/755 654/756 582/757
+f 582/757 654/756 557/758
+f 581/748 555/749 663/755
+f 663/755 555/749 654/756
+f 2092/744 549/731 2091/754
+f 2091/754 549/731 552/735
+f 547/727 546/726 2099/759
+f 2099/759 546/726 2098/760
+f 585/761 668/762 558/763
+f 558/763 668/762 655/764
+f 668/765 587/766 655/767
+f 655/767 587/766 559/768
+f 544/725 547/727 2100/769
+f 2100/769 547/727 2099/759
+f 548/729 2094/752 578/728
+f 578/728 2094/752 2095/770
+f 666/771 2061/772 583/750
+f 583/750 2061/772 556/751
+f 545/724 544/725 2101/773
+f 2101/773 544/725 2100/769
+f 587/766 667/774 559/768
+f 559/768 667/774 2062/775
+f 667/774 586/776 2062/775
+f 2062/775 586/776 560/777
+f 542/723 545/724 2102/778
+f 2102/778 545/724 2101/773
+f 543/722 542/723 2103/779
+f 2103/779 542/723 2102/778
+f 586/776 669/780 560/777
+f 560/777 669/780 2063/781
+f 588/782 561/783 669/780
+f 669/780 561/783 2063/781
+f 2104/784 540/721 2103/779
+f 2103/779 540/721 543/722
+f 537/719 2106/785 538/718
+f 538/718 2106/785 2107/786
+f 671/787 656/788 589/789
+f 589/789 656/788 562/790
+f 590/791 563/792 671/793
+f 671/793 563/792 656/794
+f 2108/795 394/717 2107/786
+f 2107/786 394/717 538/718
+f 540/721 2104/784 541/720
+f 541/720 2104/784 2105/796
+f 670/797 2064/798 588/782
+f 588/782 2064/798 561/783
+f 589/789 562/790 670/797
+f 670/797 562/790 2064/798
+f 2106/785 537/719 2105/796
+f 2105/796 537/719 541/720
+f 651/737 553/623 2090/799
+f 2090/799 553/623 2089/621
+f 584/800 665/801 2059/802
+f 2059/802 665/801 2060/803
+f 665/801 582/757 2060/803
+f 2060/803 582/757 557/758
+f 551/733 651/737 2096/753
+f 2096/753 651/737 2090/799
+f 672/804 584/800 657/805
+f 657/805 584/800 2059/802
+f 1948/806 672/804 1947/807
+f 1947/807 672/804 657/805
+f 417/808 404/604 564/809
+f 564/809 404/604 493/603
+f 564/809 493/603 418/810
+f 418/810 493/603 406/601
+f 416/811 407/616 565/812
+f 565/812 407/616 497/618
+f 565/812 497/618 417/808
+f 417/808 497/618 404/604
+f 566/813 420/814 527/689
+f 527/689 420/814 410/690
+f 527/700 409/699 566/815
+f 566/815 409/699 421/816
+f 418/810 406/601 577/817
+f 577/817 406/601 576/600
+f 567/818 421/816 530/696
+f 530/696 421/816 409/699
+f 530/696 411/695 567/818
+f 567/818 411/695 422/819
+f 568/820 422/819 533/708
+f 533/708 422/819 411/695
+f 568/820 533/708 423/821
+f 423/821 533/708 412/706
+f 424/822 414/714 569/823
+f 569/823 414/714 535/713
+f 569/824 535/712 425/825
+f 425/825 535/712 413/711
+f 423/821 412/706 570/826
+f 570/826 412/706 536/716
+f 570/826 536/716 424/822
+f 424/822 536/716 414/714
+f 571/827 419/828 554/739
+f 554/739 419/828 415/742
+f 554/739 407/616 571/827
+f 571/827 407/616 416/811
+f 419/828 572/829 415/742
+f 415/742 572/829 573/741
+f 572/829 1964/830 573/741
+f 573/741 1964/830 1963/831
+f 425/825 413/711 1964/830
+f 1964/830 413/711 1963/831
+f 658/740 573/741 1962/832
+f 1962/832 573/741 1963/831
+f 408/703 1961/833 534/710
+f 534/710 1961/833 1962/832
+f 574/617 1961/833 659/619
+f 659/619 1961/833 1960/834
+f 405/693 1959/835 531/702
+f 531/702 1959/835 1960/834
+f 575/605 1959/835 660/602
+f 660/602 1959/835 1958/836
+f 410/690 1957/837 528/692
+f 528/692 1957/837 1958/836
+f 577/817 576/600 1956/838
+f 1956/838 576/600 1957/837
+f 420/814 1956/838 410/690
+f 410/690 1956/838 1957/837
+f 2061/772 666/771 1954/839
+f 1954/839 666/771 1955/840
+f 546/726 1953/841 2098/760
+f 2098/760 1953/841 2097/842
+f 403/685 1952/843 546/726
+f 546/726 1952/843 1953/841
+f 579/688 1952/843 661/687
+f 661/687 1952/843 1951/844
+f 525/683 402/677 1951/844
+f 1951/844 402/677 1950/845
+f 580/680 1950/845 662/681
+f 662/681 1950/845 1949/846
+f 417/808 581/748 565/812
+f 565/812 581/748 663/755
+f 582/757 416/811 663/755
+f 663/755 416/811 565/812
+f 583/750 418/810 666/771
+f 666/771 418/810 577/817
+f 581/748 417/808 664/746
+f 664/746 417/808 564/809
+f 418/810 583/750 564/809
+f 564/809 583/750 664/746
+f 571/827 416/811 665/801
+f 665/801 416/811 582/757
+f 419/828 571/827 584/800
+f 584/800 571/827 665/801
+f 666/771 577/817 1955/840
+f 1955/840 577/817 1956/838
+f 1955/840 585/761 1954/839
+f 1954/839 585/761 558/763
+f 567/818 422/819 667/774
+f 667/774 422/819 586/776
+f 421/816 567/818 587/766
+f 587/766 567/818 667/774
+f 420/814 566/813 585/761
+f 585/761 566/813 668/762
+f 566/815 421/816 668/765
+f 668/765 421/816 587/766
+f 422/819 568/820 586/776
+f 586/776 568/820 669/780
+f 423/821 588/782 568/820
+f 568/820 588/782 669/780
+f 588/782 423/821 670/797
+f 670/797 423/821 570/826
+f 424/822 589/789 570/826
+f 570/826 589/789 670/797
+f 1948/806 1947/807 590/791
+f 590/791 1947/807 563/792
+f 590/791 425/825 1948/806
+f 1948/806 425/825 1964/830
+f 589/789 424/822 671/787
+f 671/787 424/822 569/823
+f 425/825 590/791 569/824
+f 569/824 590/791 671/793
+f 572/829 419/828 672/804
+f 672/804 419/828 584/800
+f 591/847 355/491 673/848
+f 673/848 355/491 441/492
+f 355/491 591/847 448/506
+f 448/506 591/847 674/849
+f 592/850 361/507 674/849
+f 674/849 361/507 448/506
+f 627/510 450/511 675/422
+f 675/422 450/511 593/423
+f 362/509 627/510 594/424
+f 594/424 627/510 675/422
+f 457/522 456/523 676/425
+f 676/425 456/523 595/426
+f 450/511 457/522 593/423
+f 593/423 457/522 676/425
+f 460/529 596/428 461/528
+f 461/528 596/428 677/427
+f 456/523 461/528 595/426
+f 595/426 461/528 677/427
+f 464/535 597/430 465/534
+f 465/534 597/430 678/429
+f 596/428 460/529 678/429
+f 678/429 460/529 465/534
+f 468/541 598/432 469/540
+f 469/540 598/432 679/431
+f 597/430 464/535 679/431
+f 679/431 464/535 469/540
+f 471/547 599/433 472/546
+f 472/546 599/433 680/434
+f 598/432 468/541 680/434
+f 680/434 468/541 472/546
+f 476/558 379/557 681/436
+f 681/436 379/557 600/438
+f 380/561 476/558 601/440
+f 601/440 476/558 681/436
+f 380/561 601/440 400/564
+f 400/564 601/440 609/442
+f 382/567 602/851 480/569
+f 480/569 602/851 682/852
+f 603/447 383/571 682/445
+f 682/445 383/571 480/570
+f 386/579 604/853 484/581
+f 484/581 604/853 683/854
+f 602/851 382/567 683/854
+f 683/854 382/567 484/581
+f 361/507 592/850 491/597
+f 491/597 592/850 684/855
+f 605/452 390/598 684/855
+f 684/855 390/598 491/597
+f 390/598 605/452 496/613
+f 496/613 605/452 685/454
+f 607/856 393/614 685/454
+f 685/454 393/614 496/613
+f 396/631 606/459 498/632
+f 498/632 606/459 686/457
+f 600/438 379/557 686/461
+f 686/461 379/557 498/624
+f 505/641 1971/463 506/640
+f 506/640 1971/463 1972/462
+f 506/640 1972/462 471/547
+f 471/547 1972/462 599/433
+f 508/646 383/571 687/465
+f 687/465 383/571 603/447
+f 396/631 508/646 606/459
+f 606/459 508/646 687/465
+f 393/648 607/857 510/650
+f 510/650 607/857 688/858
+f 608/859 401/651 688/858
+f 688/858 401/651 510/650
+f 401/651 608/859 513/658
+f 513/658 608/859 689/860
+f 604/853 386/579 689/860
+f 689/860 386/579 513/658
+f 622/662 1969/471 621/659
+f 621/659 1969/471 1970/470
+f 621/659 1970/470 505/641
+f 505/641 1970/470 1971/463
+f 400/564 609/442 519/861
+f 519/861 609/442 690/473
+f 613/475 426/862 690/473
+f 690/473 426/862 519/861
+f 524/682 362/509 691/476
+f 691/476 362/509 594/424
+f 381/565 478/563 611/671
+f 611/671 478/563 692/863
+f 478/563 400/564 692/863
+f 692/863 400/564 519/861
+f 426/862 615/864 519/861
+f 519/861 615/864 692/863
+f 615/864 427/672 692/863
+f 692/863 427/672 611/671
+f 426/862 613/475 612/865
+f 612/865 613/475 693/478
+f 351/866 612/865 435/480
+f 435/480 612/865 693/478
+f 427/672 615/864 614/867
+f 614/867 615/864 694/868
+f 615/864 426/862 694/868
+f 694/868 426/862 612/865
+f 351/866 434/869 612/865
+f 612/865 434/869 694/868
+f 350/870 614/867 434/869
+f 434/869 614/867 694/868
+f 616/871 428/666 695/872
+f 695/872 428/666 617/673
+f 617/673 427/672 695/872
+f 695/872 427/672 614/867
+f 350/870 433/873 614/867
+f 614/867 433/873 695/872
+f 433/873 349/874 695/872
+f 695/872 349/874 616/871
+f 619/667 428/666 696/875
+f 696/875 428/666 616/871
+f 349/874 432/876 616/871
+f 616/871 432/876 696/875
+f 432/876 348/877 696/875
+f 696/875 348/877 518/878
+f 429/668 619/667 518/878
+f 518/878 619/667 696/875
+f 348/877 352/879 518/878
+f 518/878 352/879 620/880
+f 430/661 429/668 620/880
+f 620/880 429/668 518/878
+f 622/662 430/661 515/881
+f 515/881 430/661 620/880
+f 352/879 431/882 620/880
+f 620/880 431/882 515/881
+f 1969/471 622/662 1968/482
+f 1968/482 622/662 515/881
+f 431/882 436/481 515/881
+f 515/881 436/481 1968/482
+f 698/883 711/884 697/885
+f 697/885 711/884 710/886
+f 699/887 712/888 698/883
+f 698/883 712/888 711/884
+f 700/889 713/890 699/887
+f 699/887 713/890 712/888
+f 701/891 714/892 700/889
+f 700/889 714/892 713/890
+f 702/893 715/894 701/891
+f 701/891 715/894 714/892
+f 703/895 716/896 702/893
+f 702/893 716/896 715/894
+f 704/897 717/898 703/895
+f 703/895 717/898 716/896
+f 705/899 718/900 704/897
+f 704/897 718/900 717/898
+f 705/899 706/901 718/900
+f 718/900 706/901 719/902
+f 706/901 707/903 719/902
+f 719/902 707/903 720/904
+f 708/905 721/906 707/903
+f 707/903 721/906 720/904
+f 1332/907 722/908 708/905
+f 708/905 722/908 721/906
+f 709/909 723/910 1981/911
+f 1981/911 723/910 1980/912
+f 711/884 725/913 710/886
+f 710/886 725/913 724/914
+f 712/888 726/915 711/884
+f 711/884 726/915 725/913
+f 713/890 727/916 712/888
+f 712/888 727/916 726/915
+f 714/892 728/917 713/890
+f 713/890 728/917 727/916
+f 715/894 729/918 714/892
+f 714/892 729/918 728/917
+f 716/896 730/919 715/894
+f 715/894 730/919 729/918
+f 717/898 731/920 716/896
+f 716/896 731/920 730/919
+f 718/900 732/921 717/898
+f 717/898 732/921 731/920
+f 718/900 719/902 732/921
+f 732/921 719/902 733/922
+f 719/902 720/904 733/922
+f 733/922 720/904 734/923
+f 721/906 735/924 720/904
+f 720/904 735/924 734/923
+f 1334/925 722/908 1983/926
+f 1983/926 722/908 1980/912
+f 725/913 738/927 724/914
+f 724/914 738/927 737/928
+f 726/915 739/929 725/913
+f 725/913 739/929 738/927
+f 727/916 740/930 726/915
+f 726/915 740/930 739/929
+f 728/917 741/931 727/916
+f 727/916 741/931 740/930
+f 729/918 742/932 728/917
+f 728/917 742/932 741/931
+f 730/919 743/933 729/918
+f 729/918 743/933 742/932
+f 731/920 744/934 730/919
+f 730/919 744/934 743/933
+f 732/921 745/935 731/920
+f 731/920 745/935 744/934
+f 732/921 733/922 745/935
+f 745/935 733/922 746/936
+f 734/923 747/937 733/922
+f 733/922 747/937 746/936
+f 735/924 748/938 734/923
+f 734/923 748/938 747/937
+f 1335/939 1334/925 1984/940
+f 1984/940 1334/925 1983/926
+f 737/928 738/927 750/941
+f 750/941 738/927 751/942
+f 739/929 752/943 738/927
+f 738/927 752/943 751/942
+f 740/930 753/944 739/929
+f 739/929 753/944 752/943
+f 741/931 754/945 740/930
+f 740/930 754/945 753/944
+f 742/932 755/946 741/931
+f 741/931 755/946 754/945
+f 743/933 756/947 742/932
+f 742/932 756/947 755/946
+f 744/934 757/948 743/933
+f 743/933 757/948 756/947
+f 745/935 758/949 744/934
+f 744/934 758/949 757/948
+f 745/935 746/936 758/949
+f 758/949 746/936 759/950
+f 747/937 760/951 746/936
+f 746/936 760/951 759/950
+f 748/938 761/952 747/937
+f 747/937 761/952 760/951
+f 1336/953 1335/939 1985/954
+f 1985/954 1335/939 1984/940
+f 750/941 751/942 763/955
+f 763/955 751/942 764/956
+f 752/943 765/957 751/942
+f 751/942 765/957 764/956
+f 753/944 766/958 752/943
+f 752/943 766/958 765/957
+f 754/945 767/959 753/944
+f 753/944 767/959 766/958
+f 755/946 768/960 754/945
+f 754/945 768/960 767/959
+f 756/947 769/961 755/946
+f 755/946 769/961 768/960
+f 757/948 770/962 756/947
+f 756/947 770/962 769/961
+f 758/949 771/963 757/948
+f 757/948 771/963 770/962
+f 758/949 759/950 771/963
+f 771/963 759/950 772/964
+f 760/951 773/965 759/950
+f 759/950 773/965 772/964
+f 761/952 774/966 760/951
+f 760/951 774/966 773/965
+f 1337/967 1336/953 1982/968
+f 1982/968 1336/953 1985/954
+f 763/955 764/956 776/969
+f 776/969 764/956 777/970
+f 765/957 778/971 764/956
+f 764/956 778/971 777/970
+f 766/958 779/972 765/957
+f 765/957 779/972 778/971
+f 767/959 780/973 766/958
+f 766/958 780/973 779/972
+f 768/960 781/974 767/959
+f 767/959 781/974 780/973
+f 769/961 782/975 768/960
+f 768/960 782/975 781/974
+f 770/962 783/976 769/961
+f 769/961 783/976 782/975
+f 771/963 784/977 770/962
+f 770/962 784/977 783/976
+f 771/963 772/964 784/977
+f 784/977 772/964 785/978
+f 772/964 773/965 785/978
+f 785/978 773/965 786/979
+f 774/966 787/980 773/965
+f 773/965 787/980 786/979
+f 3/402 1337/967 88/401
+f 88/401 1337/967 1982/968
+f 776/969 777/970 788/981
+f 788/981 777/970 789/982
+f 778/971 790/983 777/970
+f 777/970 790/983 789/982
+f 779/972 791/984 778/971
+f 778/971 791/984 790/983
+f 780/973 792/985 779/972
+f 779/972 792/985 791/984
+f 781/974 793/986 780/973
+f 780/973 793/986 792/985
+f 782/975 794/987 781/974
+f 781/974 794/987 793/986
+f 783/976 795/988 782/975
+f 782/975 795/988 794/987
+f 784/977 796/989 783/976
+f 783/976 796/989 795/988
+f 784/977 785/978 796/989
+f 796/989 785/978 797/990
+f 785/978 786/979 797/990
+f 797/990 786/979 798/991
+f 787/980 799/992 786/979
+f 786/979 799/992 798/991
+f 788/981 789/982 800/993
+f 800/993 789/982 801/994
+f 790/983 802/995 789/982
+f 789/982 802/995 801/994
+f 791/984 803/996 790/983
+f 790/983 803/996 802/995
+f 792/985 804/997 791/984
+f 791/984 804/997 803/996
+f 793/986 805/998 792/985
+f 792/985 805/998 804/997
+f 794/987 806/999 793/986
+f 793/986 806/999 805/998
+f 795/988 807/1000 794/987
+f 794/987 807/1000 806/999
+f 796/989 808/1001 795/988
+f 795/988 808/1001 807/1000
+f 796/989 797/990 808/1001
+f 808/1001 797/990 809/1002
+f 797/990 798/991 809/1002
+f 809/1002 798/991 810/1003
+f 799/992 811/1004 798/991
+f 798/991 811/1004 810/1003
+f 800/993 801/994 812/1005
+f 812/1005 801/994 813/1006
+f 802/995 814/1007 801/994
+f 801/994 814/1007 813/1006
+f 803/996 815/1008 802/995
+f 802/995 815/1008 814/1007
+f 804/997 816/1009 803/996
+f 803/996 816/1009 815/1008
+f 805/998 817/1010 804/997
+f 804/997 817/1010 816/1009
+f 806/999 818/1011 805/998
+f 805/998 818/1011 817/1010
+f 807/1000 819/1012 806/999
+f 806/999 819/1012 818/1011
+f 808/1001 820/1013 807/1000
+f 807/1000 820/1013 819/1012
+f 808/1001 809/1002 820/1013
+f 820/1013 809/1002 821/1014
+f 809/1002 810/1003 821/1014
+f 821/1014 810/1003 822/1015
+f 811/1004 823/1016 810/1003
+f 810/1003 823/1016 822/1015
+f 812/1005 813/1006 824/1017
+f 824/1017 813/1006 825/1018
+f 814/1007 826/1019 813/1006
+f 813/1006 826/1019 825/1018
+f 815/1008 827/1020 814/1007
+f 814/1007 827/1020 826/1019
+f 816/1009 828/1021 815/1008
+f 815/1008 828/1021 827/1020
+f 817/1010 829/1022 816/1009
+f 816/1009 829/1022 828/1021
+f 818/1011 830/1023 817/1010
+f 817/1010 830/1023 829/1022
+f 819/1012 831/1024 818/1011
+f 818/1011 831/1024 830/1023
+f 819/1012 820/1013 831/1024
+f 831/1024 820/1013 832/1025
+f 820/1013 821/1014 832/1025
+f 832/1025 821/1014 833/1026
+f 821/1014 822/1015 833/1026
+f 833/1026 822/1015 834/1027
+f 822/1015 823/1016 834/1027
+f 834/1027 823/1016 835/1028
+f 824/1017 825/1018 847/1029
+f 847/1029 825/1018 836/1030
+f 826/1019 837/1031 825/1018
+f 825/1018 837/1031 836/1030
+f 827/1020 838/1032 826/1019
+f 826/1019 838/1032 837/1031
+f 828/1021 839/1033 827/1020
+f 827/1020 839/1033 838/1032
+f 829/1022 840/1034 828/1021
+f 828/1021 840/1034 839/1033
+f 830/1023 841/1035 829/1022
+f 829/1022 841/1035 840/1034
+f 831/1024 842/1036 830/1023
+f 830/1023 842/1036 841/1035
+f 831/1024 832/1025 842/1036
+f 842/1036 832/1025 843/1037
+f 832/1025 833/1026 843/1037
+f 843/1037 833/1026 844/1038
+f 834/1027 845/1039 833/1026
+f 833/1026 845/1039 844/1038
+f 834/1027 835/1028 845/1039
+f 845/1039 835/1028 846/1040
+f 847/1029 836/1030 848/1041
+f 848/1041 836/1030 849/1042
+f 836/1030 837/1031 849/1042
+f 849/1042 837/1031 850/1043
+f 837/1031 838/1032 850/1043
+f 850/1043 838/1032 851/1044
+f 838/1032 839/1033 851/1044
+f 851/1044 839/1033 852/1045
+f 840/1034 853/1046 839/1033
+f 839/1033 853/1046 852/1045
+f 841/1035 854/1047 840/1034
+f 840/1034 854/1047 853/1046
+f 842/1036 855/1048 841/1035
+f 841/1035 855/1048 854/1047
+f 842/1036 843/1037 855/1048
+f 855/1048 843/1037 856/1049
+f 844/1038 857/1050 843/1037
+f 843/1037 857/1050 856/1049
+f 845/1039 858/1051 844/1038
+f 844/1038 858/1051 857/1050
+f 846/1040 859/1052 845/1039
+f 845/1039 859/1052 858/1051
+f 848/1041 849/1042 860/1053
+f 860/1053 849/1042 861/1054
+f 849/1042 850/1043 861/1054
+f 861/1054 850/1043 862/1055
+f 850/1043 851/1044 862/1055
+f 862/1055 851/1044 863/1056
+f 854/1047 855/1048 864/1057
+f 856/1049 865/1058 855/1048
+f 855/1048 865/1058 864/1057
+f 857/1050 866/1059 856/1049
+f 856/1049 866/1059 865/1058
+f 858/1051 867/1060 857/1050
+f 857/1050 867/1060 866/1059
+f 859/1052 868/1061 858/1051
+f 858/1051 868/1061 867/1060
+f 860/1053 861/1054 869/1062
+f 869/1062 861/1054 870/1063
+f 861/1054 862/1055 870/1063
+f 870/1063 862/1055 871/1064
+f 869/1062 870/1063 872/1065
+f 872/1065 870/1063 873/1066
+f 871/1064 874/1067 870/1063
+f 870/1063 874/1067 873/1066
+f 876/1068 875/1069 866/1059
+f 866/1059 875/1069 865/1058
+f 867/1060 877/1070 866/1059
+f 866/1059 877/1070 876/1068
+f 868/1061 878/1071 867/1060
+f 867/1060 878/1071 877/1070
+f 872/1065 873/1066 879/1072
+f 879/1072 873/1066 880/1073
+f 874/1067 881/1074 873/1066
+f 873/1066 881/1074 880/1073
+f 882/1075 875/1069 883/1076
+f 883/1076 875/1069 876/1068
+f 876/1068 877/1070 883/1076
+f 883/1076 877/1070 884/1077
+f 878/1071 885/1078 877/1070
+f 877/1070 885/1078 884/1077
+f 879/1072 880/1073 886/1079
+f 886/1079 880/1073 887/1080
+f 880/1073 881/1074 887/1080
+f 887/1080 881/1074 888/1081
+f 890/1082 889/1083 883/1076
+f 883/1076 889/1083 882/1075
+f 883/1076 884/1077 890/1082
+f 890/1082 884/1077 891/1084
+f 885/1078 892/1085 884/1077
+f 884/1077 892/1085 891/1084
+f 886/1079 887/1080 893/1086
+f 893/1086 887/1080 894/1087
+f 887/1080 888/1081 894/1087
+f 894/1087 888/1081 895/1088
+f 889/1083 890/1082 1314/1089
+f 1314/1089 890/1082 896/1090
+f 890/1082 891/1084 896/1090
+f 896/1090 891/1084 897/1091
+f 891/1084 892/1085 897/1091
+f 897/1091 892/1085 1315/1092
+f 892/1085 1338/1093 1315/1092
+f 1315/1092 1338/1093 898/1094
+f 1986/1095 1338/1093 1987/1096
+f 1987/1096 1338/1093 321/1097
+f 893/1086 894/1087 899/1098
+f 899/1098 894/1087 900/1099
+f 894/1087 895/1088 900/1099
+f 900/1099 895/1088 901/1100
+f 1313/1101 1312/1102 906/1103
+f 906/1103 1312/1102 907/1104
+f 698/883 697/885 908/1105
+f 699/887 698/883 908/1105
+f 700/889 699/887 908/1105
+f 701/891 700/889 908/1105
+f 702/893 701/891 908/1105
+f 703/895 702/893 908/1105
+f 704/897 703/895 908/1105
+f 705/899 704/897 908/1105
+f 706/901 705/899 908/1105
+f 707/903 706/901 908/1105
+f 708/905 707/903 908/1105
+f 708/905 908/1105 1332/907
+f 709/909 1981/911 908/1105
+f 942/1106 981/1107 1008/1108
+f 1008/1108 981/1107 1045/1109
+f 1008/1108 1029/1110 942/1106
+f 942/1106 1029/1110 957/1111
+f 943/1112 959/1113 1008/1108
+f 1008/1108 959/1113 1029/1110
+f 1008/1108 1045/1109 943/1112
+f 943/1112 1045/1109 982/1114
+f 944/1115 980/1116 1009/1117
+f 1009/1117 980/1116 1044/1118
+f 1009/1117 1030/1119 944/1115
+f 944/1115 1030/1119 956/1120
+f 942/1106 957/1111 1009/1117
+f 1009/1117 957/1111 1030/1119
+f 1009/1117 1044/1118 942/1106
+f 942/1106 1044/1118 981/1107
+f 945/1121 979/1122 1010/1123
+f 1010/1123 979/1122 1043/1124
+f 1010/1123 1028/1125 945/1121
+f 945/1121 1028/1125 954/1126
+f 944/1115 956/1120 1010/1123
+f 1010/1123 956/1120 1028/1125
+f 1010/1123 1043/1124 944/1115
+f 944/1115 1043/1124 980/1116
+f 978/1127 1042/1128 946/1129
+f 946/1129 1042/1128 1011/1130
+f 1011/1130 1031/1131 946/1129
+f 946/1129 1031/1131 961/1132
+f 945/1121 954/1126 1011/1130
+f 1011/1130 954/1126 1031/1131
+f 1011/1130 1042/1128 945/1121
+f 945/1121 1042/1128 979/1122
+f 1033/1133 965/1134 1012/1135
+f 1012/1135 965/1134 947/1136
+f 961/1132 1033/1133 946/1129
+f 946/1129 1033/1133 1012/1135
+f 1012/1135 947/1136 1068/1137
+f 1068/1137 947/1136 1067/1138
+f 977/1139 1041/1140 948/1141
+f 948/1141 1041/1140 1013/1142
+f 1035/1143 969/1144 1013/1142
+f 1013/1142 969/1144 948/1141
+f 965/1134 1035/1143 947/1136
+f 947/1136 1035/1143 1013/1142
+f 1041/1140 1067/1138 1013/1142
+f 1013/1142 1067/1138 947/1136
+f 976/1145 1040/1146 949/1147
+f 949/1147 1040/1146 1014/1148
+f 1037/1149 972/1150 1014/1148
+f 1014/1148 972/1150 949/1147
+f 969/1144 1037/1149 948/1141
+f 948/1141 1037/1149 1014/1148
+f 1040/1146 977/1139 1014/1148
+f 1014/1148 977/1139 948/1141
+f 974/1151 1039/1152 950/1153
+f 950/1153 1039/1152 1015/1154
+f 1036/1155 968/1156 1015/1154
+f 1015/1154 968/1156 950/1153
+f 972/1150 1036/1155 949/1147
+f 949/1147 1036/1155 1015/1154
+f 1039/1152 976/1145 1015/1154
+f 1015/1154 976/1145 949/1147
+f 975/1157 1038/1158 951/1159
+f 951/1159 1038/1158 1016/1160
+f 1034/1161 964/1162 1016/1160
+f 1016/1160 964/1162 951/1159
+f 968/1156 1034/1161 950/1153
+f 950/1153 1034/1161 1016/1160
+f 1038/1158 974/1151 1016/1160
+f 1016/1160 974/1151 950/1153
+f 943/1112 982/1114 1017/1163
+f 1017/1163 982/1114 1046/1164
+f 1017/1163 1032/1165 943/1112
+f 943/1112 1032/1165 959/1113
+f 951/1159 964/1162 1017/1163
+f 1017/1163 964/1162 1032/1165
+f 1017/1163 1046/1164 951/1159
+f 951/1159 1046/1164 975/1157
+f 1039/1152 974/1151 1045/1109
+f 1045/1109 974/1151 982/1114
+f 987/1166 1049/1167 1907/1168
+f 1907/1168 1049/1167 1908/1169
+f 924/1170 1018/1171 1909/1172
+f 1909/1172 1018/1171 1908/1169
+f 991/1173 1051/1174 1914/1175
+f 1914/1175 1051/1174 1912/1176
+f 926/1177 1019/1178 1910/1179
+f 1910/1179 1019/1178 1912/1176
+f 910/1180 1052/1181 1910/1179
+f 1910/1179 1052/1181 1906/1182
+f 952/1183 1020/1184 1907/1168
+f 1907/1168 1020/1184 1906/1182
+f 1053/1185 1911/1186 909/1187
+f 909/1187 1911/1186 1909/1172
+f 925/1188 1021/1189 1913/1190
+f 1913/1190 1021/1189 1911/1186
+f 911/1191 1054/1192 1913/1190
+f 1913/1190 1054/1192 1915/1193
+f 1022/1194 1915/1193 927/1195
+f 927/1195 1915/1193 1917/1196
+f 1023/1197 1919/1198 928/1199
+f 928/1199 1919/1198 1921/1200
+f 1047/1201 1919/1198 983/1202
+f 983/1202 1919/1198 1917/1196
+f 1048/1203 1923/1204 985/1205
+f 985/1205 1923/1204 1921/1200
+f 1024/1206 1923/1204 930/1207
+f 930/1207 1923/1204 1925/1208
+f 1055/1209 1924/1210 912/1211
+f 912/1211 1924/1210 1925/1208
+f 1025/1212 1924/1210 931/1213
+f 931/1213 1924/1210 1922/1214
+f 1050/1215 1920/1216 989/1217
+f 989/1217 1920/1216 1922/1214
+f 1026/1218 1920/1216 929/1219
+f 929/1219 1920/1216 1918/1220
+f 1056/1221 1916/1222 913/1223
+f 913/1223 1916/1222 1918/1220
+f 1027/1224 1916/1222 953/1225
+f 953/1225 1916/1222 1914/1175
+f 1028/1125 955/1226 954/1126
+f 954/1126 955/1226 915/1227
+f 955/1226 1028/1125 914/1228
+f 914/1228 1028/1125 956/1120
+f 1029/1110 958/1229 957/1111
+f 957/1111 958/1229 917/1230
+f 958/1229 1029/1110 916/1231
+f 916/1231 1029/1110 959/1113
+f 1030/1119 960/1232 956/1120
+f 956/1120 960/1232 914/1228
+f 960/1232 1030/1119 917/1230
+f 917/1230 1030/1119 957/1111
+f 1031/1131 962/1233 961/1132
+f 961/1132 962/1233 918/1234
+f 962/1233 1031/1131 915/1227
+f 915/1227 1031/1131 954/1126
+f 1032/1165 963/1235 959/1113
+f 959/1113 963/1235 916/1231
+f 963/1235 1032/1165 919/1236
+f 919/1236 1032/1165 964/1162
+f 965/1134 1033/1133 920/1237
+f 920/1237 1033/1133 966/1238
+f 1033/1133 961/1132 966/1238
+f 966/1238 961/1132 918/1234
+f 964/1162 1034/1161 919/1236
+f 919/1236 1034/1161 967/1239
+f 1034/1161 968/1156 967/1239
+f 967/1239 968/1156 921/1240
+f 969/1144 1035/1143 922/1241
+f 922/1241 1035/1143 970/1242
+f 1035/1143 965/1134 970/1242
+f 970/1242 965/1134 920/1237
+f 968/1156 1036/1155 921/1240
+f 921/1240 1036/1155 971/1243
+f 1036/1155 972/1150 971/1243
+f 971/1243 972/1150 923/1244
+f 972/1150 1037/1149 923/1244
+f 923/1244 1037/1149 973/1245
+f 1037/1149 969/1144 973/1245
+f 973/1245 969/1144 922/1241
+f 955/1246 1021/1189 915/1247
+f 915/1247 1021/1189 925/1188
+f 1021/1189 955/1246 924/1170
+f 924/1170 955/1246 914/1248
+f 958/1249 1020/1184 917/1250
+f 917/1250 1020/1184 952/1183
+f 926/1177 1020/1184 916/1251
+f 916/1251 1020/1184 958/1249
+f 924/1170 914/1248 1018/1171
+f 1018/1171 914/1248 960/1252
+f 917/1250 952/1183 960/1252
+f 960/1252 952/1183 1018/1171
+f 962/1253 1022/1194 918/1254
+f 918/1254 1022/1194 927/1195
+f 915/1247 925/1188 962/1253
+f 962/1253 925/1188 1022/1194
+f 916/1251 963/1255 926/1177
+f 926/1177 963/1255 1019/1178
+f 953/1225 1019/1178 919/1256
+f 919/1256 1019/1178 963/1255
+f 966/1257 1023/1197 920/1258
+f 920/1258 1023/1197 928/1199
+f 918/1254 927/1195 966/1257
+f 966/1257 927/1195 1023/1197
+f 919/1256 967/1259 953/1225
+f 953/1225 967/1259 1027/1224
+f 929/1219 1027/1224 921/1260
+f 921/1260 1027/1224 967/1259
+f 970/1261 1024/1206 922/1262
+f 922/1262 1024/1206 930/1207
+f 920/1258 928/1199 970/1261
+f 970/1261 928/1199 1024/1206
+f 921/1260 971/1263 929/1219
+f 929/1219 971/1263 1026/1218
+f 931/1213 1026/1218 923/1264
+f 923/1264 1026/1218 971/1263
+f 931/1213 923/1264 1025/1212
+f 1025/1212 923/1264 973/1265
+f 922/1262 930/1207 973/1265
+f 973/1265 930/1207 1025/1212
+f 1047/1201 997/1266 985/1205
+f 985/1205 997/1266 933/1267
+f 1047/1201 983/1202 997/1266
+f 997/1266 983/1202 932/1268
+f 1048/1203 984/1269 912/1211
+f 912/1211 984/1269 934/1270
+f 984/1269 1048/1203 933/1267
+f 933/1267 1048/1203 985/1205
+f 909/1187 1049/1167 936/1271
+f 936/1271 1049/1167 986/1272
+f 1049/1167 987/1166 986/1272
+f 986/1272 987/1166 935/1273
+f 1050/1215 988/1274 913/1223
+f 913/1223 988/1274 938/1275
+f 988/1274 1050/1215 937/1276
+f 937/1276 1050/1215 989/1217
+f 910/1180 1051/1174 940/1277
+f 940/1277 1051/1174 990/1278
+f 990/1278 1051/1174 939/1279
+f 939/1279 1051/1174 991/1173
+f 987/1166 1052/1181 935/1273
+f 935/1273 1052/1181 992/1280
+f 1052/1181 910/1180 992/1280
+f 992/1280 910/1180 940/1277
+f 911/1191 1053/1185 941/1281
+f 941/1281 1053/1185 993/1282
+f 1053/1185 909/1187 993/1282
+f 993/1282 909/1187 936/1271
+f 983/1202 1054/1192 932/1268
+f 932/1268 1054/1192 994/1283
+f 1054/1192 911/1191 994/1283
+f 994/1283 911/1191 941/1281
+f 1055/1209 995/1284 989/1217
+f 989/1217 995/1284 937/1276
+f 995/1284 1055/1209 934/1270
+f 934/1270 1055/1209 912/1211
+f 1056/1221 996/1285 991/1173
+f 991/1173 996/1285 939/1279
+f 996/1285 1056/1221 938/1275
+f 938/1275 1056/1221 913/1223
+f 1057/1286 998/1287 997/1266
+f 997/1266 998/1287 933/1267
+f 999/1288 1057/1286 932/1268
+f 932/1268 1057/1286 997/1266
+f 984/1269 1058/1289 934/1270
+f 934/1270 1058/1289 1000/1290
+f 1058/1289 984/1269 998/1287
+f 998/1287 984/1269 933/1267
+f 1059/1291 1001/1292 986/1272
+f 986/1272 1001/1292 936/1271
+f 1002/1293 1059/1291 935/1273
+f 935/1273 1059/1291 986/1272
+f 988/1274 1060/1294 938/1275
+f 938/1275 1060/1294 1003/1295
+f 1060/1294 988/1274 1004/1296
+f 1004/1296 988/1274 937/1276
+f 990/1278 1061/1297 940/1277
+f 940/1277 1061/1297 1005/1298
+f 1061/1297 990/1278 1006/1299
+f 1006/1299 990/1278 939/1279
+f 1062/1300 1002/1293 992/1280
+f 992/1280 1002/1293 935/1273
+f 1005/1298 1062/1300 940/1277
+f 940/1277 1062/1300 992/1280
+f 1063/1301 1007/1302 993/1282
+f 993/1282 1007/1302 941/1281
+f 1001/1292 1063/1301 936/1271
+f 936/1271 1063/1301 993/1282
+f 1064/1303 999/1288 994/1283
+f 994/1283 999/1288 932/1268
+f 1007/1302 1064/1303 941/1281
+f 941/1281 1064/1303 994/1283
+f 995/1284 1065/1304 937/1276
+f 937/1276 1065/1304 1004/1296
+f 1065/1304 995/1284 1000/1290
+f 1000/1290 995/1284 934/1270
+f 996/1285 1066/1305 939/1279
+f 939/1279 1066/1305 1006/1299
+f 1066/1305 996/1285 1003/1295
+f 1003/1295 996/1285 938/1275
+f 1038/1158 975/1157 974/1151
+f 974/1151 975/1157 982/1114
+f 975/1157 1046/1164 982/1114
+f 981/1107 1044/1118 976/1145
+f 976/1145 1044/1118 1040/1146
+f 1045/1109 981/1107 1039/1152
+f 1039/1152 981/1107 976/1145
+f 1044/1118 980/1116 1040/1146
+f 1040/1146 980/1116 977/1139
+f 1042/1128 978/1127 979/1122
+f 1067/1138 979/1122 1068/1137
+f 979/1122 978/1127 1068/1137
+f 1043/1124 1041/1140 980/1116
+f 980/1116 1041/1140 977/1139
+f 1068/1137 978/1127 1012/1135
+f 1012/1135 978/1127 946/1129
+f 979/1122 1067/1138 1043/1124
+f 1043/1124 1067/1138 1041/1140
+f 998/1287 1057/1286 904/1306
+f 904/1306 1057/1286 905/1307
+f 1057/1286 999/1288 905/1307
+f 905/1307 999/1288 1314/1089
+f 1058/1289 903/1308 1000/1290
+f 1000/1290 903/1308 902/1309
+f 1059/1291 864/1057 1001/1292
+f 1001/1292 864/1057 865/1058
+f 1059/1291 1002/1293 864/1057
+f 864/1057 1002/1293 854/1047
+f 1060/1294 888/1081 1003/1295
+f 1003/1295 888/1081 881/1074
+f 1004/1296 895/1088 1060/1294
+f 1060/1294 895/1088 888/1081
+f 1005/1298 1061/1297 852/1045
+f 852/1045 1061/1297 863/1056
+f 1061/1297 1006/1299 863/1056
+f 863/1056 1006/1299 871/1064
+f 1062/1300 853/1046 1002/1293
+f 1002/1293 853/1046 854/1047
+f 1062/1300 1005/1298 853/1046
+f 853/1046 1005/1298 852/1045
+f 1007/1302 1063/1301 882/1075
+f 882/1075 1063/1301 875/1069
+f 1063/1301 1001/1292 875/1069
+f 875/1069 1001/1292 865/1058
+f 1064/1303 1007/1302 889/1083
+f 889/1083 1007/1302 882/1075
+f 1004/1296 1065/1304 895/1088
+f 895/1088 1065/1304 901/1100
+f 1000/1290 902/1309 1065/1304
+f 1065/1304 902/1309 901/1100
+f 1006/1299 1066/1305 871/1064
+f 871/1064 1066/1305 874/1067
+f 1066/1305 1003/1295 874/1067
+f 874/1067 1003/1295 881/1074
+f 998/1287 904/1306 1058/1289
+f 1058/1289 904/1306 903/1308
+f 871/1064 862/1055 863/1056
+f 863/1056 851/1044 852/1045
+f 1064/1303 889/1083 999/1288
+f 999/1288 889/1083 1314/1089
+f 1117/1310 1238/1311 1118/1312
+f 1238/1311 1153/1313 1152/1314
+f 1152/1314 1153/1313 1118/1312
+f 1154/1315 1239/1316 1071/1317
+f 1071/1317 1239/1316 1093/1318
+f 1239/1316 1154/1315 1309/1319
+f 1309/1319 1154/1315 1310/1320
+f 1240/1321 1311/1322 1154/1315
+f 1154/1315 1311/1322 1310/1320
+f 1111/1323 1288/1324 1094/1325
+f 1094/1325 1288/1324 1240/1321
+f 1240/1321 1154/1315 1094/1325
+f 1094/1325 1154/1315 1071/1317
+f 1119/1326 1155/1327 1220/1328
+f 1220/1328 1155/1327 1298/1329
+f 1116/1330 1298/1329 1114/1331
+f 1114/1331 1298/1329 1155/1327
+f 1113/1332 1151/1333 1121/1334
+f 1121/1334 1151/1333 1156/1335
+f 1072/1336 1156/1335 1070/1337
+f 1070/1337 1156/1335 1151/1333
+f 1155/1327 1241/1338 1114/1331
+f 1114/1331 1241/1338 1157/1339
+f 1241/1338 1278/1340 1157/1339
+f 1157/1339 1278/1340 1115/1341
+f 1278/1340 1241/1338 1223/1342
+f 1223/1342 1241/1338 1158/1343
+f 1241/1338 1155/1327 1158/1343
+f 1158/1343 1155/1327 1119/1326
+f 1122/1344 1159/1345 1219/1346
+f 1219/1346 1159/1345 1275/1347
+f 1220/1328 1275/1347 1119/1326
+f 1119/1326 1275/1347 1159/1345
+f 1160/1348 1242/1349 1124/1350
+f 1124/1350 1242/1349 1161/1351
+f 1242/1349 1246/1352 1161/1351
+f 1161/1351 1246/1352 1125/1353
+f 1246/1352 1242/1349 1162/1354
+f 1162/1354 1242/1349 1123/1355
+f 1123/1355 1242/1349 1160/1348
+f 1163/1356 1243/1357 1121/1334
+f 1121/1334 1243/1357 1120/1358
+f 1243/1357 1163/1356 1221/1359
+f 1221/1359 1163/1356 1126/1360
+f 1163/1356 1244/1361 1126/1360
+f 1126/1360 1244/1361 1164/1362
+f 1244/1361 1095/1363 1164/1362
+f 1164/1362 1095/1363 1073/1364
+f 1095/1363 1244/1361 1072/1336
+f 1072/1336 1244/1361 1156/1335
+f 1244/1361 1163/1356 1156/1335
+f 1156/1335 1163/1356 1121/1334
+f 1279/1365 1245/1366 1224/1367
+f 1224/1367 1245/1366 1165/1368
+f 1165/1368 1245/1366 1122/1344
+f 1122/1344 1245/1366 1159/1345
+f 1245/1366 1158/1343 1159/1345
+f 1159/1345 1158/1343 1119/1326
+f 1245/1366 1279/1365 1158/1343
+f 1158/1343 1279/1365 1223/1342
+f 1125/1353 1246/1352 1127/1369
+f 1127/1369 1246/1352 1166/1370
+f 1166/1370 1246/1352 1167/1371
+f 1130/1372 1168/1373 1209/1374
+f 1209/1374 1168/1373 1129/1375
+f 1168/1373 1130/1372 1210/1376
+f 1210/1376 1130/1372 1297/1377
+f 1169/1378 1247/1379 1074/1380
+f 1074/1380 1247/1379 1096/1381
+f 1096/1381 1247/1379 1075/1382
+f 1075/1382 1247/1379 1267/1383
+f 1247/1379 1168/1373 1267/1383
+f 1267/1383 1168/1373 1210/1376
+f 1247/1379 1169/1378 1168/1373
+f 1168/1373 1169/1378 1129/1375
+f 1229/1384 1248/1385 1132/1386
+f 1132/1386 1248/1385 1171/1387
+f 1248/1385 1170/1388 1171/1387
+f 1171/1387 1170/1388 1131/1389
+f 1249/1390 1173/1391 1172/1392
+f 1172/1392 1173/1391 1133/1393
+f 1249/1390 1238/1311 1173/1391
+f 1173/1391 1238/1311 1117/1310
+f 1302/1394 1175/1395 1172/1392
+f 1172/1392 1175/1395 1249/1390
+f 1249/1390 1174/1396 1238/1311
+f 1238/1311 1174/1396 1153/1313
+f 1174/1396 1249/1390 1134/1397
+f 1134/1397 1249/1390 1175/1395
+f 1176/1398 1250/1399 1307/1400
+f 1307/1400 1250/1399 1308/1401
+f 1250/1399 1239/1316 1308/1401
+f 1308/1401 1239/1316 1309/1319
+f 1239/1316 1250/1399 1093/1318
+f 1093/1318 1250/1399 1097/1402
+f 1250/1399 1176/1398 1097/1402
+f 1097/1402 1176/1398 1076/1403
+f 1219/1346 1274/1404 1122/1344
+f 1122/1344 1274/1404 1177/1405
+f 1136/1406 1177/1405 1135/1407
+f 1135/1407 1177/1405 1274/1404
+f 1178/1408 1251/1409 1137/1410
+f 1137/1410 1251/1409 1179/1411
+f 1251/1409 1180/1412 1179/1411
+f 1179/1411 1180/1412 1138/1413
+f 1180/1412 1251/1409 1133/1393
+f 1133/1393 1251/1409 1172/1392
+f 1252/1414 1178/1408 1181/1415
+f 1181/1415 1178/1408 1137/1410
+f 1175/1395 1302/1394 1236/1416
+f 1182/1417 1236/1416 1302/1394
+f 1182/1417 1252/1414 1127/1369
+f 1127/1369 1252/1414 1183/1418
+f 1252/1414 1181/1415 1183/1418
+f 1183/1418 1181/1415 1139/1419
+f 1253/1420 1180/1412 1184/1421
+f 1184/1421 1180/1412 1133/1393
+f 1180/1412 1253/1420 1138/1413
+f 1138/1413 1253/1420 1185/1422
+f 1253/1420 1287/1423 1185/1422
+f 1185/1422 1287/1423 1233/1424
+f 1287/1423 1253/1420 1234/1425
+f 1234/1425 1253/1420 1184/1421
+f 1218/1426 1273/1427 1140/1428
+f 1140/1428 1273/1427 1186/1429
+f 1141/1430 1186/1429 1217/1431
+f 1217/1431 1186/1429 1273/1427
+f 1187/1432 1254/1433 1143/1434
+f 1143/1434 1254/1433 1188/1435
+f 1254/1433 1189/1436 1188/1435
+f 1188/1435 1189/1436 1145/1437
+f 1189/1436 1254/1433 1144/1438
+f 1144/1438 1254/1433 1190/1439
+f 1254/1433 1187/1432 1190/1439
+f 1190/1439 1187/1432 1142/1440
+f 1191/1441 1255/1442 1146/1443
+f 1146/1443 1255/1442 1192/1444
+f 1255/1442 1193/1445 1192/1444
+f 1192/1444 1193/1445 1131/1389
+f 1255/1442 1187/1432 1193/1445
+f 1193/1445 1187/1432 1143/1434
+f 1187/1432 1255/1442 1142/1440
+f 1142/1440 1255/1442 1191/1441
+f 1194/1446 1256/1447 1226/1448
+f 1226/1448 1256/1447 1282/1449
+f 1256/1447 1195/1450 1282/1449
+f 1282/1449 1195/1450 1227/1451
+f 1195/1450 1256/1447 1141/1430
+f 1141/1430 1256/1447 1186/1429
+f 1256/1447 1194/1446 1186/1429
+f 1186/1429 1194/1446 1140/1428
+f 1257/1452 1189/1436 1196/1453
+f 1196/1453 1189/1436 1144/1438
+f 1189/1436 1257/1452 1145/1437
+f 1145/1437 1257/1452 1197/1454
+f 1257/1452 1285/1455 1197/1454
+f 1197/1454 1285/1455 1231/1456
+f 1285/1455 1257/1452 1232/1457
+f 1232/1457 1257/1452 1196/1453
+f 1217/1431 1272/1458 1141/1430
+f 1141/1430 1272/1458 1195/1450
+f 1228/1459 1227/1451 1272/1458
+f 1272/1458 1227/1451 1195/1450
+f 1258/1460 1284/1461 1198/1462
+f 1198/1462 1284/1461 1230/1463
+f 1284/1461 1258/1460 1231/1456
+f 1231/1456 1258/1460 1197/1454
+f 1197/1454 1258/1460 1145/1437
+f 1145/1437 1258/1460 1188/1435
+f 1258/1460 1198/1462 1188/1435
+f 1188/1435 1198/1462 1143/1434
+f 1199/1464 1259/1465 1126/1360
+f 1126/1360 1259/1465 1221/1359
+f 1259/1465 1199/1464 1222/1466
+f 1222/1466 1199/1464 1147/1467
+f 1260/1468 1200/1469 1201/1470
+f 1201/1470 1200/1469 1148/1471
+f 1175/1395 1261/1472 1134/1397
+f 1134/1397 1261/1472 1202/1473
+f 1305/1474 1306/1475 1303/1476
+f 1303/1476 1306/1475 1202/1473
+f 1199/1464 1262/1477 1147/1467
+f 1147/1467 1262/1477 1204/1478
+f 1262/1477 1098/1479 1204/1478
+f 1204/1478 1098/1479 1077/1480
+f 1098/1479 1262/1477 1073/1364
+f 1073/1364 1262/1477 1164/1362
+f 1262/1477 1199/1464 1164/1362
+f 1164/1362 1199/1464 1126/1360
+f 1263/1481 1176/1398 1306/1475
+f 1306/1475 1176/1398 1307/1400
+f 1176/1398 1263/1481 1076/1403
+f 1076/1403 1263/1481 1099/1482
+f 1263/1481 1205/1483 1099/1482
+f 1099/1482 1205/1483 1078/1484
+f 1205/1483 1263/1481 1305/1474
+f 1305/1474 1263/1481 1306/1475
+f 1149/1485 1277/1486 1147/1467
+f 1147/1467 1277/1486 1222/1466
+f 1200/1469 1207/1487 1148/1471
+f 1148/1471 1207/1487 1206/1488
+f 1304/1489 1305/1474 1266/1490
+f 1266/1490 1305/1474 1303/1476
+f 1079/1491 1264/1492 1077/1480
+f 1077/1480 1264/1492 1204/1478
+f 1264/1492 1149/1485 1204/1478
+f 1204/1478 1149/1485 1147/1467
+f 1205/1483 1265/1493 1078/1484
+f 1078/1484 1265/1493 1100/1494
+f 1265/1493 1205/1483 1304/1489
+f 1304/1489 1205/1483 1305/1474
+f 1207/1487 1130/1372 1206/1488
+f 1206/1488 1130/1372 1209/1374
+f 1075/1382 1267/1383 1079/1491
+f 1079/1491 1267/1383 1264/1492
+f 1267/1383 1210/1376 1264/1492
+f 1264/1492 1210/1376 1149/1485
+f 1268/1495 1211/1496 1280/1497
+f 1280/1497 1211/1496 1225/1498
+f 1211/1496 1268/1495 1136/1406
+f 1136/1406 1268/1495 1177/1405
+f 1268/1495 1165/1368 1177/1405
+f 1177/1405 1165/1368 1122/1344
+f 1165/1368 1268/1495 1224/1367
+f 1224/1367 1268/1495 1280/1497
+f 1286/1499 1269/1500 1233/1424
+f 1233/1424 1269/1500 1185/1422
+f 1269/1500 1212/1501 1185/1422
+f 1185/1422 1212/1501 1138/1413
+f 1212/1501 1269/1500 1144/1438
+f 1144/1438 1269/1500 1196/1453
+f 1269/1500 1286/1499 1196/1453
+f 1196/1453 1286/1499 1232/1457
+f 1150/1502 1270/1503 1146/1443
+f 1146/1443 1270/1503 1213/1504
+f 1270/1503 1214/1505 1213/1504
+f 1213/1504 1214/1505 1139/1419
+f 1135/1407 1301/1506 1136/1406
+f 1136/1406 1301/1506 1215/1507
+f 1140/1428 1215/1507 1218/1426
+f 1218/1426 1215/1507 1301/1506
+f 1216/1508 1271/1509 1142/1440
+f 1142/1440 1271/1509 1190/1439
+f 1271/1509 1212/1501 1190/1439
+f 1190/1439 1212/1501 1144/1438
+f 1212/1501 1271/1509 1138/1413
+f 1138/1413 1271/1509 1179/1411
+f 1271/1509 1216/1508 1179/1411
+f 1179/1411 1216/1508 1137/1410
+f 1228/1459 1272/1458 1229/1384
+f 1229/1384 1272/1458 1248/1385
+f 1272/1458 1217/1431 1248/1385
+f 1248/1385 1217/1431 1170/1388
+f 1217/1431 1273/1427 1170/1388
+f 1170/1388 1273/1427 1294/1510
+f 1273/1427 1218/1426 1294/1510
+f 1294/1510 1218/1426 1150/1502
+f 1135/1407 1274/1404 1214/1505
+f 1214/1505 1274/1404 1291/1511
+f 1274/1404 1219/1346 1291/1511
+f 1291/1511 1219/1346 1125/1353
+f 1219/1346 1275/1347 1125/1353
+f 1125/1353 1275/1347 1161/1351
+f 1275/1347 1220/1328 1161/1351
+f 1161/1351 1220/1328 1124/1350
+f 1276/1512 1160/1348 1116/1330
+f 1116/1330 1160/1348 1124/1350
+f 1331/1513 1162/1354 1123/1355
+f 1260/1468 1259/1465 1200/1469
+f 1200/1469 1259/1465 1222/1466
+f 1277/1486 1207/1487 1222/1466
+f 1222/1466 1207/1487 1200/1469
+f 1297/1377 1130/1372 1277/1486
+f 1277/1486 1130/1372 1207/1487
+f 1115/1341 1278/1340 1069/1514
+f 1069/1514 1278/1340 1101/1515
+f 1101/1515 1278/1340 1080/1516
+f 1080/1516 1278/1340 1223/1342
+f 1279/1365 1102/1517 1223/1342
+f 1223/1342 1102/1517 1080/1516
+f 1102/1517 1279/1365 1081/1518
+f 1081/1518 1279/1365 1224/1367
+f 1280/1497 1103/1519 1224/1367
+f 1224/1367 1103/1519 1081/1518
+f 1280/1497 1225/1498 1103/1519
+f 1103/1519 1225/1498 1082/1520
+f 1225/1498 1281/1521 1082/1520
+f 1082/1520 1281/1521 1104/1522
+f 1281/1521 1226/1448 1104/1522
+f 1104/1522 1226/1448 1083/1523
+f 1226/1448 1282/1449 1083/1523
+f 1083/1523 1282/1449 1105/1524
+f 1282/1449 1227/1451 1105/1524
+f 1105/1524 1227/1451 1084/1525
+f 1227/1451 1228/1459 1084/1525
+f 1084/1525 1228/1459 1085/1526
+f 1228/1459 1229/1384 1085/1526
+f 1085/1526 1229/1384 1086/1527
+f 1283/1528 1106/1529 1132/1386
+f 1132/1386 1106/1529 1088/1530
+f 1106/1529 1283/1528 1087/1531
+f 1087/1531 1283/1528 1230/1463
+f 1107/1532 1284/1461 1089/1533
+f 1089/1533 1284/1461 1231/1456
+f 1284/1461 1107/1532 1230/1463
+f 1230/1463 1107/1532 1087/1531
+f 1285/1455 1108/1534 1231/1456
+f 1231/1456 1108/1534 1089/1533
+f 1108/1534 1285/1455 1090/1535
+f 1090/1535 1285/1455 1232/1457
+f 1109/1536 1286/1499 1091/1537
+f 1091/1537 1286/1499 1233/1424
+f 1286/1499 1109/1536 1232/1457
+f 1232/1457 1109/1536 1090/1535
+f 1287/1423 1110/1538 1233/1424
+f 1233/1424 1110/1538 1091/1537
+f 1110/1538 1287/1423 1092/1539
+f 1092/1539 1287/1423 1234/1425
+f 1288/1324 1111/1323 1234/1425
+f 1234/1425 1111/1323 1092/1539
+f 1169/1378 1289/1540 1129/1375
+f 1129/1375 1289/1540 1235/1541
+f 1289/1540 1265/1493 1235/1541
+f 1235/1541 1265/1493 1304/1489
+f 1265/1493 1289/1540 1100/1494
+f 1100/1494 1289/1540 1112/1542
+f 1289/1540 1169/1378 1112/1542
+f 1112/1542 1169/1378 1074/1380
+f 1086/1527 1229/1384 1088/1530
+f 1088/1530 1229/1384 1132/1386
+f 1290/1543 1236/1416 1201/1470
+f 1201/1470 1236/1416 1128/1544
+f 1236/1416 1290/1543 1175/1395
+f 1175/1395 1290/1543 1261/1472
+f 1290/1543 1237/1545 1261/1472
+f 1261/1472 1237/1545 1203/1546
+f 1237/1545 1290/1543 1148/1471
+f 1148/1471 1290/1543 1201/1470
+f 1214/1505 1291/1511 1139/1419
+f 1139/1419 1291/1511 1183/1418
+f 1183/1418 1291/1511 1127/1369
+f 1127/1369 1291/1511 1125/1353
+f 1292/1547 1237/1545 1206/1488
+f 1206/1488 1237/1545 1148/1471
+f 1237/1545 1292/1547 1203/1546
+f 1203/1546 1292/1547 1208/1548
+f 1216/1508 1293/1549 1137/1410
+f 1137/1410 1293/1549 1181/1415
+f 1181/1415 1293/1549 1139/1419
+f 1139/1419 1293/1549 1213/1504
+f 1293/1549 1191/1441 1213/1504
+f 1213/1504 1191/1441 1146/1443
+f 1293/1549 1216/1508 1191/1441
+f 1191/1441 1216/1508 1142/1440
+f 1295/1550 1292/1547 1209/1374
+f 1209/1374 1292/1547 1206/1488
+f 1292/1547 1295/1550 1208/1548
+f 1208/1548 1295/1550 1266/1490
+f 1170/1388 1294/1510 1131/1389
+f 1131/1389 1294/1510 1192/1444
+f 1294/1510 1150/1502 1192/1444
+f 1192/1444 1150/1502 1146/1443
+f 1235/1541 1295/1550 1129/1375
+f 1129/1375 1295/1550 1209/1374
+f 1283/1528 1296/1551 1230/1463
+f 1230/1463 1296/1551 1198/1462
+f 1296/1551 1193/1445 1198/1462
+f 1198/1462 1193/1445 1143/1434
+f 1193/1445 1296/1551 1131/1389
+f 1131/1389 1296/1551 1171/1387
+f 1296/1551 1283/1528 1171/1387
+f 1171/1387 1283/1528 1132/1386
+f 1210/1376 1297/1377 1149/1485
+f 1149/1485 1297/1377 1277/1486
+f 1220/1328 1298/1329 1124/1350
+f 1124/1350 1298/1329 1116/1330
+f 1127/1369 1166/1370 1182/1417
+f 1166/1370 1128/1544 1182/1417
+f 1236/1416 1182/1417 1128/1544
+f 1299/1552 1288/1324 1184/1421
+f 1184/1421 1288/1324 1234/1425
+f 1288/1324 1299/1552 1240/1321
+f 1240/1321 1299/1552 1311/1322
+f 1173/1391 1299/1552 1133/1393
+f 1133/1393 1299/1552 1184/1421
+f 1211/1496 1300/1553 1225/1498
+f 1225/1498 1300/1553 1281/1521
+f 1300/1553 1194/1446 1281/1521
+f 1281/1521 1194/1446 1226/1448
+f 1194/1446 1300/1553 1140/1428
+f 1140/1428 1300/1553 1215/1507
+f 1300/1553 1211/1496 1215/1507
+f 1215/1507 1211/1496 1136/1406
+f 1218/1426 1301/1506 1150/1502
+f 1150/1502 1301/1506 1270/1503
+f 1301/1506 1135/1407 1270/1503
+f 1270/1503 1135/1407 1214/1505
+f 1302/1394 1178/1408 1182/1417
+f 1182/1417 1178/1408 1252/1414
+f 1251/1409 1178/1408 1172/1392
+f 1172/1392 1178/1408 1302/1394
+f 1203/1546 1208/1548 1303/1476
+f 1303/1476 1208/1548 1266/1490
+f 1261/1472 1203/1546 1202/1473
+f 1202/1473 1203/1546 1303/1476
+f 1266/1490 1295/1550 1304/1489
+f 1304/1489 1295/1550 1235/1541
+f 1306/1475 1307/1400 1202/1473
+f 1202/1473 1307/1400 1134/1397
+f 1308/1401 1174/1396 1307/1400
+f 1307/1400 1174/1396 1134/1397
+f 1174/1396 1308/1401 1153/1313
+f 1153/1313 1308/1401 1309/1319
+f 1309/1319 1310/1320 1153/1313
+f 1153/1313 1310/1320 1118/1312
+f 1311/1322 1117/1310 1310/1320
+f 1310/1320 1117/1310 1118/1312
+f 1299/1552 1173/1391 1311/1322
+f 1311/1322 1173/1391 1117/1310
+f 1320/1554 1319/1555 1069/1556
+f 1069/1556 1319/1555 1115/1557
+f 1322/1558 1319/1555 903/1308
+f 903/1308 1319/1555 907/1104
+f 903/1308 907/1104 902/1309
+f 902/1309 907/1104 1312/1102
+f 900/1099 901/1100 1312/1102
+f 1312/1102 901/1100 902/1309
+f 899/1098 900/1099 1313/1101
+f 1313/1101 900/1099 1312/1102
+f 1323/1559 1324/1560 1114/1561
+f 1114/1561 1324/1560 1116/1562
+f 1325/1563 1324/1560 1314/1089
+f 1314/1089 1324/1560 905/1307
+f 1326/1564 1327/1565 1317/1566
+f 1317/1566 1327/1565 1318/1567
+f 1321/1568 1333/1569 1316/1570
+f 1316/1570 1333/1569 898/1094
+f 1323/1559 1322/1558 904/1306
+f 904/1306 1322/1558 903/1308
+f 1326/1564 1325/1563 896/1090
+f 896/1090 1325/1563 1314/1089
+f 1276/1512 1317/1571 1160/1348
+f 1160/1348 1317/1571 1123/1355
+f 1121/1334 1120/1358 1113/1332
+f 1113/1332 1120/1358 1318/1572
+f 1328/1573 1327/1565 1315/1092
+f 1315/1092 1327/1565 897/1091
+f 1120/1358 1123/1355 1318/1572
+f 1318/1572 1123/1355 1317/1571
+f 1319/1555 1320/1554 907/1104
+f 907/1104 1320/1554 906/1103
+f 1333/1569 1321/1568 1151/1574
+f 1151/1574 1321/1568 1070/1575
+f 1115/1557 1319/1555 1157/1576
+f 1157/1576 1319/1555 1322/1558
+f 1157/1576 1322/1558 1114/1561
+f 1114/1561 1322/1558 1323/1559
+f 905/1307 1324/1560 904/1306
+f 904/1306 1324/1560 1323/1559
+f 1324/1560 1325/1563 1116/1562
+f 1116/1562 1325/1563 1276/1577
+f 1325/1563 1326/1564 1276/1577
+f 1276/1577 1326/1564 1317/1566
+f 1327/1565 1326/1564 897/1091
+f 897/1091 1326/1564 896/1090
+f 1327/1565 1328/1573 1318/1567
+f 1318/1567 1328/1573 1113/1578
+f 1328/1573 1333/1569 1113/1578
+f 1113/1578 1333/1569 1151/1574
+f 1329/1579 1260/1468 1128/1544
+f 1128/1544 1260/1468 1201/1470
+f 1259/1465 1260/1468 1221/1359
+f 1221/1359 1260/1468 1329/1579
+f 1243/1357 1221/1359 1330/1580
+f 1330/1580 1221/1359 1329/1579
+f 1329/1579 1128/1544 1167/1371
+f 1167/1371 1128/1544 1166/1370
+f 1330/1580 1329/1579 1331/1513
+f 1331/1513 1329/1579 1167/1371
+f 1330/1580 1331/1513 1120/1358
+f 1120/1358 1331/1513 1123/1355
+f 1120/1358 1243/1357 1330/1580
+f 1331/1513 1167/1371 1162/1354
+f 1162/1354 1167/1371 1246/1352
+f 1315/1092 898/1094 1328/1573
+f 1328/1573 898/1094 1333/1569
+f 722/908 1334/925 721/906
+f 721/906 1334/925 735/924
+f 1334/925 1335/939 735/924
+f 735/924 1335/939 748/938
+f 1335/939 1336/953 748/938
+f 748/938 1336/953 761/952
+f 1336/953 1337/967 761/952
+f 761/952 1337/967 774/966
+f 774/966 1337/967 787/980
+f 787/980 1337/967 3/402
+f 3/402 87/405 787/980
+f 787/980 87/405 799/992
+f 885/1078 321/1097 892/1085
+f 892/1085 321/1097 1338/1093
+f 710/886 1350/1581 697/885
+f 697/885 1350/1581 1339/1582
+f 1350/1581 1351/1583 1339/1582
+f 1339/1582 1351/1583 1340/1584
+f 1351/1583 1352/1585 1340/1584
+f 1340/1584 1352/1585 1341/1586
+f 1352/1585 1353/1587 1341/1586
+f 1341/1586 1353/1587 1342/1588
+f 1353/1587 1354/1589 1342/1588
+f 1342/1588 1354/1589 1343/1590
+f 1354/1589 1355/1591 1343/1590
+f 1343/1590 1355/1591 1344/1592
+f 1355/1591 1356/1593 1344/1592
+f 1344/1592 1356/1593 1345/1594
+f 1356/1593 1357/1595 1345/1594
+f 1345/1594 1357/1595 1346/1596
+f 1346/1596 1357/1595 1347/1597
+f 1347/1597 1357/1595 1358/1598
+f 1347/1597 1358/1598 1348/1599
+f 1348/1599 1358/1598 1359/1600
+f 1359/1600 1360/1601 1348/1599
+f 1348/1599 1360/1601 1349/1602
+f 1360/1601 1361/1603 1349/1602
+f 1349/1602 1361/1603 1898/1604
+f 1361/1603 1973/1605 1898/1604
+f 1898/1604 1973/1605 1974/1606
+f 724/914 1362/1607 710/886
+f 710/886 1362/1607 1350/1581
+f 1362/1607 1363/1608 1350/1581
+f 1350/1581 1363/1608 1351/1583
+f 1363/1608 1364/1609 1351/1583
+f 1351/1583 1364/1609 1352/1585
+f 1364/1609 1365/1610 1352/1585
+f 1352/1585 1365/1610 1353/1587
+f 1365/1610 1366/1611 1353/1587
+f 1353/1587 1366/1611 1354/1589
+f 1366/1611 1367/1612 1354/1589
+f 1354/1589 1367/1612 1355/1591
+f 1367/1612 1368/1613 1355/1591
+f 1355/1591 1368/1613 1356/1593
+f 1368/1613 1369/1614 1356/1593
+f 1356/1593 1369/1614 1357/1595
+f 1357/1595 1369/1614 1358/1598
+f 1358/1598 1369/1614 1370/1615
+f 1358/1598 1370/1615 1359/1600
+f 1359/1600 1370/1615 1371/1616
+f 1371/1616 1372/1617 1359/1600
+f 1359/1600 1372/1617 1360/1601
+f 1900/1618 1976/1619 1361/1603
+f 1361/1603 1976/1619 1973/1605
+f 737/928 1373/1620 724/914
+f 724/914 1373/1620 1362/1607
+f 1373/1620 1374/1621 1362/1607
+f 1362/1607 1374/1621 1363/1608
+f 1374/1621 1375/1622 1363/1608
+f 1363/1608 1375/1622 1364/1609
+f 1375/1622 1376/1623 1364/1609
+f 1364/1609 1376/1623 1365/1610
+f 1376/1623 1377/1624 1365/1610
+f 1365/1610 1377/1624 1366/1611
+f 1377/1624 1378/1625 1366/1611
+f 1366/1611 1378/1625 1367/1612
+f 1378/1625 1379/1626 1367/1612
+f 1367/1612 1379/1626 1368/1613
+f 1379/1626 1380/1627 1368/1613
+f 1368/1613 1380/1627 1369/1614
+f 1369/1614 1380/1627 1370/1615
+f 1370/1615 1380/1627 1381/1628
+f 1370/1615 1381/1628 1371/1616
+f 1371/1616 1381/1628 1382/1629
+f 1371/1616 1382/1629 1372/1617
+f 1372/1617 1382/1629 1383/1630
+f 1901/1631 1977/1632 1900/1618
+f 1900/1618 1977/1632 1976/1619
+f 737/928 750/941 1373/1620
+f 1373/1620 750/941 1384/1633
+f 1384/1633 1385/1634 1373/1620
+f 1373/1620 1385/1634 1374/1621
+f 1385/1634 1386/1635 1374/1621
+f 1374/1621 1386/1635 1375/1622
+f 1386/1635 1387/1636 1375/1622
+f 1375/1622 1387/1636 1376/1623
+f 1387/1636 1388/1637 1376/1623
+f 1376/1623 1388/1637 1377/1624
+f 1388/1637 1389/1638 1377/1624
+f 1377/1624 1389/1638 1378/1625
+f 1389/1638 1390/1639 1378/1625
+f 1378/1625 1390/1639 1379/1626
+f 1390/1639 1391/1640 1379/1626
+f 1379/1626 1391/1640 1380/1627
+f 1380/1627 1391/1640 1381/1628
+f 1381/1628 1391/1640 1392/1641
+f 1392/1641 1393/1642 1381/1628
+f 1381/1628 1393/1642 1382/1629
+f 1382/1629 1393/1642 1383/1630
+f 1383/1630 1393/1642 1394/1643
+f 1902/1644 1978/1645 1901/1631
+f 1901/1631 1978/1645 1977/1632
+f 750/941 763/955 1384/1633
+f 1384/1633 763/955 1395/1646
+f 1395/1646 1396/1647 1384/1633
+f 1384/1633 1396/1647 1385/1634
+f 1396/1647 1397/1648 1385/1634
+f 1385/1634 1397/1648 1386/1635
+f 1397/1648 1398/1649 1386/1635
+f 1386/1635 1398/1649 1387/1636
+f 1398/1649 1399/1650 1387/1636
+f 1387/1636 1399/1650 1388/1637
+f 1399/1650 1400/1651 1388/1637
+f 1388/1637 1400/1651 1389/1638
+f 1400/1651 1401/1652 1389/1638
+f 1389/1638 1401/1652 1390/1639
+f 1401/1652 1402/1653 1390/1639
+f 1390/1639 1402/1653 1391/1640
+f 1391/1640 1402/1653 1392/1641
+f 1392/1641 1402/1653 1403/1654
+f 1403/1654 1404/1655 1392/1641
+f 1392/1641 1404/1655 1393/1642
+f 1404/1655 1405/1656 1393/1642
+f 1393/1642 1405/1656 1394/1643
+f 1903/1657 1975/1658 1902/1644
+f 1902/1644 1975/1658 1978/1645
+f 763/955 776/969 1395/1646
+f 1395/1646 776/969 1406/1659
+f 1406/1659 1407/1660 1395/1646
+f 1395/1646 1407/1660 1396/1647
+f 1407/1660 1408/1661 1396/1647
+f 1396/1647 1408/1661 1397/1648
+f 1408/1661 1409/1662 1397/1648
+f 1397/1648 1409/1662 1398/1649
+f 1409/1662 1410/1663 1398/1649
+f 1398/1649 1410/1663 1399/1650
+f 1410/1663 1411/1664 1399/1650
+f 1399/1650 1411/1664 1400/1651
+f 1411/1664 1412/1665 1400/1651
+f 1400/1651 1412/1665 1401/1652
+f 1412/1665 1413/1666 1401/1652
+f 1401/1652 1413/1666 1402/1653
+f 1402/1653 1413/1666 1403/1654
+f 1403/1654 1413/1666 1414/1667
+f 1414/1667 1415/1668 1403/1654
+f 1403/1654 1415/1668 1404/1655
+f 1415/1668 1416/1669 1404/1655
+f 1404/1655 1416/1669 1405/1656
+f 2045/1670 1975/1658 2044/479
+f 2044/479 1975/1658 435/480
+f 776/969 788/981 1406/1659
+f 1406/1659 788/981 1417/1671
+f 1417/1671 1418/1672 1406/1659
+f 1406/1659 1418/1672 1407/1660
+f 1418/1672 1419/1673 1407/1660
+f 1407/1660 1419/1673 1408/1661
+f 1419/1673 1420/1674 1408/1661
+f 1408/1661 1420/1674 1409/1662
+f 1420/1674 1421/1675 1409/1662
+f 1409/1662 1421/1675 1410/1663
+f 1421/1675 1422/1676 1410/1663
+f 1410/1663 1422/1676 1411/1664
+f 1422/1676 1423/1677 1411/1664
+f 1411/1664 1423/1677 1412/1665
+f 1423/1677 1424/1678 1412/1665
+f 1412/1665 1424/1678 1413/1666
+f 1413/1666 1424/1678 1414/1667
+f 1414/1667 1424/1678 1425/1679
+f 1414/1667 1425/1679 1415/1668
+f 1415/1668 1425/1679 1426/1680
+f 1426/1680 1427/1681 1415/1668
+f 1415/1668 1427/1681 1416/1669
+f 788/981 800/993 1417/1671
+f 1417/1671 800/993 1428/1682
+f 1428/1682 1429/1683 1417/1671
+f 1417/1671 1429/1683 1418/1672
+f 1429/1683 1430/1684 1418/1672
+f 1418/1672 1430/1684 1419/1673
+f 1430/1684 1431/1685 1419/1673
+f 1419/1673 1431/1685 1420/1674
+f 1431/1685 1432/1686 1420/1674
+f 1420/1674 1432/1686 1421/1675
+f 1432/1686 1433/1687 1421/1675
+f 1421/1675 1433/1687 1422/1676
+f 1433/1687 1434/1688 1422/1676
+f 1422/1676 1434/1688 1423/1677
+f 1434/1688 1435/1689 1423/1677
+f 1423/1677 1435/1689 1424/1678
+f 1424/1678 1435/1689 1425/1679
+f 1425/1679 1435/1689 1436/1690
+f 1436/1690 1437/1691 1425/1679
+f 1425/1679 1437/1691 1426/1680
+f 1437/1691 1438/1692 1426/1680
+f 1426/1680 1438/1692 1427/1681
+f 800/993 812/1005 1428/1682
+f 1428/1682 812/1005 1439/1693
+f 1439/1693 1440/1694 1428/1682
+f 1428/1682 1440/1694 1429/1683
+f 1440/1694 1441/1695 1429/1683
+f 1429/1683 1441/1695 1430/1684
+f 1441/1695 1442/1696 1430/1684
+f 1430/1684 1442/1696 1431/1685
+f 1442/1696 1443/1697 1431/1685
+f 1431/1685 1443/1697 1432/1686
+f 1443/1697 1444/1698 1432/1686
+f 1432/1686 1444/1698 1433/1687
+f 1444/1698 1445/1699 1433/1687
+f 1433/1687 1445/1699 1434/1688
+f 1445/1699 1446/1700 1434/1688
+f 1434/1688 1446/1700 1435/1689
+f 1435/1689 1446/1700 1436/1690
+f 1436/1690 1446/1700 1447/1701
+f 1447/1701 1448/1702 1436/1690
+f 1436/1690 1448/1702 1437/1691
+f 1448/1702 1449/1703 1437/1691
+f 1437/1691 1449/1703 1438/1692
+f 812/1005 824/1017 1439/1693
+f 1439/1693 824/1017 1450/1704
+f 1450/1704 1451/1705 1439/1693
+f 1439/1693 1451/1705 1440/1694
+f 1451/1705 1452/1706 1440/1694
+f 1440/1694 1452/1706 1441/1695
+f 1452/1706 1453/1707 1441/1695
+f 1441/1695 1453/1707 1442/1696
+f 1453/1707 1454/1708 1442/1696
+f 1442/1696 1454/1708 1443/1697
+f 1454/1708 1455/1709 1443/1697
+f 1443/1697 1455/1709 1444/1698
+f 1455/1709 1456/1710 1444/1698
+f 1444/1698 1456/1710 1445/1699
+f 1456/1710 1457/1711 1445/1699
+f 1445/1699 1457/1711 1446/1700
+f 1446/1700 1457/1711 1447/1701
+f 1447/1701 1457/1711 1458/1712
+f 1447/1701 1458/1712 1448/1702
+f 1448/1702 1458/1712 1459/1713
+f 1448/1702 1459/1713 1449/1703
+f 1449/1703 1459/1713 1460/1714
+f 824/1017 847/1029 1450/1704
+f 1450/1704 847/1029 1461/1715
+f 1461/1715 1462/1716 1450/1704
+f 1450/1704 1462/1716 1451/1705
+f 1462/1716 1463/1717 1451/1705
+f 1451/1705 1463/1717 1452/1706
+f 1463/1717 1464/1718 1452/1706
+f 1452/1706 1464/1718 1453/1707
+f 1464/1718 1465/1719 1453/1707
+f 1453/1707 1465/1719 1454/1708
+f 1465/1719 1466/1720 1454/1708
+f 1454/1708 1466/1720 1455/1709
+f 1466/1720 1467/1721 1455/1709
+f 1455/1709 1467/1721 1456/1710
+f 1456/1710 1467/1721 1457/1711
+f 1457/1711 1467/1721 1468/1722
+f 1457/1711 1468/1722 1458/1712
+f 1458/1712 1468/1722 1469/1723
+f 1458/1712 1469/1723 1459/1713
+f 1459/1713 1469/1723 1470/1724
+f 1459/1713 1470/1724 1460/1714
+f 1460/1714 1470/1724 1471/1725
+f 847/1029 848/1041 1461/1715
+f 1461/1715 848/1041 1472/1726
+f 1461/1715 1472/1726 1462/1716
+f 1462/1716 1472/1726 1473/1727
+f 1462/1716 1473/1727 1463/1717
+f 1463/1717 1473/1727 1474/1728
+f 1463/1717 1474/1728 1464/1718
+f 1464/1718 1474/1728 1475/1729
+f 1475/1729 1476/1730 1464/1718
+f 1464/1718 1476/1730 1465/1719
+f 1476/1730 1477/1731 1465/1719
+f 1465/1719 1477/1731 1466/1720
+f 1477/1731 1478/1732 1466/1720
+f 1466/1720 1478/1732 1467/1721
+f 1467/1721 1478/1732 1468/1722
+f 1468/1722 1478/1732 1479/1733
+f 1468/1722 1479/1733 1469/1723
+f 1469/1723 1479/1733 1480/1734
+f 1469/1723 1480/1734 1470/1724
+f 1470/1724 1480/1734 1481/1735
+f 1470/1724 1481/1735 1471/1725
+f 1471/1725 1481/1735 1482/1736
+f 848/1041 860/1053 1472/1726
+f 1472/1726 860/1053 1483/1737
+f 1472/1726 1483/1737 1473/1727
+f 1473/1727 1483/1737 1484/1738
+f 1473/1727 1484/1738 1474/1728
+f 1474/1728 1484/1738 1485/1739
+f 1477/1731 1486/1740 1478/1732
+f 1486/1740 1489/1741 1478/1732
+f 1478/1732 1489/1741 1479/1733
+f 1489/1741 1490/1742 1479/1733
+f 1479/1733 1490/1742 1480/1734
+f 1490/1742 1491/1743 1480/1734
+f 1480/1734 1491/1743 1481/1735
+f 1481/1735 1491/1743 1482/1736
+f 1482/1736 1491/1743 1492/1744
+f 860/1053 869/1062 1483/1737
+f 1483/1737 869/1062 1487/1745
+f 1483/1737 1487/1745 1484/1738
+f 1484/1738 1487/1745 1488/1746
+f 869/1062 872/1065 1487/1745
+f 1487/1745 872/1065 1493/1747
+f 1493/1747 1494/1748 1487/1745
+f 1487/1745 1494/1748 1488/1746
+f 1496/1749 1490/1742 1495/1750
+f 1495/1750 1490/1742 1489/1741
+f 1496/1749 1497/1751 1490/1742
+f 1490/1742 1497/1751 1491/1743
+f 1491/1743 1497/1751 1492/1744
+f 1492/1744 1497/1751 1498/1752
+f 872/1065 879/1072 1493/1747
+f 1493/1747 879/1072 1499/1753
+f 1499/1753 1500/1754 1493/1747
+f 1493/1747 1500/1754 1494/1748
+f 1496/1749 1495/1750 1502/1755
+f 1502/1755 1495/1750 1501/1756
+f 1496/1749 1502/1755 1497/1751
+f 1497/1751 1502/1755 1503/1757
+f 1497/1751 1503/1757 1498/1752
+f 1498/1752 1503/1757 1504/1758
+f 879/1072 886/1079 1499/1753
+f 1499/1753 886/1079 1505/1759
+f 1499/1753 1505/1759 1500/1754
+f 1500/1754 1505/1759 1506/1760
+f 1508/1761 1502/1755 1507/1762
+f 1507/1762 1502/1755 1501/1756
+f 1502/1755 1508/1761 1503/1757
+f 1503/1757 1508/1761 1509/1763
+f 1503/1757 1509/1763 1504/1758
+f 1504/1758 1509/1763 1510/1764
+f 886/1079 893/1086 1505/1759
+f 1505/1759 893/1086 1511/1765
+f 1505/1759 1511/1765 1506/1760
+f 1506/1760 1511/1765 1512/1766
+f 1513/1767 1508/1761 1883/1768
+f 1883/1768 1508/1761 1507/1762
+f 1508/1761 1513/1767 1509/1763
+f 1509/1763 1513/1767 1514/1769
+f 1509/1763 1514/1769 1510/1764
+f 1510/1764 1514/1769 1884/1770
+f 1510/1764 1884/1770 1905/1771
+f 1905/1771 1884/1770 1515/1772
+f 1905/1771 1986/1095 1904/1773
+f 1904/1773 1986/1095 1987/1096
+f 893/1086 899/1098 1511/1765
+f 1511/1765 899/1098 1516/1774
+f 1511/1765 1516/1774 1512/1766
+f 1512/1766 1516/1774 1517/1775
+f 1313/1101 906/1103 1882/1776
+f 1882/1776 906/1103 1522/1777
+f 1339/1582 908/1105 697/885
+f 1340/1584 908/1105 1339/1582
+f 1341/1586 908/1105 1340/1584
+f 1342/1588 908/1105 1341/1586
+f 1343/1590 908/1105 1342/1588
+f 1344/1592 908/1105 1343/1590
+f 1345/1594 908/1105 1344/1592
+f 1346/1596 908/1105 1345/1594
+f 1347/1597 908/1105 1346/1596
+f 1348/1599 908/1105 1347/1597
+f 1349/1602 908/1105 1348/1599
+f 1349/1602 1898/1604 908/1105
+f 1974/1606 908/1105 1898/1604
+f 1556/1778 1622/1779 1595/1780
+f 1595/1780 1622/1779 1659/1781
+f 1622/1779 1556/1778 1643/1782
+f 1643/1782 1556/1778 1571/1783
+f 1557/1784 1622/1779 1573/1785
+f 1573/1785 1622/1779 1643/1782
+f 1622/1779 1557/1784 1659/1781
+f 1659/1781 1557/1784 1596/1786
+f 1558/1787 1623/1788 1594/1789
+f 1594/1789 1623/1788 1658/1790
+f 1623/1788 1558/1787 1644/1791
+f 1644/1791 1558/1787 1570/1792
+f 1556/1778 1623/1788 1571/1783
+f 1571/1783 1623/1788 1644/1791
+f 1623/1788 1556/1778 1658/1790
+f 1658/1790 1556/1778 1595/1780
+f 1559/1793 1624/1794 1593/1795
+f 1593/1795 1624/1794 1657/1796
+f 1624/1794 1559/1793 1642/1797
+f 1642/1797 1559/1793 1568/1798
+f 1558/1787 1624/1794 1570/1792
+f 1570/1792 1624/1794 1642/1797
+f 1624/1794 1558/1787 1657/1796
+f 1657/1796 1558/1787 1594/1789
+f 1625/1799 1656/1800 1560/1801
+f 1560/1801 1656/1800 1592/1802
+f 1625/1799 1560/1801 1645/1803
+f 1645/1803 1560/1801 1575/1804
+f 1559/1793 1625/1799 1568/1798
+f 1568/1798 1625/1799 1645/1803
+f 1625/1799 1559/1793 1656/1800
+f 1656/1800 1559/1793 1593/1795
+f 1561/1805 1579/1806 1626/1807
+f 1626/1807 1579/1806 1647/1808
+f 1626/1807 1647/1808 1560/1801
+f 1560/1801 1647/1808 1575/1804
+f 1626/1807 1682/1809 1561/1805
+f 1561/1805 1682/1809 1681/1810
+f 1627/1811 1655/1812 1562/1813
+f 1562/1813 1655/1812 1591/1814
+f 1562/1813 1583/1815 1627/1811
+f 1627/1811 1583/1815 1649/1816
+f 1627/1811 1649/1816 1561/1805
+f 1561/1805 1649/1816 1579/1806
+f 1561/1805 1681/1810 1627/1811
+f 1627/1811 1681/1810 1655/1812
+f 1628/1817 1654/1818 1563/1819
+f 1563/1819 1654/1818 1590/1820
+f 1563/1819 1586/1821 1628/1817
+f 1628/1817 1586/1821 1651/1822
+f 1628/1817 1651/1822 1562/1813
+f 1562/1813 1651/1822 1583/1815
+f 1562/1813 1591/1814 1628/1817
+f 1628/1817 1591/1814 1654/1818
+f 1629/1823 1653/1824 1564/1825
+f 1564/1825 1653/1824 1588/1826
+f 1564/1825 1582/1827 1629/1823
+f 1629/1823 1582/1827 1650/1828
+f 1629/1823 1650/1828 1563/1819
+f 1563/1819 1650/1828 1586/1821
+f 1563/1819 1590/1820 1629/1823
+f 1629/1823 1590/1820 1653/1824
+f 1630/1829 1652/1830 1565/1831
+f 1565/1831 1652/1830 1589/1832
+f 1565/1831 1578/1833 1630/1829
+f 1630/1829 1578/1833 1648/1834
+f 1630/1829 1648/1834 1564/1825
+f 1564/1825 1648/1834 1582/1827
+f 1564/1825 1588/1826 1630/1829
+f 1630/1829 1588/1826 1652/1830
+f 1557/1784 1631/1835 1596/1786
+f 1596/1786 1631/1835 1660/1836
+f 1631/1835 1557/1784 1646/1837
+f 1646/1837 1557/1784 1573/1785
+f 1565/1831 1631/1835 1578/1833
+f 1578/1833 1631/1835 1646/1837
+f 1631/1835 1565/1831 1660/1836
+f 1660/1836 1565/1831 1589/1832
+f 1596/1786 1588/1826 1659/1781
+f 1659/1781 1588/1826 1653/1824
+f 1601/1838 1928/1839 1663/1840
+f 1663/1840 1928/1839 1927/1841
+f 1538/1842 1926/1843 1632/1844
+f 1632/1844 1926/1843 1927/1841
+f 1605/1845 1935/1846 1665/1847
+f 1665/1847 1935/1846 1933/1848
+f 1540/1849 1931/1850 1633/1851
+f 1633/1851 1931/1850 1933/1848
+f 1524/1852 1931/1850 1666/1853
+f 1666/1853 1931/1850 1929/1854
+f 1566/1855 1928/1839 1634/1856
+f 1634/1856 1928/1839 1929/1854
+f 1667/1857 1523/1858 1930/1859
+f 1930/1859 1523/1858 1926/1843
+f 1539/1860 1932/1861 1635/1862
+f 1635/1862 1932/1861 1930/1859
+f 1525/1863 1932/1861 1668/1864
+f 1668/1864 1932/1861 1934/1865
+f 1636/1866 1541/1867 1934/1865
+f 1934/1865 1541/1867 1936/1868
+f 1637/1869 1542/1870 1938/1871
+f 1938/1871 1542/1870 1940/1872
+f 1661/1873 1597/1874 1938/1871
+f 1938/1871 1597/1874 1936/1868
+f 1662/1875 1599/1876 1942/1877
+f 1942/1877 1599/1876 1940/1872
+f 1638/1878 1544/1879 1942/1877
+f 1942/1877 1544/1879 1944/1880
+f 1669/1881 1526/1882 1945/1883
+f 1945/1883 1526/1882 1944/1880
+f 1639/1884 1545/1885 1945/1883
+f 1945/1883 1545/1885 1943/1886
+f 1664/1887 1603/1888 1941/1889
+f 1941/1889 1603/1888 1943/1886
+f 1640/1890 1543/1891 1941/1889
+f 1941/1889 1543/1891 1939/1892
+f 1670/1893 1527/1894 1937/1895
+f 1937/1895 1527/1894 1939/1892
+f 1641/1896 1567/1897 1937/1895
+f 1937/1895 1567/1897 1935/1846
+f 1529/1898 1569/1899 1568/1798
+f 1568/1798 1569/1899 1642/1797
+f 1569/1899 1528/1900 1642/1797
+f 1642/1797 1528/1900 1570/1792
+f 1531/1901 1572/1902 1571/1783
+f 1571/1783 1572/1902 1643/1782
+f 1572/1902 1530/1903 1643/1782
+f 1643/1782 1530/1903 1573/1785
+f 1528/1900 1574/1904 1570/1792
+f 1570/1792 1574/1904 1644/1791
+f 1574/1904 1531/1901 1644/1791
+f 1644/1791 1531/1901 1571/1783
+f 1532/1905 1576/1906 1575/1804
+f 1575/1804 1576/1906 1645/1803
+f 1576/1906 1529/1898 1645/1803
+f 1645/1803 1529/1898 1568/1798
+f 1530/1903 1577/1907 1573/1785
+f 1573/1785 1577/1907 1646/1837
+f 1577/1907 1533/1908 1646/1837
+f 1646/1837 1533/1908 1578/1833
+f 1579/1806 1534/1909 1647/1808
+f 1647/1808 1534/1909 1580/1910
+f 1532/1905 1575/1804 1580/1910
+f 1580/1910 1575/1804 1647/1808
+f 1578/1833 1533/1908 1648/1834
+f 1648/1834 1533/1908 1581/1911
+f 1535/1912 1582/1827 1581/1911
+f 1581/1911 1582/1827 1648/1834
+f 1583/1815 1536/1913 1649/1816
+f 1649/1816 1536/1913 1584/1914
+f 1534/1909 1579/1806 1584/1914
+f 1584/1914 1579/1806 1649/1816
+f 1582/1827 1535/1912 1650/1828
+f 1650/1828 1535/1912 1585/1915
+f 1537/1916 1586/1821 1585/1915
+f 1585/1915 1586/1821 1650/1828
+f 1586/1821 1537/1916 1651/1822
+f 1651/1822 1537/1916 1587/1917
+f 1536/1913 1583/1815 1587/1917
+f 1587/1917 1583/1815 1651/1822
+f 1569/1918 1529/1919 1635/1862
+f 1635/1862 1529/1919 1539/1860
+f 1635/1862 1538/1842 1569/1918
+f 1569/1918 1538/1842 1528/1920
+f 1572/1921 1531/1922 1634/1856
+f 1634/1856 1531/1922 1566/1855
+f 1572/1921 1634/1856 1530/1923
+f 1530/1923 1634/1856 1540/1849
+f 1574/1924 1528/1920 1632/1844
+f 1632/1844 1528/1920 1538/1842
+f 1632/1844 1566/1855 1574/1924
+f 1574/1924 1566/1855 1531/1922
+f 1576/1925 1532/1926 1636/1866
+f 1636/1866 1532/1926 1541/1867
+f 1636/1866 1539/1860 1576/1925
+f 1576/1925 1539/1860 1529/1919
+f 1530/1923 1540/1849 1577/1927
+f 1577/1927 1540/1849 1633/1851
+f 1577/1927 1633/1851 1533/1928
+f 1533/1928 1633/1851 1567/1897
+f 1580/1929 1534/1930 1637/1869
+f 1637/1869 1534/1930 1542/1870
+f 1637/1869 1541/1867 1580/1929
+f 1580/1929 1541/1867 1532/1926
+f 1533/1928 1567/1897 1581/1931
+f 1581/1931 1567/1897 1641/1896
+f 1581/1931 1641/1896 1535/1932
+f 1535/1932 1641/1896 1543/1891
+f 1584/1933 1536/1934 1638/1878
+f 1638/1878 1536/1934 1544/1879
+f 1638/1878 1542/1870 1584/1933
+f 1584/1933 1542/1870 1534/1930
+f 1535/1932 1543/1891 1585/1935
+f 1585/1935 1543/1891 1640/1890
+f 1585/1935 1640/1890 1537/1936
+f 1537/1936 1640/1890 1545/1885
+f 1587/1937 1537/1936 1639/1884
+f 1639/1884 1537/1936 1545/1885
+f 1639/1884 1544/1879 1587/1937
+f 1587/1937 1544/1879 1536/1934
+f 1547/1938 1611/1939 1599/1876
+f 1599/1876 1611/1939 1661/1873
+f 1546/1940 1597/1874 1611/1939
+f 1611/1939 1597/1874 1661/1873
+f 1548/1941 1598/1942 1526/1882
+f 1526/1882 1598/1942 1662/1875
+f 1598/1942 1547/1938 1662/1875
+f 1662/1875 1547/1938 1599/1876
+f 1523/1858 1550/1943 1663/1840
+f 1663/1840 1550/1943 1600/1944
+f 1549/1945 1601/1838 1600/1944
+f 1600/1944 1601/1838 1663/1840
+f 1552/1946 1602/1947 1527/1894
+f 1527/1894 1602/1947 1664/1887
+f 1602/1947 1551/1948 1664/1887
+f 1664/1887 1551/1948 1603/1888
+f 1524/1852 1554/1949 1665/1847
+f 1665/1847 1554/1949 1604/1950
+f 1604/1950 1553/1951 1665/1847
+f 1665/1847 1553/1951 1605/1845
+f 1601/1838 1549/1945 1666/1853
+f 1666/1853 1549/1945 1606/1952
+f 1554/1949 1524/1852 1606/1952
+f 1606/1952 1524/1852 1666/1853
+f 1525/1863 1555/1953 1667/1857
+f 1667/1857 1555/1953 1607/1954
+f 1550/1943 1523/1858 1607/1954
+f 1607/1954 1523/1858 1667/1857
+f 1597/1874 1546/1940 1668/1864
+f 1668/1864 1546/1940 1608/1955
+f 1555/1953 1525/1863 1608/1955
+f 1608/1955 1525/1863 1668/1864
+f 1551/1948 1609/1956 1603/1888
+f 1603/1888 1609/1956 1669/1881
+f 1609/1956 1548/1941 1669/1881
+f 1669/1881 1548/1941 1526/1882
+f 1553/1951 1610/1957 1605/1845
+f 1605/1845 1610/1957 1670/1893
+f 1610/1957 1552/1946 1670/1893
+f 1670/1893 1552/1946 1527/1894
+f 1547/1938 1612/1958 1611/1939
+f 1611/1939 1612/1958 1671/1959
+f 1613/1960 1546/1940 1671/1959
+f 1671/1959 1546/1940 1611/1939
+f 1598/1942 1548/1941 1672/1961
+f 1672/1961 1548/1941 1614/1962
+f 1547/1938 1598/1942 1612/1958
+f 1612/1958 1598/1942 1672/1961
+f 1550/1943 1615/1963 1600/1944
+f 1600/1944 1615/1963 1673/1964
+f 1616/1965 1549/1945 1673/1964
+f 1673/1964 1549/1945 1600/1944
+f 1602/1947 1552/1946 1674/1966
+f 1674/1966 1552/1946 1617/1967
+f 1551/1948 1602/1947 1618/1968
+f 1618/1968 1602/1947 1674/1966
+f 1604/1950 1554/1949 1675/1969
+f 1675/1969 1554/1949 1619/1970
+f 1553/1951 1604/1950 1620/1971
+f 1620/1971 1604/1950 1675/1969
+f 1549/1945 1616/1965 1606/1952
+f 1606/1952 1616/1965 1676/1972
+f 1619/1970 1554/1949 1676/1972
+f 1676/1972 1554/1949 1606/1952
+f 1555/1953 1621/1973 1607/1954
+f 1607/1954 1621/1973 1677/1974
+f 1615/1963 1550/1943 1677/1974
+f 1677/1974 1550/1943 1607/1954
+f 1546/1940 1613/1960 1608/1955
+f 1608/1955 1613/1960 1678/1975
+f 1621/1973 1555/1953 1678/1975
+f 1678/1975 1555/1953 1608/1955
+f 1609/1956 1551/1948 1679/1976
+f 1679/1976 1551/1948 1618/1968
+f 1548/1941 1609/1956 1614/1962
+f 1614/1962 1609/1956 1679/1976
+f 1610/1957 1553/1951 1680/1977
+f 1680/1977 1553/1951 1620/1971
+f 1552/1946 1610/1957 1617/1967
+f 1617/1967 1610/1957 1680/1977
+f 1652/1830 1588/1826 1589/1832
+f 1588/1826 1596/1786 1589/1832
+f 1596/1786 1660/1836 1589/1832
+f 1595/1780 1590/1820 1658/1790
+f 1658/1790 1590/1820 1654/1818
+f 1659/1781 1653/1824 1595/1780
+f 1595/1780 1653/1824 1590/1820
+f 1658/1790 1654/1818 1594/1789
+f 1594/1789 1654/1818 1591/1814
+f 1656/1800 1593/1795 1592/1802
+f 1681/1810 1682/1809 1593/1795
+f 1593/1795 1682/1809 1592/1802
+f 1591/1814 1655/1812 1594/1789
+f 1594/1789 1655/1812 1657/1796
+f 1560/1801 1592/1802 1626/1807
+f 1626/1807 1592/1802 1682/1809
+f 1593/1795 1657/1796 1681/1810
+f 1681/1810 1657/1796 1655/1812
+f 1612/1958 1520/1978 1671/1959
+f 1671/1959 1520/1978 1521/1979
+f 1671/1959 1521/1979 1613/1960
+f 1613/1960 1521/1979 1883/1768
+f 1518/1980 1519/1981 1614/1962
+f 1614/1962 1519/1981 1672/1961
+f 1489/1741 1486/1740 1615/1963
+f 1615/1963 1486/1740 1673/1964
+f 1673/1964 1486/1740 1616/1965
+f 1616/1965 1486/1740 1477/1731
+f 1500/1754 1506/1760 1617/1967
+f 1617/1967 1506/1760 1674/1966
+f 1506/1760 1512/1766 1674/1966
+f 1674/1966 1512/1766 1618/1968
+f 1619/1970 1475/1729 1675/1969
+f 1675/1969 1475/1729 1485/1739
+f 1675/1969 1485/1739 1620/1971
+f 1620/1971 1485/1739 1488/1746
+f 1477/1731 1476/1730 1616/1965
+f 1616/1965 1476/1730 1676/1972
+f 1676/1972 1476/1730 1619/1970
+f 1619/1970 1476/1730 1475/1729
+f 1621/1973 1501/1756 1677/1974
+f 1677/1974 1501/1756 1495/1750
+f 1677/1974 1495/1750 1615/1963
+f 1615/1963 1495/1750 1489/1741
+f 1678/1975 1507/1762 1621/1973
+f 1621/1973 1507/1762 1501/1756
+f 1618/1968 1512/1766 1679/1976
+f 1679/1976 1512/1766 1517/1775
+f 1517/1775 1518/1980 1679/1976
+f 1679/1976 1518/1980 1614/1962
+f 1620/1971 1488/1746 1680/1977
+f 1680/1977 1488/1746 1494/1748
+f 1680/1977 1494/1748 1617/1967
+f 1617/1967 1494/1748 1500/1754
+f 1519/1981 1520/1978 1672/1961
+f 1672/1961 1520/1978 1612/1958
+f 1488/1746 1485/1739 1484/1738
+f 1485/1739 1475/1729 1474/1728
+f 1678/1975 1613/1960 1507/1762
+f 1507/1762 1613/1960 1883/1768
+f 1808/1982 1688/1983 1722/1984
+f 1688/1983 1723/1985 1722/1984
+f 1722/1984 1723/1985 1808/1982
+f 1724/1986 1071/1317 1809/1987
+f 1809/1987 1071/1317 1093/1318
+f 1880/1988 1724/1986 1879/1989
+f 1879/1989 1724/1986 1809/1987
+f 1724/1986 1880/1988 1810/1990
+f 1810/1990 1880/1988 1881/1991
+f 1094/1325 1810/1990 1111/1323
+f 1111/1323 1810/1990 1858/1992
+f 1071/1317 1724/1986 1094/1325
+f 1094/1325 1724/1986 1810/1990
+f 1868/1993 1725/1994 1790/1995
+f 1790/1995 1725/1994 1689/1996
+f 1725/1994 1868/1993 1684/1997
+f 1684/1997 1868/1993 1686/1998
+f 1726/1999 1721/2000 1691/2001
+f 1691/2001 1721/2000 1683/2002
+f 1072/1336 1070/1337 1726/1999
+f 1726/1999 1070/1337 1721/2000
+f 1725/1994 1684/1997 1811/2003
+f 1811/2003 1684/1997 1727/2004
+f 1685/2005 1848/2006 1727/2004
+f 1727/2004 1848/2006 1811/2003
+f 1848/2006 1793/2007 1811/2003
+f 1811/2003 1793/2007 1728/2008
+f 1689/1996 1725/1994 1728/2008
+f 1728/2008 1725/1994 1811/2003
+f 1845/2009 1729/2010 1789/2011
+f 1789/2011 1729/2010 1692/2012
+f 1790/1995 1689/1996 1845/2009
+f 1845/2009 1689/1996 1729/2010
+f 1730/2013 1694/2014 1812/2015
+f 1812/2015 1694/2014 1731/2016
+f 1695/2017 1816/2018 1731/2016
+f 1731/2016 1816/2018 1812/2015
+f 1816/2018 1732/2019 1812/2015
+f 1732/2019 1693/2020 1812/2015
+f 1812/2015 1693/2020 1730/2013
+f 1733/2021 1691/2001 1813/2022
+f 1813/2022 1691/2001 1690/2023
+f 1696/2024 1733/2021 1791/2025
+f 1791/2025 1733/2021 1813/2022
+f 1733/2021 1696/2024 1814/2026
+f 1814/2026 1696/2024 1734/2027
+f 1073/1364 1095/1363 1734/2027
+f 1734/2027 1095/1363 1814/2026
+f 1095/1363 1072/1336 1814/2026
+f 1814/2026 1072/1336 1726/1999
+f 1691/2001 1733/2021 1726/1999
+f 1726/1999 1733/2021 1814/2026
+f 1849/2028 1794/2029 1815/2030
+f 1815/2030 1794/2029 1735/2031
+f 1735/2031 1692/2012 1815/2030
+f 1815/2030 1692/2012 1729/2010
+f 1689/1996 1728/2008 1729/2010
+f 1729/2010 1728/2008 1815/2030
+f 1793/2007 1849/2028 1728/2008
+f 1728/2008 1849/2028 1815/2030
+f 1695/2017 1697/2032 1816/2018
+f 1816/2018 1697/2032 1736/2033
+f 1736/2033 1737/2034 1816/2018
+f 1699/2035 1738/2036 1779/2037
+f 1779/2037 1738/2036 1700/2038
+f 1738/2036 1780/2039 1700/2038
+f 1700/2038 1780/2039 1867/2040
+f 1739/2041 1074/1380 1817/2042
+f 1817/2042 1074/1380 1096/1381
+f 1096/1381 1075/1382 1817/2042
+f 1817/2042 1075/1382 1837/2043
+f 1780/2039 1738/2036 1837/2043
+f 1837/2043 1738/2036 1817/2042
+f 1699/2035 1739/2041 1738/2036
+f 1738/2036 1739/2041 1817/2042
+f 1799/2044 1702/2045 1818/2046
+f 1818/2046 1702/2045 1741/2047
+f 1701/2048 1740/2049 1741/2047
+f 1741/2047 1740/2049 1818/2046
+f 1703/2050 1743/2051 1742/2052
+f 1742/2052 1743/2051 1819/2053
+f 1687/2054 1808/1982 1743/2051
+f 1743/2051 1808/1982 1819/2053
+f 1819/2053 1745/2055 1742/2052
+f 1742/2052 1745/2055 1872/2056
+f 1723/1985 1744/2057 1808/1982
+f 1808/1982 1744/2057 1819/2053
+f 1744/2057 1704/2058 1819/2053
+f 1819/2053 1704/2058 1745/2055
+f 1877/2059 1878/2060 1746/2061
+f 1746/2061 1878/2060 1820/2062
+f 1878/2060 1879/1989 1820/2062
+f 1820/2062 1879/1989 1809/1987
+f 1809/1987 1093/1318 1820/2062
+f 1820/2062 1093/1318 1097/1402
+f 1076/1403 1746/2061 1097/1402
+f 1097/1402 1746/2061 1820/2062
+f 1789/2011 1692/2012 1844/2063
+f 1844/2063 1692/2012 1747/2064
+f 1844/2063 1747/2064 1705/2065
+f 1705/2065 1747/2064 1706/2066
+f 1748/2067 1707/2068 1821/2069
+f 1821/2069 1707/2068 1749/2070
+f 1708/2071 1750/2072 1749/2070
+f 1749/2070 1750/2072 1821/2069
+f 1750/2072 1703/2050 1821/2069
+f 1821/2069 1703/2050 1742/2052
+f 1707/2068 1748/2067 1751/2073
+f 1751/2073 1748/2067 1822/2074
+f 1745/2055 1806/2075 1872/2056
+f 1752/2076 1872/2056 1806/2075
+f 1752/2076 1697/2032 1822/2074
+f 1822/2074 1697/2032 1753/2077
+f 1709/2078 1751/2073 1753/2077
+f 1753/2077 1751/2073 1822/2074
+f 1703/2050 1750/2072 1754/2079
+f 1754/2079 1750/2072 1823/2080
+f 1750/2072 1708/2071 1823/2080
+f 1823/2080 1708/2071 1755/2081
+f 1803/2082 1857/2083 1755/2081
+f 1755/2081 1857/2083 1823/2080
+f 1857/2083 1804/2084 1823/2080
+f 1823/2080 1804/2084 1754/2079
+f 1756/2085 1843/2086 1710/2087
+f 1710/2087 1843/2086 1788/2088
+f 1843/2086 1756/2085 1787/2089
+f 1787/2089 1756/2085 1711/2090
+f 1757/2091 1713/2092 1824/2093
+f 1824/2093 1713/2092 1758/2094
+f 1715/2095 1759/2096 1758/2094
+f 1758/2094 1759/2096 1824/2093
+f 1759/2096 1714/2097 1824/2093
+f 1824/2093 1714/2097 1760/2098
+f 1712/2099 1757/2091 1760/2098
+f 1760/2098 1757/2091 1824/2093
+f 1761/2100 1716/2101 1825/2102
+f 1825/2102 1716/2101 1762/2103
+f 1701/2048 1763/2104 1762/2103
+f 1762/2103 1763/2104 1825/2102
+f 1713/2092 1757/2091 1763/2104
+f 1763/2104 1757/2091 1825/2102
+f 1757/2091 1712/2099 1825/2102
+f 1825/2102 1712/2099 1761/2100
+f 1764/2105 1796/2106 1826/2107
+f 1826/2107 1796/2106 1852/2108
+f 1797/2109 1765/2110 1852/2108
+f 1852/2108 1765/2110 1826/2107
+f 1765/2110 1711/2090 1826/2107
+f 1826/2107 1711/2090 1756/2085
+f 1710/2087 1764/2105 1756/2085
+f 1756/2085 1764/2105 1826/2107
+f 1714/2097 1759/2096 1766/2111
+f 1766/2111 1759/2096 1827/2112
+f 1759/2096 1715/2095 1827/2112
+f 1827/2112 1715/2095 1767/2113
+f 1801/2114 1855/2115 1767/2113
+f 1767/2113 1855/2115 1827/2112
+f 1855/2115 1802/2116 1827/2112
+f 1827/2112 1802/2116 1766/2111
+f 1787/2089 1711/2090 1842/2117
+f 1842/2117 1711/2090 1765/2110
+f 1798/2118 1842/2117 1797/2109
+f 1797/2109 1842/2117 1765/2110
+f 1800/2119 1854/2120 1768/2121
+f 1768/2121 1854/2120 1828/2122
+f 1854/2120 1801/2114 1828/2122
+f 1828/2122 1801/2114 1767/2113
+f 1767/2113 1715/2095 1828/2122
+f 1828/2122 1715/2095 1758/2094
+f 1713/2092 1768/2121 1758/2094
+f 1758/2094 1768/2121 1828/2122
+f 1769/2123 1696/2024 1829/2124
+f 1829/2124 1696/2024 1791/2025
+f 1717/2125 1769/2123 1792/2126
+f 1792/2126 1769/2123 1829/2124
+f 1718/2127 1770/2128 1771/2129
+f 1771/2129 1770/2128 1830/2130
+f 1745/2055 1704/2058 1831/2131
+f 1831/2131 1704/2058 1772/2132
+f 1772/2132 1876/2133 1873/2134
+f 1873/2134 1876/2133 1875/2135
+f 1769/2123 1717/2125 1832/2136
+f 1832/2136 1717/2125 1774/2137
+f 1077/1480 1098/1479 1774/2137
+f 1774/2137 1098/1479 1832/2136
+f 1098/1479 1073/1364 1832/2136
+f 1832/2136 1073/1364 1734/2027
+f 1696/2024 1769/2123 1734/2027
+f 1734/2027 1769/2123 1832/2136
+f 1877/2059 1746/2061 1876/2133
+f 1876/2133 1746/2061 1833/2138
+f 1746/2061 1076/1403 1833/2138
+f 1833/2138 1076/1403 1099/1482
+f 1078/1484 1775/2139 1099/1482
+f 1099/1482 1775/2139 1833/2138
+f 1775/2139 1875/2135 1833/2138
+f 1833/2138 1875/2135 1876/2133
+f 1719/2140 1717/2125 1847/2141
+f 1847/2141 1717/2125 1792/2126
+f 1770/2128 1718/2127 1777/2142
+f 1777/2142 1718/2127 1776/2143
+f 1873/2134 1875/2135 1836/2144
+f 1836/2144 1875/2135 1874/2145
+f 1079/1491 1077/1480 1834/2146
+f 1834/2146 1077/1480 1774/2137
+f 1717/2125 1719/2140 1774/2137
+f 1774/2137 1719/2140 1834/2146
+f 1775/2139 1078/1484 1835/2147
+f 1835/2147 1078/1484 1100/1494
+f 1875/2135 1775/2139 1874/2145
+f 1874/2145 1775/2139 1835/2147
+f 1777/2142 1776/2143 1700/2038
+f 1700/2038 1776/2143 1779/2037
+f 1075/1382 1079/1491 1837/2043
+f 1837/2043 1079/1491 1834/2146
+f 1719/2140 1780/2039 1834/2146
+f 1834/2146 1780/2039 1837/2043
+f 1795/2148 1781/2149 1850/2150
+f 1850/2150 1781/2149 1838/2151
+f 1781/2149 1706/2066 1838/2151
+f 1838/2151 1706/2066 1747/2064
+f 1692/2012 1735/2031 1747/2064
+f 1747/2064 1735/2031 1838/2151
+f 1735/2031 1794/2029 1838/2151
+f 1838/2151 1794/2029 1850/2150
+f 1856/2152 1803/2082 1839/2153
+f 1839/2153 1803/2082 1755/2081
+f 1708/2071 1782/2154 1755/2081
+f 1755/2081 1782/2154 1839/2153
+f 1782/2154 1714/2097 1839/2153
+f 1839/2153 1714/2097 1766/2111
+f 1802/2116 1856/2152 1766/2111
+f 1766/2111 1856/2152 1839/2153
+f 1720/2155 1716/2101 1840/2156
+f 1840/2156 1716/2101 1783/2157
+f 1709/2078 1784/2158 1783/2157
+f 1783/2157 1784/2158 1840/2156
+f 1705/2065 1706/2066 1871/2159
+f 1871/2159 1706/2066 1785/2160
+f 1710/2087 1788/2088 1785/2160
+f 1785/2160 1788/2088 1871/2159
+f 1786/2161 1712/2099 1841/2162
+f 1841/2162 1712/2099 1760/2098
+f 1714/2097 1782/2154 1760/2098
+f 1760/2098 1782/2154 1841/2162
+f 1782/2154 1708/2071 1841/2162
+f 1841/2162 1708/2071 1749/2070
+f 1707/2068 1786/2161 1749/2070
+f 1749/2070 1786/2161 1841/2162
+f 1798/2118 1799/2044 1842/2117
+f 1842/2117 1799/2044 1818/2046
+f 1740/2049 1787/2089 1818/2046
+f 1818/2046 1787/2089 1842/2117
+f 1787/2089 1740/2049 1843/2086
+f 1843/2086 1740/2049 1864/2163
+f 1720/2155 1788/2088 1864/2163
+f 1864/2163 1788/2088 1843/2086
+f 1705/2065 1784/2158 1844/2063
+f 1844/2063 1784/2158 1861/2164
+f 1695/2017 1789/2011 1861/2164
+f 1861/2164 1789/2011 1844/2063
+f 1789/2011 1695/2017 1845/2009
+f 1845/2009 1695/2017 1731/2016
+f 1694/2014 1790/1995 1731/2016
+f 1731/2016 1790/1995 1845/2009
+f 1694/2014 1730/2013 1686/1998
+f 1686/1998 1730/2013 1846/2165
+f 1897/2166 1693/2020 1732/2019
+f 1830/2130 1770/2128 1829/2124
+f 1829/2124 1770/2128 1792/2126
+f 1770/2128 1777/2142 1792/2126
+f 1792/2126 1777/2142 1847/2141
+f 1777/2142 1700/2038 1847/2141
+f 1847/2141 1700/2038 1867/2040
+f 1069/1514 1101/1515 1685/2005
+f 1685/2005 1101/1515 1848/2006
+f 1101/1515 1080/1516 1848/2006
+f 1848/2006 1080/1516 1793/2007
+f 1080/1516 1102/1517 1793/2007
+f 1793/2007 1102/1517 1849/2028
+f 1102/1517 1081/1518 1849/2028
+f 1849/2028 1081/1518 1794/2029
+f 1081/1518 1103/1519 1794/2029
+f 1794/2029 1103/1519 1850/2150
+f 1082/1520 1795/2148 1103/1519
+f 1103/1519 1795/2148 1850/2150
+f 1795/2148 1082/1520 1851/2167
+f 1851/2167 1082/1520 1104/1522
+f 1083/1523 1796/2106 1104/1522
+f 1104/1522 1796/2106 1851/2167
+f 1796/2106 1083/1523 1852/2108
+f 1852/2108 1083/1523 1105/1524
+f 1084/1525 1797/2109 1105/1524
+f 1105/1524 1797/2109 1852/2108
+f 1085/1526 1798/2118 1084/1525
+f 1084/1525 1798/2118 1797/2109
+f 1086/1527 1799/2044 1085/1526
+f 1085/1526 1799/2044 1798/2118
+f 1088/1530 1106/1529 1702/2045
+f 1702/2045 1106/1529 1853/2168
+f 1106/1529 1087/1531 1853/2168
+f 1853/2168 1087/1531 1800/2119
+f 1107/1532 1089/1533 1854/2120
+f 1854/2120 1089/1533 1801/2114
+f 1087/1531 1107/1532 1800/2119
+f 1800/2119 1107/1532 1854/2120
+f 1089/1533 1108/1534 1801/2114
+f 1801/2114 1108/1534 1855/2115
+f 1108/1534 1090/1535 1855/2115
+f 1855/2115 1090/1535 1802/2116
+f 1109/1536 1091/1537 1856/2152
+f 1856/2152 1091/1537 1803/2082
+f 1090/1535 1109/1536 1802/2116
+f 1802/2116 1109/1536 1856/2152
+f 1091/1537 1110/1538 1803/2082
+f 1803/2082 1110/1538 1857/2083
+f 1110/1538 1092/1539 1857/2083
+f 1857/2083 1092/1539 1804/2084
+f 1092/1539 1111/1323 1804/2084
+f 1804/2084 1111/1323 1858/1992
+f 1739/2041 1699/2035 1859/2169
+f 1859/2169 1699/2035 1805/2170
+f 1874/2145 1835/2147 1805/2170
+f 1805/2170 1835/2147 1859/2169
+f 1835/2147 1100/1494 1859/2169
+f 1859/2169 1100/1494 1112/1542
+f 1074/1380 1739/2041 1112/1542
+f 1112/1542 1739/2041 1859/2169
+f 1086/1527 1088/1530 1799/2044
+f 1799/2044 1088/1530 1702/2045
+f 1698/2171 1806/2075 1771/2129
+f 1771/2129 1806/2075 1860/2172
+f 1806/2075 1745/2055 1860/2172
+f 1860/2172 1745/2055 1831/2131
+f 1773/2173 1807/2174 1831/2131
+f 1831/2131 1807/2174 1860/2172
+f 1807/2174 1718/2127 1860/2172
+f 1860/2172 1718/2127 1771/2129
+f 1784/2158 1709/2078 1861/2164
+f 1861/2164 1709/2078 1753/2077
+f 1695/2017 1861/2164 1697/2032
+f 1697/2032 1861/2164 1753/2077
+f 1718/2127 1807/2174 1776/2143
+f 1776/2143 1807/2174 1862/2175
+f 1807/2174 1773/2173 1862/2175
+f 1862/2175 1773/2173 1778/2176
+f 1786/2161 1707/2068 1863/2177
+f 1863/2177 1707/2068 1751/2073
+f 1751/2073 1709/2078 1863/2177
+f 1863/2177 1709/2078 1783/2157
+f 1716/2101 1761/2100 1783/2157
+f 1783/2157 1761/2100 1863/2177
+f 1712/2099 1786/2161 1761/2100
+f 1761/2100 1786/2161 1863/2177
+f 1776/2143 1862/2175 1779/2037
+f 1779/2037 1862/2175 1865/2178
+f 1778/2176 1836/2144 1862/2175
+f 1862/2175 1836/2144 1865/2178
+f 1740/2049 1701/2048 1864/2163
+f 1864/2163 1701/2048 1762/2103
+f 1716/2101 1720/2155 1762/2103
+f 1762/2103 1720/2155 1864/2163
+f 1805/2170 1699/2035 1865/2178
+f 1865/2178 1699/2035 1779/2037
+f 1853/2168 1800/2119 1866/2179
+f 1866/2179 1800/2119 1768/2121
+f 1713/2092 1763/2104 1768/2121
+f 1768/2121 1763/2104 1866/2179
+f 1763/2104 1701/2048 1866/2179
+f 1866/2179 1701/2048 1741/2047
+f 1702/2045 1853/2168 1741/2047
+f 1741/2047 1853/2168 1866/2179
+f 1780/2039 1719/2140 1867/2040
+f 1867/2040 1719/2140 1847/2141
+f 1790/1995 1694/2014 1868/1993
+f 1868/1993 1694/2014 1686/1998
+f 1697/2032 1752/2076 1736/2033
+f 1736/2033 1752/2076 1698/2171
+f 1806/2075 1698/2171 1752/2076
+f 1804/2084 1858/1992 1754/2079
+f 1754/2079 1858/1992 1869/2180
+f 1858/1992 1810/1990 1869/2180
+f 1810/1990 1881/1991 1869/2180
+f 1743/2051 1703/2050 1869/2180
+f 1869/2180 1703/2050 1754/2079
+f 1781/2149 1795/2148 1870/2181
+f 1870/2181 1795/2148 1851/2167
+f 1796/2106 1764/2105 1851/2167
+f 1851/2167 1764/2105 1870/2181
+f 1764/2105 1710/2087 1870/2181
+f 1870/2181 1710/2087 1785/2160
+f 1706/2066 1781/2149 1785/2160
+f 1785/2160 1781/2149 1870/2181
+f 1788/2088 1720/2155 1871/2159
+f 1871/2159 1720/2155 1840/2156
+f 1784/2158 1705/2065 1840/2156
+f 1840/2156 1705/2065 1871/2159
+f 1872/2056 1752/2076 1748/2067
+f 1748/2067 1752/2076 1822/2074
+f 1872/2056 1748/2067 1742/2052
+f 1742/2052 1748/2067 1821/2069
+f 1773/2173 1873/2134 1778/2176
+f 1778/2176 1873/2134 1836/2144
+f 1873/2134 1773/2173 1772/2132
+f 1772/2132 1773/2173 1831/2131
+f 1836/2144 1874/2145 1865/2178
+f 1865/2178 1874/2145 1805/2170
+f 1704/2058 1877/2059 1772/2132
+f 1772/2132 1877/2059 1876/2133
+f 1878/2060 1877/2059 1744/2057
+f 1744/2057 1877/2059 1704/2058
+f 1744/2057 1723/1985 1878/2060
+f 1878/2060 1723/1985 1879/1989
+f 1688/1983 1880/1988 1723/1985
+f 1723/1985 1880/1988 1879/1989
+f 1881/1991 1880/1988 1687/2054
+f 1687/2054 1880/1988 1688/1983
+f 1869/2180 1881/1991 1743/2051
+f 1743/2051 1881/1991 1687/2054
+f 1685/2182 1887/2183 1069/1556
+f 1069/1556 1887/2183 1320/1554
+f 1522/1777 1887/2183 1519/1981
+f 1519/1981 1887/2183 1888/2184
+f 1519/1981 1518/1980 1522/1777
+f 1522/1777 1518/1980 1882/1776
+f 1518/1980 1517/1775 1882/1776
+f 1882/1776 1517/1775 1516/1774
+f 899/1098 1313/1101 1516/1774
+f 1516/1774 1313/1101 1882/1776
+f 1684/2185 1686/2186 1889/2187
+f 1889/2187 1686/2186 1890/2188
+f 1883/1768 1521/1979 1891/2189
+f 1891/2189 1521/1979 1890/2188
+f 1886/2190 1893/2191 1885/2192
+f 1885/2192 1893/2191 1892/2193
+f 1515/1772 1899/2194 1316/1570
+f 1316/1570 1899/2194 1321/1568
+f 1519/1981 1888/2184 1520/1978
+f 1520/1978 1888/2184 1889/2187
+f 1883/1768 1891/2189 1513/1767
+f 1513/1767 1891/2189 1892/2193
+f 1730/2013 1693/2020 1846/2165
+f 1846/2165 1693/2020 1885/2195
+f 1691/2001 1683/2002 1690/2023
+f 1690/2023 1683/2002 1886/2196
+f 1514/1769 1893/2191 1884/1770
+f 1884/1770 1893/2191 1894/2197
+f 1885/2195 1693/2020 1886/2196
+f 1886/2196 1693/2020 1690/2023
+f 906/1103 1320/1554 1522/1777
+f 1522/1777 1320/1554 1887/2183
+f 1070/1575 1321/1568 1721/2198
+f 1721/2198 1321/1568 1899/2194
+f 1685/2182 1727/2199 1887/2183
+f 1887/2183 1727/2199 1888/2184
+f 1727/2199 1684/2185 1888/2184
+f 1888/2184 1684/2185 1889/2187
+f 1521/1979 1520/1978 1890/2188
+f 1890/2188 1520/1978 1889/2187
+f 1846/2200 1891/2189 1686/2186
+f 1686/2186 1891/2189 1890/2188
+f 1885/2192 1892/2193 1846/2200
+f 1846/2200 1892/2193 1891/2189
+f 1513/1767 1892/2193 1514/1769
+f 1514/1769 1892/2193 1893/2191
+f 1683/2201 1894/2197 1886/2190
+f 1886/2190 1894/2197 1893/2191
+f 1721/2198 1899/2194 1683/2201
+f 1683/2201 1899/2194 1894/2197
+f 1895/2202 1698/2171 1830/2130
+f 1830/2130 1698/2171 1771/2129
+f 1895/2202 1830/2130 1791/2025
+f 1791/2025 1830/2130 1829/2124
+f 1895/2202 1791/2025 1896/2203
+f 1896/2203 1791/2025 1813/2022
+f 1736/2033 1698/2171 1737/2034
+f 1737/2034 1698/2171 1895/2202
+f 1737/2034 1895/2202 1897/2166
+f 1897/2166 1895/2202 1896/2203
+f 1693/2020 1897/2166 1690/2023
+f 1690/2023 1897/2166 1896/2203
+f 1690/2023 1896/2203 1813/2022
+f 1816/2018 1737/2034 1732/2019
+f 1732/2019 1737/2034 1897/2166
+f 1884/1770 1894/2197 1515/1772
+f 1515/1772 1894/2197 1899/2194
+f 1372/1617 1900/1618 1360/1601
+f 1360/1601 1900/1618 1361/1603
+f 1383/1630 1901/1631 1372/1617
+f 1372/1617 1901/1631 1900/1618
+f 1394/1643 1902/1644 1383/1630
+f 1383/1630 1902/1644 1901/1631
+f 1405/1656 1903/1657 1394/1643
+f 1394/1643 1903/1657 1902/1644
+f 1405/1656 1416/1669 1903/1657
+f 1903/1657 1416/1669 351/866
+f 1427/1681 434/869 1416/1669
+f 1416/1669 434/869 351/866
+f 1498/1752 1904/1773 431/882
+f 1504/1758 1510/1764 1904/1773
+f 1904/1773 1510/1764 1905/1771
+f 1052/1181 987/1166 1906/1182
+f 1906/1182 987/1166 1907/1168
+f 1049/1167 909/1187 1908/1169
+f 1908/1169 909/1187 1909/1172
+f 1018/1171 952/1183 1908/1169
+f 1908/1169 952/1183 1907/1168
+f 1020/1184 926/1177 1906/1182
+f 1906/1182 926/1177 1910/1179
+f 1021/1189 924/1170 1911/1186
+f 1911/1186 924/1170 1909/1172
+f 1051/1174 910/1180 1912/1176
+f 1912/1176 910/1180 1910/1179
+f 1053/1185 911/1191 1911/1186
+f 1911/1186 911/1191 1913/1190
+f 1019/1178 953/1225 1912/1176
+f 1912/1176 953/1225 1914/1175
+f 1022/1194 925/1188 1915/1193
+f 1915/1193 925/1188 1913/1190
+f 991/1173 1914/1175 1056/1221
+f 1056/1221 1914/1175 1916/1222
+f 983/1202 1917/1196 1054/1192
+f 1054/1192 1917/1196 1915/1193
+f 929/1219 1918/1220 1027/1224
+f 1027/1224 1918/1220 1916/1222
+f 927/1195 1917/1196 1023/1197
+f 1023/1197 1917/1196 1919/1198
+f 913/1223 1918/1220 1050/1215
+f 1050/1215 1918/1220 1920/1216
+f 985/1205 1921/1200 1047/1201
+f 1047/1201 1921/1200 1919/1198
+f 931/1213 1922/1214 1026/1218
+f 1026/1218 1922/1214 1920/1216
+f 928/1199 1921/1200 1024/1206
+f 1024/1206 1921/1200 1923/1204
+f 989/1217 1922/1214 1055/1209
+f 1055/1209 1922/1214 1924/1210
+f 912/1211 1925/1208 1048/1203
+f 1048/1203 1925/1208 1923/1204
+f 930/1207 1925/1208 1025/1212
+f 1025/1212 1925/1208 1924/1210
+f 1663/1840 1927/1841 1523/1858
+f 1523/1858 1927/1841 1926/1843
+f 1666/1853 1929/1854 1601/1838
+f 1601/1838 1929/1854 1928/1839
+f 1632/1844 1927/1841 1566/1855
+f 1566/1855 1927/1841 1928/1839
+f 1635/1862 1930/1859 1538/1842
+f 1538/1842 1930/1859 1926/1843
+f 1634/1856 1929/1854 1540/1849
+f 1540/1849 1929/1854 1931/1850
+f 1667/1857 1930/1859 1525/1863
+f 1525/1863 1930/1859 1932/1861
+f 1665/1847 1933/1848 1524/1852
+f 1524/1852 1933/1848 1931/1850
+f 1636/1866 1934/1865 1539/1860
+f 1539/1860 1934/1865 1932/1861
+f 1633/1851 1933/1848 1567/1897
+f 1567/1897 1933/1848 1935/1846
+f 1597/1874 1668/1864 1936/1868
+f 1936/1868 1668/1864 1934/1865
+f 1605/1845 1670/1893 1935/1846
+f 1935/1846 1670/1893 1937/1895
+f 1541/1867 1637/1869 1936/1868
+f 1936/1868 1637/1869 1938/1871
+f 1543/1891 1641/1896 1939/1892
+f 1939/1892 1641/1896 1937/1895
+f 1599/1876 1661/1873 1940/1872
+f 1940/1872 1661/1873 1938/1871
+f 1527/1894 1664/1887 1939/1892
+f 1939/1892 1664/1887 1941/1889
+f 1542/1870 1638/1878 1940/1872
+f 1940/1872 1638/1878 1942/1877
+f 1545/1885 1640/1890 1943/1886
+f 1943/1886 1640/1890 1941/1889
+f 1526/1882 1662/1875 1944/1880
+f 1944/1880 1662/1875 1942/1877
+f 1603/1888 1669/1881 1943/1886
+f 1943/1886 1669/1881 1945/1883
+f 1544/1879 1639/1884 1944/1880
+f 1944/1880 1639/1884 1945/1883
+f 652/622 1946/743 2088/620
+f 2088/620 1946/743 2087/2204
+f 375/556 394/717 1965/550
+f 1965/550 394/717 1946/743
+f 2087/2204 1946/743 2108/795
+f 2108/795 1946/743 394/717
+f 402/677 522/676 1950/845
+f 1950/845 522/676 1949/846
+f 661/687 1951/844 580/680
+f 580/680 1951/844 1950/845
+f 403/685 525/683 1952/843
+f 1952/843 525/683 1951/844
+f 578/728 1953/841 579/688
+f 579/688 1953/841 1952/843
+f 578/728 2095/770 1953/841
+f 1953/841 2095/770 2097/842
+f 585/761 1955/840 420/814
+f 420/814 1955/840 1956/838
+f 660/602 1958/836 576/600
+f 576/600 1958/836 1957/837
+f 528/692 1958/836 405/693
+f 405/693 1958/836 1959/835
+f 659/619 1960/834 575/605
+f 575/605 1960/834 1959/835
+f 531/702 1960/834 408/703
+f 408/703 1960/834 1961/833
+f 574/617 658/740 1961/833
+f 1961/833 658/740 1962/832
+f 413/711 534/710 1963/831
+f 1963/831 534/710 1962/832
+f 672/804 1948/806 572/829
+f 572/829 1948/806 1964/830
+f 1471/1725 1482/1736 432/876
+f 432/876 1482/1736 348/877
+f 1460/1714 1471/1725 349/874
+f 349/874 1471/1725 432/876
+f 1449/1703 1460/1714 433/873
+f 433/873 1460/1714 349/874
+f 1438/1692 1449/1703 350/870
+f 350/870 1449/1703 433/873
+f 352/879 348/877 1492/1744
+f 1492/1744 348/877 1482/1736
+f 431/882 352/879 1498/1752
+f 1498/1752 352/879 1492/1744
+f 431/882 1904/1773 436/481
+f 436/481 1904/1773 1987/1096
+f 436/481 1987/1096 89/416
+f 89/416 1987/1096 321/1097
+f 434/869 1427/1681 350/870
+f 350/870 1427/1681 1438/1692
+f 2050/2205 1974/1606 2049/2206
+f 2049/2206 1974/1606 1973/1605
+f 351/866 435/480 1903/1657
+f 1903/1657 435/480 1975/1658
+f 2048/2207 2049/2206 1976/1619
+f 1976/1619 2049/2206 1973/1605
+f 2047/2208 2048/2207 1977/1632
+f 1977/1632 2048/2207 1976/1619
+f 2047/2208 1977/1632 2046/2209
+f 2046/2209 1977/1632 1978/1645
+f 2045/1670 2046/2209 1975/1658
+f 1975/1658 2046/2209 1978/1645
+f 685/454 2027/455 607/856
+f 607/856 2027/455 2028/2210
+f 311/125 1979/127 2025/451
+f 2025/451 1979/127 2026/453
+f 859/1052 5/411 868/1061
+f 868/1061 5/411 85/414
+f 859/1052 846/1040 5/411
+f 5/411 846/1040 1/409
+f 722/908 1332/907 1980/912
+f 1980/912 1332/907 1981/911
+f 4/400 88/401 775/2211
+f 775/2211 88/401 1982/968
+f 736/2212 1983/926 723/910
+f 723/910 1983/926 1980/912
+f 749/2213 1984/940 736/2212
+f 736/2212 1984/940 1983/926
+f 762/2214 1985/954 749/2213
+f 749/2213 1985/954 1984/940
+f 775/2211 1982/968 762/2214
+f 762/2214 1982/968 1985/954
+f 811/1004 799/992 2/406
+f 2/406 799/992 87/405
+f 2/406 86/408 811/1004
+f 811/1004 86/408 823/1016
+f 823/1016 86/408 835/1028
+f 835/1028 86/408 1/409
+f 846/1040 835/1028 1/409
+f 89/416 321/1097 2016/2215
+f 2016/2215 85/414 89/416
+f 1515/1772 1316/1570 1905/1771
+f 1905/1771 1316/1570 1986/1095
+f 1338/1093 1986/1095 898/1094
+f 898/1094 1986/1095 1316/1570
+f 244/12 1988/1 91/14
+f 91/14 1988/1 1989/2216
+f 92/15 1990/2 244/12
+f 244/12 1990/2 1988/1
+f 245/18 1991/4 92/15
+f 92/15 1991/4 1990/2
+f 300/19 1992/5 245/18
+f 245/18 1992/5 1991/4
+f 165/203 164/202 1994/2217
+f 1994/2217 164/202 1993/2218
+f 267/205 166/204 1996/2219
+f 1996/2219 166/204 1995/2220
+f 164/202 267/205 1993/2218
+f 1993/2218 267/205 1996/2219
+f 91/14 1989/2216 289/208
+f 289/208 1989/2216 1997/2221
+f 168/210 165/203 1998/7
+f 1998/7 165/203 1994/2217
+f 289/208 1997/2221 325/374
+f 325/374 1997/2221 1999/2222
+f 316/392 168/210 2000/6
+f 2000/6 168/210 1998/7
+f 325/374 1999/2222 166/204
+f 166/204 1999/2222 1995/2220
+f 1992/5 300/19 2019/8
+f 2019/8 300/19 2018/418
+f 316/392 2000/6 691/476
+f 691/476 2000/6 2002/2223
+f 623/485 438/486 2004/2224
+f 2004/2224 438/486 2003/2225
+f 439/488 623/485 2005/2226
+f 2005/2226 623/485 2004/2224
+f 624/490 439/488 2006/2227
+f 2006/2227 439/488 2005/2226
+f 624/490 2006/2227 441/492
+f 441/492 2006/2227 2007/10
+f 521/674 2008/2228 520/675
+f 520/675 2008/2228 2009/2229
+f 646/678 2010/2230 522/676
+f 522/676 2010/2230 2011/2231
+f 520/675 2009/2229 646/678
+f 646/678 2009/2229 2010/2230
+f 438/486 662/681 2003/2225
+f 2003/2225 662/681 2012/2232
+f 524/682 2013/2233 521/674
+f 521/674 2013/2233 2008/2228
+f 662/681 1949/846 2012/2232
+f 2012/2232 1949/846 2014/2234
+f 441/492 2007/10 673/848
+f 673/848 2007/10 2001/9
+f 691/476 2002/2223 524/682
+f 524/682 2002/2223 2013/2233
+f 1949/846 522/676 2014/2234
+f 2014/2234 522/676 2011/2231
+f 1989/2216 1988/1 2015/3
+f 1994/2217 1993/2218 2015/3
+f 1996/2219 1995/2220 2015/3
+f 1993/2218 1996/2219 2015/3
+f 1997/2221 1989/2216 2015/3
+f 1998/7 1994/2217 2015/3
+f 1999/2222 1997/2221 2015/3
+f 1995/2220 1999/2222 2015/3
+f 2002/2223 2000/6 2015/3
+f 2004/2224 2003/2225 2015/3
+f 2005/2226 2004/2224 2015/3
+f 2006/2227 2005/2226 2015/3
+f 2007/10 2006/2227 2015/3
+f 2009/2229 2008/2228 2015/3
+f 2011/2231 2010/2230 2015/3
+f 2010/2230 2009/2229 2015/3
+f 2003/2225 2012/2232 2015/3
+f 2008/2228 2013/2233 2015/3
+f 2012/2232 2014/2234 2015/3
+f 2013/2233 2002/2223 2015/3
+f 2014/2234 2011/2231 2015/3
+f 321/1097 885/1078 2016/2215
+f 2016/2215 885/1078 878/1071
+f 868/1061 85/414 878/1071
+f 878/1071 85/414 2016/2215
+f 1498/1752 1504/1758 1904/1773
+f 2001/9 2019/8 673/848
+f 673/848 2019/8 2018/418
+f 229/100 2021/2235 309/98
+f 309/98 2021/2235 2020/2236
+f 674/849 591/847 2022/420
+f 2022/420 591/847 2023/419
+f 591/847 673/848 2023/419
+f 2023/419 673/848 2018/418
+f 592/850 674/849 2024/421
+f 2024/421 674/849 2022/420
+f 592/850 2024/421 684/855
+f 684/855 2024/421 2025/451
+f 684/855 2025/451 605/452
+f 605/452 2025/451 2026/453
+f 1979/127 143/141 2026/453
+f 2026/453 143/141 2027/455
+f 143/141 230/143 2027/455
+f 2027/455 230/143 2028/2210
+f 607/857 2028/2237 688/858
+f 688/858 2028/2237 2029/2238
+f 688/858 2029/2238 608/859
+f 608/859 2029/2238 2030/2239
+f 608/859 2030/2239 689/860
+f 689/860 2030/2239 2031/2240
+f 689/860 2031/2240 604/853
+f 604/853 2031/2240 2032/2241
+f 683/854 604/853 2033/2242
+f 2033/2242 604/853 2032/2241
+f 602/851 683/854 2034/2243
+f 2034/2243 683/854 2033/2242
+f 602/851 2034/2243 682/852
+f 682/852 2034/2243 2020/2244
+f 2021/2235 229/100 2017/2245
+f 2017/2245 229/100 313/174
+f 42/160 2035/2246 313/174
+f 313/174 2035/2246 2017/2245
+f 312/159 2036/2247 42/160
+f 42/160 2036/2247 2035/2246
+f 312/152 30/88 2036/460
+f 2036/460 30/88 2037/437
+f 308/86 2038/435 30/88
+f 30/88 2038/435 2037/437
+f 228/89 2039/439 308/86
+f 308/86 2039/439 2038/435
+f 228/89 46/92 2039/439
+f 2039/439 46/92 2040/441
+f 46/92 163/394 2040/441
+f 2040/441 163/394 2041/472
+f 163/394 80/396 2041/472
+f 2041/472 80/396 2042/474
+f 234/399 2043/477 80/396
+f 80/396 2043/477 2042/474
+f 4/400 2044/479 234/399
+f 234/399 2044/479 2043/477
+f 775/2211 2045/1670 4/400
+f 4/400 2045/1670 2044/479
+f 762/2214 2046/2209 775/2211
+f 775/2211 2046/2209 2045/1670
+f 762/2214 749/2213 2046/2209
+f 2046/2209 749/2213 2047/2208
+f 749/2213 736/2212 2047/2208
+f 2047/2208 736/2212 2048/2207
+f 736/2212 723/910 2048/2207
+f 2048/2207 723/910 2049/2206
+f 723/910 709/909 2049/2206
+f 2049/2206 709/909 2050/2205
+f 908/1105 2050/2205 709/909
+f 1974/1606 2050/2205 908/1105
+f 908/1105 1981/911 1332/907
+f 2052/329 2066/2248 2051/331
+f 2051/331 2066/2248 2065/2249
+f 323/335 2068/2250 283/333
+f 283/333 2068/2250 2067/2251
+f 283/333 2067/2251 2052/329
+f 2052/329 2067/2251 2066/2248
+f 186/279 279/276 2069/2252
+f 2069/2252 279/276 2070/2253
+f 185/277 280/285 2071/2254
+f 2071/2254 280/285 2072/2255
+f 279/276 185/277 2070/2253
+f 2070/2253 185/277 2071/2254
+f 2053/300 186/279 2073/2256
+f 2073/2256 186/279 2069/2252
+f 2051/331 2065/2249 187/286
+f 187/286 2065/2249 2074/2257
+f 280/285 187/286 2072/2255
+f 2072/2255 187/286 2074/2257
+f 281/294 2076/287 188/296
+f 188/296 2076/287 2075/297
+f 2055/368 2078/2258 2054/290
+f 2054/290 2078/2258 2077/2259
+f 2054/290 2077/2259 281/292
+f 281/292 2077/2259 2076/2260
+f 188/296 2075/297 2056/303
+f 2056/303 2075/297 2079/301
+f 2055/368 2053/300 2078/2258
+f 2078/2258 2053/300 2073/2256
+f 2056/303 2079/301 189/305
+f 189/305 2079/301 2080/306
+f 189/305 2080/306 2057/309
+f 2057/309 2080/306 2081/307
+f 190/311 2057/309 2082/312
+f 2082/312 2057/309 2081/307
+f 2058/326 190/311 2083/324
+f 2083/324 190/311 2082/312
+f 192/321 282/322 2084/2261
+f 2084/2261 282/322 2085/2262
+f 191/318 2058/326 2086/314
+f 2086/314 2058/326 2083/324
+f 282/317 191/318 2085/313
+f 2085/313 191/318 2086/314
+f 323/335 192/321 2068/2250
+f 2068/2250 192/321 2084/2261
+f 1947/807 657/805 2087/2263
+f 2087/2263 657/805 2088/2264
+f 2059/802 2060/803 2089/2265
+f 2089/2265 2060/803 2090/2266
+f 657/805 2059/802 2088/2264
+f 2088/2264 2059/802 2089/2265
+f 555/749 2092/2267 654/756
+f 654/756 2092/2267 2091/2268
+f 556/751 2094/2269 653/747
+f 653/747 2094/2269 2093/2270
+f 653/747 2093/2270 555/749
+f 555/749 2093/2270 2092/2267
+f 2061/772 2095/2271 556/751
+f 556/751 2095/2271 2094/2269
+f 2060/803 557/758 2090/2266
+f 2090/2266 557/758 2096/2272
+f 654/756 2091/2268 557/758
+f 557/758 2091/2268 2096/2272
+f 1954/839 558/763 2097/2273
+f 2097/2273 558/763 2098/2274
+f 655/767 559/768 2099/759
+f 2099/759 559/768 2100/769
+f 558/763 655/764 2098/2274
+f 2098/2274 655/764 2099/2275
+f 559/768 2062/775 2100/769
+f 2100/769 2062/775 2101/773
+f 1954/839 2097/2273 2061/772
+f 2061/772 2097/2273 2095/2271
+f 2062/775 560/777 2101/773
+f 2101/773 560/777 2102/778
+f 560/777 2063/781 2102/778
+f 2102/778 2063/781 2103/779
+f 561/783 2104/784 2063/781
+f 2063/781 2104/784 2103/779
+f 2064/798 2105/796 561/783
+f 561/783 2105/796 2104/784
+f 562/790 2106/785 2064/798
+f 2064/798 2106/785 2105/796
+f 563/792 2108/2276 656/794
+f 656/794 2108/2276 2107/2277
+f 656/788 2107/786 562/790
+f 562/790 2107/786 2106/785
+f 1947/807 2087/2263 563/792
+f 563/792 2087/2263 2108/2276
+f 1687/2054 1688/1983 1808/1982
+f 1118/1312 1238/1311 1152/1314
diff --git a/data/duckCM.png b/data/duckCM.png
new file mode 100644
index 0000000..62d9200
Binary files /dev/null and b/data/duckCM.png differ
diff --git a/data/duck_vhacd.obj b/data/duck_vhacd.obj
new file mode 100644
index 0000000..54b8990
--- /dev/null
+++ b/data/duck_vhacd.obj
@@ -0,0 +1,609 @@
+o convex_0
+v -0.086018 0.502143 0.615873
+v -0.225720 0.427101 -0.544625
+v -0.268729 0.362660 -0.533833
+v 0.623016 0.276638 -0.017993
+v 0.601512 0.867587 -0.082516
+v -0.601818 0.867587 0.003477
+v -0.677007 0.126174 0.228965
+v 0.386620 0.244455 0.476150
+v 0.300756 0.137003 -0.351171
+v 0.569331 0.835252 0.282696
+v -0.677007 0.190691 -0.286648
+v -0.709188 0.631101 0.379366
+v -0.472792 0.706295 -0.383432
+v 0.483314 0.577413 -0.404901
+v -0.859718 0.545154 -0.050254
+v 0.150225 0.104745 0.432983
+v -0.386774 0.867587 0.293487
+v -0.537305 0.244455 0.486828
+v -0.247225 0.094067 -0.329701
+v 0.623016 0.588166 0.368574
+v -0.666179 0.534402 -0.394110
+v -0.257901 0.867587 -0.232917
+v -0.107523 0.620348 -0.512363
+v -0.290081 0.620348 0.572820
+v 0.386620 0.104745 0.121503
+v 0.666025 0.523649 -0.189864
+v -0.838214 0.287390 0.121503
+v 0.515647 0.856758 -0.200656
+v 0.161053 0.373413 -0.523041
+v 0.225414 0.577413 0.562029
+v -0.257901 0.362660 0.615873
+v -0.279405 0.094067 0.400949
+v 0.687529 0.502295 0.207382
+v -0.655503 0.405595 0.476150
+v 0.461810 0.330402 -0.394110
+v -0.633999 0.104745 -0.146925
+v 0.526323 0.222949 0.293374
+v -0.677007 0.770811 -0.168508
+v -0.161207 0.169261 -0.447841
+v 0.515647 0.867587 0.271904
+v -0.838214 0.620424 0.132295
+v 0.118045 0.341079 0.594290
+v -0.805881 0.287390 -0.168508
+v 0.311431 0.094067 -0.157716
+v -0.601818 0.287466 -0.415579
+v 0.676853 0.749306 0.024946
+v 0.483314 0.566660 0.486828
+v -0.193388 0.169261 0.519089
+v -0.752196 0.341154 0.357896
+v -0.644674 0.792241 0.250435
+v 0.128721 0.577413 -0.512250
+v 0.633845 0.652607 -0.254387
+v -0.580314 0.663359 0.454567
+v 0.472638 0.180014 -0.222239
+v -0.419107 0.566660 -0.490894
+v -0.118199 0.663359 0.540559
+v -0.504972 0.158508 -0.372527
+v -0.773701 0.179938 -0.039576
+v 0.289927 0.094067 0.304165
+v -0.397603 0.523649 0.583612
+v -0.483620 0.867587 -0.179186
+v 0.547827 0.362660 0.400949
+v -0.655503 0.169186 0.357896
+v -0.773701 0.727800 -0.039576
+f 41 50 64
+f 5 6 17
+f 6 5 22
+f 13 22 23
+f 22 5 28
+f 23 22 28
+f 3 2 29
+f 24 1 30
+f 4 26 33
+f 18 31 34
+f 9 29 35
+f 29 14 35
+f 32 7 36
+f 19 32 36
+f 25 4 37
+f 4 33 37
+f 13 21 38
+f 9 19 39
+f 3 29 39
+f 29 9 39
+f 10 5 40
+f 5 17 40
+f 30 10 40
+f 15 27 41
+f 16 8 42
+f 30 1 42
+f 1 31 42
+f 15 21 43
+f 27 15 43
+f 19 9 44
+f 32 19 44
+f 11 43 45
+f 43 21 45
+f 5 10 46
+f 10 20 46
+f 20 33 46
+f 33 26 46
+f 20 10 47
+f 10 30 47
+f 42 8 47
+f 30 42 47
+f 31 18 48
+f 32 16 48
+f 16 42 48
+f 42 31 48
+f 34 12 49
+f 12 41 49
+f 41 27 49
+f 17 6 50
+f 41 12 50
+f 2 23 51
+f 28 14 51
+f 23 28 51
+f 29 2 51
+f 14 29 51
+f 28 5 52
+f 14 28 52
+f 26 35 52
+f 35 14 52
+f 5 46 52
+f 46 26 52
+f 24 17 53
+f 12 34 53
+f 17 50 53
+f 50 12 53
+f 4 25 54
+f 26 4 54
+f 9 35 54
+f 35 26 54
+f 44 9 54
+f 25 44 54
+f 2 3 55
+f 21 13 55
+f 23 2 55
+f 13 23 55
+f 3 45 55
+f 45 21 55
+f 17 24 56
+f 24 30 56
+f 40 17 56
+f 30 40 56
+f 36 11 57
+f 19 36 57
+f 3 39 57
+f 39 19 57
+f 45 3 57
+f 11 45 57
+f 7 27 58
+f 11 36 58
+f 36 7 58
+f 43 11 58
+f 27 43 58
+f 8 16 59
+f 16 32 59
+f 37 8 59
+f 25 37 59
+f 44 25 59
+f 32 44 59
+f 1 24 60
+f 31 1 60
+f 34 31 60
+f 24 53 60
+f 53 34 60
+f 6 22 61
+f 22 13 61
+f 38 6 61
+f 13 38 61
+f 33 20 62
+f 8 37 62
+f 37 33 62
+f 47 8 62
+f 20 47 62
+f 27 7 63
+f 7 32 63
+f 18 34 63
+f 48 18 63
+f 32 48 63
+f 49 27 63
+f 34 49 63
+f 21 15 64
+f 6 38 64
+f 38 21 64
+f 15 41 64
+f 50 6 64
+o convex_1
+v -0.193580 1.125601 0.443872
+v -0.236532 1.179294 -0.383444
+v -0.236532 1.275851 -0.383444
+v -0.741519 1.222234 0.368646
+v -0.150542 1.619669 0.089200
+v -0.032381 0.867587 -0.007315
+v -0.709262 1.114847 -0.179140
+v -0.601710 1.544392 -0.125539
+v 0.139599 1.254419 -0.018086
+v -0.526501 0.867587 0.207508
+v -0.451206 1.533714 0.347105
+v -0.365302 0.867587 -0.232909
+v 0.021352 1.297359 0.314961
+v -0.139847 1.512131 -0.243679
+v -0.000124 1.104094 -0.275907
+v -0.043162 0.942863 0.261193
+v -0.440511 1.061079 0.454558
+v -0.505025 1.200802 -0.351133
+v -0.677005 0.953692 -0.028772
+v -0.300960 1.437006 0.422331
+v -0.741519 1.372559 0.175280
+v -0.215056 0.867587 0.293336
+v 0.053609 1.458514 -0.071770
+v -0.386864 1.641176 -0.028772
+v -0.365388 1.501453 -0.286593
+v -0.741519 1.093340 0.336418
+v 0.107342 1.039647 0.067827
+v -0.000124 1.480021 0.207340
+v -0.000124 1.340298 -0.265136
+v -0.612577 1.533714 0.164510
+v -0.150628 0.932109 -0.265136
+v -0.505025 1.050325 -0.329676
+v -0.537196 0.867587 -0.136226
+v -0.741519 1.275851 -0.082541
+v -0.558758 1.447760 -0.254366
+v -0.440511 1.190048 0.476184
+v -0.279570 1.608991 0.228881
+v 0.075085 1.028817 -0.125455
+v 0.064304 1.125601 0.261193
+v 0.118037 1.286605 0.153739
+v -0.193580 1.608991 -0.114684
+v -0.096809 1.404745 0.368646
+v -0.451206 1.630422 0.099886
+v -0.225837 1.050325 -0.361903
+v 0.096561 1.254419 -0.168453
+v -0.741519 1.082662 -0.050229
+v -0.515806 0.953692 0.347105
+v -0.644748 1.211556 -0.265136
+v -0.107590 1.028893 0.379332
+v -0.182799 1.555222 0.282734
+v -0.096895 0.867587 -0.157683
+v -0.397559 1.350976 -0.361903
+v -0.139847 1.232987 0.433185
+v -0.698481 1.447760 -0.050229
+v -0.666224 1.018139 -0.189910
+v -0.193580 1.404745 -0.340446
+v -0.096809 1.232987 -0.351133
+v -0.032381 1.555222 -0.028772
+v -0.580234 0.867587 0.110741
+v -0.354693 1.576729 -0.200765
+v -0.558758 1.523036 0.282650
+v 0.075085 1.437006 0.089200
+v -0.397559 1.480021 0.400874
+v -0.053943 0.867587 0.153655
+f 91 80 128
+f 70 74 76
+f 67 66 82
+f 74 70 86
+f 81 68 90
+f 68 85 90
+f 78 87 93
+f 82 66 96
+f 76 74 97
+f 96 76 97
+f 90 85 98
+f 72 89 99
+f 68 81 100
+f 81 65 100
+f 73 91 102
+f 91 70 102
+f 80 91 103
+f 103 91 104
+f 91 73 104
+f 92 77 104
+f 77 103 104
+f 88 69 105
+f 77 92 106
+f 69 88 107
+f 88 72 107
+f 72 94 107
+f 75 101 107
+f 101 69 107
+f 95 76 108
+f 79 95 108
+f 96 66 108
+f 76 96 108
+f 87 73 109
+f 79 93 109
+f 93 87 109
+f 73 102 109
+f 102 79 109
+f 83 90 110
+f 98 71 110
+f 90 98 110
+f 74 86 111
+f 86 81 111
+f 81 90 111
+f 90 74 111
+f 82 96 112
+f 71 98 112
+f 99 82 112
+f 65 81 113
+f 86 80 113
+f 81 86 113
+f 80 103 113
+f 92 69 114
+f 101 75 114
+f 69 101 114
+f 84 106 114
+f 106 92 114
+f 70 76 115
+f 76 95 115
+f 95 79 115
+f 79 102 115
+f 102 70 115
+f 67 82 116
+f 82 99 116
+f 99 89 116
+f 100 65 117
+f 84 100 117
+f 103 77 117
+f 77 106 117
+f 106 84 117
+f 65 113 117
+f 113 103 117
+f 85 94 118
+f 94 72 118
+f 98 85 118
+f 72 99 118
+f 112 98 118
+f 99 112 118
+f 97 83 119
+f 96 97 119
+f 110 71 119
+f 83 110 119
+f 112 96 119
+f 71 112 119
+f 89 78 120
+f 78 93 120
+f 67 116 120
+f 116 89 120
+f 66 67 121
+f 93 79 121
+f 108 66 121
+f 79 108 121
+f 67 120 121
+f 120 93 121
+f 87 78 122
+f 69 92 122
+f 105 69 122
+f 78 105 122
+f 74 90 123
+f 90 83 123
+f 97 74 123
+f 83 97 123
+f 72 88 124
+f 78 89 124
+f 89 72 124
+f 105 78 124
+f 88 105 124
+f 68 75 125
+f 85 68 125
+f 94 85 125
+f 75 107 125
+f 107 94 125
+f 73 87 126
+f 104 73 126
+f 92 104 126
+f 87 122 126
+f 122 92 126
+f 75 68 127
+f 68 100 127
+f 100 84 127
+f 114 75 127
+f 84 114 127
+f 86 70 128
+f 80 86 128
+f 70 91 128
+o convex_2
+v -0.881191 1.372585 0.314915
+v -0.752297 1.157735 -0.093320
+v -0.741541 1.157735 -0.093320
+v -0.741541 1.157735 0.357909
+v -0.945660 1.243704 0.078656
+v -0.741541 1.340341 0.046399
+v -0.956394 1.361845 0.100130
+v -0.891925 1.275925 0.347218
+v -0.741541 1.361845 0.185979
+v -0.870435 1.168500 0.035662
+v -0.945660 1.404830 0.228973
+v -0.741541 1.211460 0.379476
+v -0.806010 1.179240 0.357909
+v -0.752297 1.232964 -0.082537
+v -0.945660 1.254444 0.035662
+v -0.967172 1.351105 0.228973
+v -0.838256 1.157735 -0.039589
+v -0.902681 1.404830 0.207499
+v -0.902681 1.351105 0.336481
+v -0.827500 1.329601 0.336481
+v -0.848990 1.222224 0.368692
+v -0.741541 1.318861 0.003405
+v -0.924170 1.394066 0.153815
+v -0.827500 1.179240 -0.061063
+v -0.967172 1.318861 0.078656
+v -0.848990 1.297381 0.368692
+v -0.773787 1.157735 0.336481
+v -0.945660 1.297381 0.046399
+v -0.741541 1.243704 0.357909
+v -0.967172 1.383326 0.228973
+v -0.956394 1.265185 0.100130
+v -0.881191 1.394066 0.261230
+v -0.741541 1.361845 0.132341
+f 146 137 161
+f 130 131 132
+f 132 131 134
+f 132 134 137
+f 132 137 140
+f 133 138 141
+f 132 140 141
+f 131 130 142
+f 138 133 143
+f 130 132 145
+f 138 143 145
+f 129 139 147
+f 144 136 147
+f 137 129 148
+f 133 141 149
+f 141 140 149
+f 134 131 150
+f 131 142 150
+f 142 135 150
+f 135 139 151
+f 146 134 151
+f 139 146 151
+f 134 150 151
+f 150 135 151
+f 142 130 152
+f 130 145 152
+f 145 143 152
+f 129 147 154
+f 147 136 154
+f 148 129 154
+f 136 149 154
+f 149 140 154
+f 132 141 155
+f 141 138 155
+f 145 132 155
+f 138 145 155
+f 135 142 156
+f 142 152 156
+f 152 143 156
+f 153 135 156
+f 143 153 156
+f 140 137 157
+f 137 148 157
+f 148 154 157
+f 154 140 157
+f 139 135 158
+f 147 139 158
+f 144 147 158
+f 135 153 158
+f 153 144 158
+f 143 133 159
+f 136 144 159
+f 133 149 159
+f 149 136 159
+f 153 143 159
+f 144 153 159
+f 129 137 160
+f 139 129 160
+f 137 146 160
+f 146 139 160
+f 137 134 161
+f 134 146 161
+o convex_3
+v 0.343788 0.867587 0.153823
+v 0.623140 0.878339 0.057098
+v 0.623140 0.867587 0.057098
+v 0.429760 0.878339 -0.168537
+v 0.483464 0.953541 0.046370
+v 0.515649 0.878339 0.261277
+v 0.547888 0.867587 -0.147038
+v 0.408267 0.921300 -0.093312
+v 0.408267 0.921300 0.164552
+v 0.558634 0.910564 -0.093312
+v 0.558634 0.921300 0.164552
+v 0.354535 0.867587 -0.103998
+v 0.569381 0.867587 0.229007
+v 0.580127 0.921300 0.024913
+v 0.494156 0.932044 -0.093312
+v 0.376028 0.910564 0.024913
+v 0.429760 0.867587 0.250506
+v 0.472745 0.932044 0.175280
+v 0.537142 0.942788 0.024913
+v 0.408267 0.932044 0.024913
+v 0.429760 0.889084 0.239778
+v 0.515649 0.878339 -0.168537
+v 0.590874 0.878339 -0.082541
+v 0.343788 0.878339 0.153823
+v 0.354535 0.878339 -0.103998
+f 185 177 186
+f 164 162 168
+f 168 162 173
+f 165 168 173
+f 162 164 174
+f 164 163 174
+f 163 172 174
+f 172 167 174
+f 172 163 175
+f 169 166 176
+f 165 169 176
+f 162 174 178
+f 174 167 178
+f 166 170 179
+f 167 172 179
+f 172 166 179
+f 166 172 180
+f 175 171 180
+f 172 175 180
+f 176 166 180
+f 171 176 180
+f 166 169 181
+f 170 166 181
+f 169 177 181
+f 177 170 181
+f 178 167 182
+f 179 170 182
+f 167 179 182
+f 168 165 183
+f 171 168 183
+f 165 176 183
+f 176 171 183
+f 163 164 184
+f 164 168 184
+f 168 171 184
+f 175 163 184
+f 171 175 184
+f 173 162 185
+f 170 177 185
+f 162 178 185
+f 182 170 185
+f 178 182 185
+f 169 165 186
+f 165 173 186
+f 177 169 186
+f 173 185 186
+o convex_4
+v -0.924164 1.082508 0.121620
+v -0.741519 1.136206 0.357985
+v -0.752291 1.136206 0.357985
+v -0.741519 1.157711 -0.082575
+v -0.741519 1.061012 0.089363
+v -0.881191 1.157711 0.089363
+v -0.902678 1.061012 0.250519
+v -0.902678 1.125462 0.261257
+v -0.816750 1.114719 -0.050317
+v -0.752291 1.157711 0.336466
+v -0.741519 1.071765 0.271952
+v -0.902678 1.146949 0.057149
+v -0.773777 1.082508 -0.018060
+v -0.902678 1.061012 0.143009
+v -0.795264 1.093251 0.336466
+v -0.827503 1.146949 0.325728
+v -0.924164 1.136206 0.164529
+v -0.806017 1.157711 -0.071793
+v -0.741519 1.093251 -0.039580
+v -0.870438 1.103995 0.304252
+v -0.934917 1.103995 0.153747
+v -0.741519 1.061012 0.207480
+f 197 193 208
+f 190 188 191
+f 189 188 196
+f 188 190 196
+f 190 192 196
+f 191 188 197
+f 195 187 198
+f 195 199 200
+f 193 187 200
+f 191 193 200
+f 187 195 200
+f 199 191 200
+f 188 189 201
+f 197 188 201
+f 193 197 201
+f 189 196 202
+f 196 192 202
+f 192 198 203
+f 202 192 203
+f 194 202 203
+f 192 190 204
+f 190 195 204
+f 198 192 204
+f 195 198 204
+f 190 191 205
+f 195 190 205
+f 191 199 205
+f 199 195 205
+f 194 193 206
+f 201 189 206
+f 193 201 206
+f 202 194 206
+f 189 202 206
+f 187 193 207
+f 193 194 207
+f 198 187 207
+f 194 203 207
+f 203 198 207
+f 193 191 208
+f 191 197 208
diff --git a/data/duck_vhacd.urdf b/data/duck_vhacd.urdf
new file mode 100644
index 0000000..8d9c4be
--- /dev/null
+++ b/data/duck_vhacd.urdf
@@ -0,0 +1,32 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <rolling_friction value="0.0"/>
+ <contact_cfm value="0.0"/>
+ <contact_erp value="1.0"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0 0.02 0.0"/>
+ <mass value=".1"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="duck.obj" scale=".05 .05 .05"/>
+ </geometry>
+ <material name="yellow">
+ <color rgba="1 1 0.4 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="duck_vhacd.obj" scale=".05 .05 .05"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/cube.mtl b/data/floor.mtl
similarity index 74%
copy from data/cube.mtl
copy to data/floor.mtl
index 935e48a..b59e224 100644
--- a/data/cube.mtl
+++ b/data/floor.mtl
@@ -1,4 +1,4 @@
-newmtl cube
+newmtl floor
Ns 10.0000
Ni 1.5000
d 1.0000
@@ -9,6 +9,5 @@ newmtl cube
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
- map_Ka cube.png
- map_Kd cube.png
-
+ map_Ka floor_diffuse.jpg
+ map_Kd checker_huge.gif
diff --git a/data/floor.obj b/data/floor.obj
new file mode 100644
index 0000000..42e1458
--- /dev/null
+++ b/data/floor.obj
@@ -0,0 +1,18 @@
+o
+mtllib floor.mtl
+v -1 -1 -1
+v 1 -1 -1
+v 1 -1 1
+v -1 -1 1
+
+vt 0 0
+vt 1 0
+vt 1 1
+vt 0 1
+
+vn 0 1 0
+usemtl floor
+
+f 3/3/1 2/2/1 1/1/1
+f 4/4/1 3/3/1 1/1/1
+
diff --git a/data/floor_diffuse.jpg b/data/floor_diffuse.jpg
new file mode 100644
index 0000000..36a2625
Binary files /dev/null and b/data/floor_diffuse.jpg differ
diff --git a/data/floor_diffuse.tga b/data/floor_diffuse.tga
new file mode 100644
index 0000000..7bb70dd
Binary files /dev/null and b/data/floor_diffuse.tga differ
diff --git a/data/floor_nm_tangent.tga b/data/floor_nm_tangent.tga
new file mode 100644
index 0000000..3b8474e
Binary files /dev/null and b/data/floor_nm_tangent.tga differ
diff --git a/data/gripper/meshes/GUIDE_WSG50_110.stl b/data/gripper/meshes/GUIDE_WSG50_110.stl
new file mode 100644
index 0000000..a7b5847
Binary files /dev/null and b/data/gripper/meshes/GUIDE_WSG50_110.stl differ
diff --git a/data/gripper/meshes/WSG-FMF.stl b/data/gripper/meshes/WSG-FMF.stl
new file mode 100644
index 0000000..f61a9cf
Binary files /dev/null and b/data/gripper/meshes/WSG-FMF.stl differ
diff --git a/data/gripper/meshes/WSG50_110.stl b/data/gripper/meshes/WSG50_110.stl
new file mode 100644
index 0000000..fee723b
Binary files /dev/null and b/data/gripper/meshes/WSG50_110.stl differ
diff --git a/data/gripper/meshes/l_gripper_tip_scaled.stl b/data/gripper/meshes/l_gripper_tip_scaled.stl
new file mode 100644
index 0000000..e38df0a
Binary files /dev/null and b/data/gripper/meshes/l_gripper_tip_scaled.stl differ
diff --git a/data/gripper/wsg50_one_motor_gripper.sdf b/data/gripper/wsg50_one_motor_gripper.sdf
new file mode 100644
index 0000000..1f19f17
--- /dev/null
+++ b/data/gripper/wsg50_one_motor_gripper.sdf
@@ -0,0 +1,388 @@
+<?xml version="1.0" ?>
+<sdf version='1.6'>
+ <world name='default'>
+ <model name='wsg50_with_gripper'>
+ <pose frame=''>0 0 0.26 3.14 0 0</pose>
+
+ <link name='world'>
+ </link>
+
+ <joint name='base_joint' type='prismatic'>
+ <parent>world</parent>
+ <child>base_link</child>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-0.5</lower>
+ <upper>10</upper>
+ <effort>1</effort>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='base_link'>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <mass>1.2</mass>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='base_link_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+
+ </material>
+ </visual>
+
+ </link>
+
+ <link name='motor'>
+ <pose frame=''>0 0 0.03 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>0 0 0.01 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.02 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='base_joint_motor' type='prismatic'>
+ <child>motor</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-0.047</lower>
+ <upper>0.001</upper>
+ <effort>10.0</effort>
+ <velocity>10.0</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='left_hinge'>
+ <pose frame=''>0 0 0.04 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.035 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.07 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='motor_left_hinge_joint' type='revolute'>
+ <child>left_hinge</child>
+ <parent>motor</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-20.0</lower>
+ <upper>20.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='right_hinge'>
+ <pose frame=''>0 0 0.04 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.035 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>0.03 0 0.01 0 1.2 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.07 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='motor_right_hinge_joint' type='revolute'>
+ <child>right_hinge</child>
+ <parent>motor</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-20.0</lower>
+ <upper>20.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='gripper_left'>
+ <pose frame=''>-0.055 0 0.06 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.0115 0 -0 0</pose>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='gripper_left_visual'>
+ <pose frame=''>0 0 -0.06 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
+ <pose frame=''>0 0 -0.037 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+
+ </link>
+
+ <joint name='gripper_left_hinge_joint' type='revolute'>
+ <child>gripper_left</child>
+ <parent>left_hinge</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-1.0</lower>
+ <upper>1.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.01</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='gripper_right'>
+ <pose frame=''>0.055 0 0.06 0 0 3.14159</pose>
+ <inertial>
+ <pose frame=''>0 0 0.0115 0 -0 0</pose>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='gripper_right_visual'>
+ <pose frame=''>0 0 -0.06 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
+ <pose frame=''>0 0 -0.037 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_right_hinge_joint' type='revolute'>
+ <child>gripper_right</child>
+ <parent>right_hinge</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-1.0</lower>
+ <upper>1.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.01</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='finger_right'>
+ <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
+ <inertial>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='finger_right_collision'>
+ <pose frame=''>0 0 0.042 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.15 </size>
+ </box>
+
+ </geometry>
+ </collision>
+
+ <visual name='finger_right_visual'>
+ <pose frame=''>0 0 0 0 0 0 </pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1 </scale>
+ <uri>meshes/l_gripper_tip_scaled.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_finger_right' type='fixed'>
+ <parent>gripper_right</parent>
+ <child>finger_right</child>
+ </joint>
+
+ <link name='finger_left'>
+ <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
+ <inertial>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='finger_left_collision'>
+ <pose frame=''>0 0 0.042 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.15 </size>
+ </box>
+
+ </geometry>
+ </collision>
+
+ <visual name='finger_left_visual'>
+ <pose frame=''>0 0 0 0 0 0 </pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1 </scale>
+ <uri>meshes/l_gripper_tip_scaled.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_finger_left' type='fixed'>
+ <parent>gripper_left</parent>
+ <child>finger_left</child>
+ </joint>
+ </model>
+</world>
+</sdf>
\ No newline at end of file
diff --git a/data/gripper/wsg50_one_motor_gripper_free_base.sdf b/data/gripper/wsg50_one_motor_gripper_free_base.sdf
new file mode 100644
index 0000000..7fc3ed2
--- /dev/null
+++ b/data/gripper/wsg50_one_motor_gripper_free_base.sdf
@@ -0,0 +1,386 @@
+<?xml version="1.0" ?>
+<sdf version='1.6'>
+ <world name='default'>
+ <model name='wsg50_with_gripper'>
+
+ <pose frame=''>0 -2.3 2.1 0 0 0</pose>
+ <link name='world'>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ </inertial>
+ </link>
+
+ <joint name='base_joint' type='fixed'>
+ <parent>world</parent>
+ <child>base_link</child>
+ </joint>
+
+ <link name='base_link'>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <mass>1.2</mass>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='base_link_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+
+ </material>
+ </visual>
+
+ </link>
+
+ <link name='motor'>
+ <pose frame=''>0 0 0.03 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>0 0 0.01 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.02 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='base_joint_motor' type='prismatic'>
+ <child>motor</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-0.047</lower>
+ <upper>0.001</upper>
+ <effort>10.0</effort>
+ <velocity>10.0</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='left_hinge'>
+ <pose frame=''>0 0 0.04 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.035 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.07 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='motor_left_hinge_joint' type='revolute'>
+ <child>left_hinge</child>
+ <parent>motor</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-20.0</lower>
+ <upper>20.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='right_hinge'>
+ <pose frame=''>0 0 0.04 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.035 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>0.03 0 0.01 0 1.2 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.07 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='motor_right_hinge_joint' type='revolute'>
+ <child>right_hinge</child>
+ <parent>motor</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-20.0</lower>
+ <upper>20.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='gripper_left'>
+ <pose frame=''>-0.055 0 0.06 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.0115 0 -0 0</pose>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='gripper_left_visual'>
+ <pose frame=''>0 0 -0.06 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
+ <pose frame=''>0 0 -0.037 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+
+ </link>
+
+ <joint name='gripper_left_hinge_joint' type='revolute'>
+ <child>gripper_left</child>
+ <parent>left_hinge</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-4.0</lower>
+ <upper>4.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.01</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='gripper_right'>
+ <pose frame=''>0.055 0 0.06 0 0 3.14159</pose>
+ <inertial>
+ <pose frame=''>0 0 0.0115 0 -0 0</pose>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='gripper_right_visual'>
+ <pose frame=''>0 0 -0.06 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
+ <pose frame=''>0 0 -0.037 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_right_hinge_joint' type='revolute'>
+ <child>gripper_right</child>
+ <parent>right_hinge</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-4.0</lower>
+ <upper>4.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.01</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='finger_right'>
+ <pose frame=''>0.042 0 0.145 0 0 1.5708</pose>
+ <inertial>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='finger_right_collision'>
+ <pose frame=''>0 0 0.042 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.15 </size>
+ </box>
+
+ </geometry>
+ </collision>
+
+ <visual name='finger_right_visual'>
+ <pose frame=''>0 0 0 0 0 0 </pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1 </scale>
+ <uri>meshes/l_gripper_tip_scaled.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_finger_right' type='fixed'>
+ <parent>gripper_right</parent>
+ <child>finger_right</child>
+ </joint>
+
+ <link name='finger_left'>
+ <pose frame=''>-0.042 0 0.145 0 0 4.71239</pose>
+ <inertial>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='finger_left_collision'>
+ <pose frame=''>0 0 0.042 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.15 </size>
+ </box>
+
+ </geometry>
+ </collision>
+
+ <visual name='finger_left_visual'>
+ <pose frame=''>0 0 0 0 0 0 </pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1 </scale>
+ <uri>meshes/l_gripper_tip_scaled.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_finger_left' type='fixed'>
+ <parent>gripper_left</parent>
+ <child>finger_left</child>
+ </joint>
+ </model>
+</world>
+</sdf>
\ No newline at end of file
diff --git a/data/gripper/wsg50_one_motor_gripper_new.sdf b/data/gripper/wsg50_one_motor_gripper_new.sdf
new file mode 100644
index 0000000..a3a11f3
--- /dev/null
+++ b/data/gripper/wsg50_one_motor_gripper_new.sdf
@@ -0,0 +1,386 @@
+<?xml version="1.0" ?>
+<sdf version='1.6'>
+ <world name='default'>
+ <model name='wsg50_with_gripper'>
+ <pose frame=''>0 0 0.26 3.14 0 0</pose>
+
+ <link name='world'>
+ </link>
+
+ <joint name='base_joint' type='prismatic'>
+ <parent>world</parent>
+ <child>base_link</child>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-0.5</lower>
+ <upper>10</upper>
+ <effort>1</effort>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='base_link'>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <mass>1.2</mass>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='base_link_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+
+ </material>
+ </visual>
+
+ </link>
+
+ <link name='motor'>
+ <pose frame=''>0 0 0.03 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>0 0 0.01 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.02 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='base_joint_motor' type='prismatic'>
+ <child>motor</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-0.055</lower>
+ <upper>0.001</upper>
+ <effort>10.0</effort>
+ <velocity>10.0</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='left_hinge'>
+ <pose frame=''>0 0 0.04 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.035 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.07 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='motor_left_hinge_joint' type='revolute'>
+ <child>left_hinge</child>
+ <parent>motor</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-20.0</lower>
+ <upper>20.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='right_hinge'>
+ <pose frame=''>0 0 0.04 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.035 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>0.03 0 0.01 0 1.2 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.07 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='motor_right_hinge_joint' type='revolute'>
+ <child>right_hinge</child>
+ <parent>motor</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-20.0</lower>
+ <upper>20.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='gripper_left'>
+ <pose frame=''>-0.055 0 0.06 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.0115 0 -0 0</pose>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='gripper_left_visual'>
+ <pose frame=''>0 0 -0.06 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
+ <pose frame=''>0 0 -0.037 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+
+ </link>
+
+ <joint name='gripper_left_hinge_joint' type='prismatic'>
+ <child>gripper_left</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>-0.01</lower>
+ <upper>0.05</upper>
+ <effort>1</effort>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='gripper_right'>
+ <pose frame=''>0.055 0 0.06 0 0 3.14159</pose>
+ <inertial>
+ <pose frame=''>0 0 0.0115 0 -0 0</pose>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='gripper_right_visual'>
+ <pose frame=''>0 0 -0.06 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
+ <pose frame=''>0 0 -0.037 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_right_hinge_joint' type='prismatic'>
+ <child>gripper_right</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>-0.01</lower>
+ <upper>0.05</upper>
+ <effort>1</effort>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='finger_right'>
+ <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
+ <inertial>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='finger_right_collision'>
+ <pose frame=''>0 0 0.042 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.15 </size>
+ </box>
+
+ </geometry>
+ </collision>
+
+ <visual name='finger_right_visual'>
+ <pose frame=''>0 0 0 0 0 0 </pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1 </scale>
+ <uri>meshes/l_gripper_tip_scaled.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_finger_right' type='fixed'>
+ <parent>gripper_right</parent>
+ <child>finger_right</child>
+ </joint>
+
+ <link name='finger_left'>
+ <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
+ <inertial>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='finger_left_collision'>
+ <pose frame=''>0 0 0.042 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.15 </size>
+ </box>
+
+ </geometry>
+ </collision>
+
+ <visual name='finger_left_visual'>
+ <pose frame=''>0 0 0 0 0 0 </pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1 </scale>
+ <uri>meshes/l_gripper_tip_scaled.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_finger_left' type='fixed'>
+ <parent>gripper_left</parent>
+ <child>finger_left</child>
+ </joint>
+ </model>
+</world>
+</sdf>
\ No newline at end of file
diff --git a/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf b/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf
new file mode 100644
index 0000000..68e2a0a
--- /dev/null
+++ b/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf
@@ -0,0 +1,383 @@
+<?xml version="1.0" ?>
+<sdf version='1.6'>
+ <world name='default'>
+ <model name='wsg50_with_gripper'>
+ <pose frame=''>1.4 -0.2 2.1 0 0 0</pose>
+ <link name='world'>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ </inertial>
+ </link>
+
+ <joint name='base_joint' type='fixed'>
+ <parent>world</parent>
+ <child>base_link</child>
+ </joint>
+
+ <link name='base_link'>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <mass>1.2</mass>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='base_link_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+
+ </material>
+ </visual>
+
+ </link>
+
+ <link name='motor'>
+ <pose frame=''>0 0 0.03 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>0 0 0.01 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.02 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='base_joint_motor' type='prismatic'>
+ <child>motor</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-0.055</lower>
+ <upper>0.001</upper>
+ <effort>10.0</effort>
+ <velocity>10.0</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='left_hinge'>
+ <pose frame=''>0 0 0.04 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.035 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.07 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='motor_left_hinge_joint' type='revolute'>
+ <child>left_hinge</child>
+ <parent>motor</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-20.0</lower>
+ <upper>20.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='right_hinge'>
+ <pose frame=''>0 0 0.04 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.035 0 0 0</pose>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+ <visual name='motor_visual'>
+ <pose frame=''>0.03 0 0.01 0 1.2 0</pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.07 </size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='motor_right_hinge_joint' type='revolute'>
+ <child>right_hinge</child>
+ <parent>motor</parent>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>-20.0</lower>
+ <upper>20.0</upper>
+ <effort>10</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+
+ <link name='gripper_left'>
+ <pose frame=''>-0.055 0 0.06 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.0115 0 -0 0</pose>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='gripper_left_visual'>
+ <pose frame=''>0 0 -0.06 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
+ <pose frame=''>0 0 -0.037 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+
+ </link>
+
+ <joint name='gripper_left_hinge_joint' type='prismatic'>
+ <child>gripper_left</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>-0.01</lower>
+ <upper>0.05</upper>
+ <effort>1</effort>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='gripper_right'>
+ <pose frame=''>0.055 0 0.06 0 0 3.14159</pose>
+ <inertial>
+ <pose frame=''>0 0 0.0115 0 -0 0</pose>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='gripper_right_visual'>
+ <pose frame=''>0 0 -0.06 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
+ <pose frame=''>0 0 -0.037 0 0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_right_hinge_joint' type='prismatic'>
+ <child>gripper_right</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>-0.01</lower>
+ <upper>0.05</upper>
+ <effort>1</effort>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='finger_right'>
+ <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
+ <inertial>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='finger_right_collision'>
+ <pose frame=''>0 0 0.042 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.15 </size>
+ </box>
+
+ </geometry>
+ </collision>
+
+ <visual name='finger_right_visual'>
+ <pose frame=''>0 0 0 0 0 0 </pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1 </scale>
+ <uri>meshes/l_gripper_tip_scaled.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_finger_right' type='fixed'>
+ <parent>gripper_right</parent>
+ <child>finger_right</child>
+ </joint>
+
+ <link name='finger_left'>
+ <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
+ <inertial>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='finger_left_collision'>
+ <pose frame=''>0 0 0.042 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.15 </size>
+ </box>
+
+ </geometry>
+ </collision>
+
+ <visual name='finger_left_visual'>
+ <pose frame=''>0 0 0 0 0 0 </pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1 </scale>
+ <uri>meshes/l_gripper_tip_scaled.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_finger_left' type='fixed'>
+ <parent>gripper_left</parent>
+ <child>finger_left</child>
+ </joint>
+ </model>
+</world>
+</sdf>
\ No newline at end of file
diff --git a/data/gripper/wsg50_with_r2d2_gripper.sdf b/data/gripper/wsg50_with_r2d2_gripper.sdf
new file mode 100644
index 0000000..b022faf
--- /dev/null
+++ b/data/gripper/wsg50_with_r2d2_gripper.sdf
@@ -0,0 +1,298 @@
+<?xml version="1.0" ?>
+<sdf version='1.6'>
+ <world name='default'>
+ <model name='wsg50_with_gripper'>
+ <pose frame=''>0 0 0.27 3.14 0 0</pose>
+
+ <link name='world'>
+ </link>
+
+ <joint name='base_joint' type='prismatic'>
+ <parent>world</parent>
+ <child>base_link</child>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-0.5</lower>
+ <upper>10</upper>
+ <effort>1</effort>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>100</damping>
+ <friction>100</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='base_link'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <mass>1.2</mass>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ </inertial>
+ <collision name='base_link_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <box>
+ <size>0.2 0.05 0.05 </size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name='base_link_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+
+ </material>
+ </visual>
+ <gravity>1</gravity>
+ <velocity_decay/>
+ <self_collide>0</self_collide>
+ </link>
+
+ <link name='gripper_left'>
+ <pose frame=''>-0.055 0 0 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.0115 0 -0 0</pose>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='gripper_left_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <collision name='gripper_left_fixed_joint_lump__finger_left_collision_1'>
+ <pose frame=''>0 0 0.023 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+
+ <visual name='gripper_left_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
+ <pose frame=''>0 0 0.023 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+
+ </link>
+ <joint name='base_joint_gripper_left' type='prismatic'>
+ <child>gripper_left</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>-0.001</lower>
+ <upper>0.055</upper>
+ <effort>1</effort>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>100</damping>
+ <friction>100</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='gripper_right'>
+ <pose frame=''>0.055 0 0 0 -0 3.14159</pose>
+ <inertial>
+ <pose frame=''>0 0 0.0115 0 -0 0</pose>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='gripper_right_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <collision name='gripper_right_fixed_joint_lump__finger_right_collision_1'>
+ <pose frame=''>0 0 0.023 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+
+ <visual name='gripper_right_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/GUIDE_WSG50_110.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
+ <pose frame=''>0 0 0.023 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>meshes/WSG-FMF.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='base_joint_gripper_right' type='prismatic'>
+ <child>gripper_right</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>-1 0 0</xyz>
+ <limit>
+ <lower>-0.055</lower>
+ <upper>0.001</upper>
+ <effort>1</effort>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>100</damping>
+ <friction>100</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <link name='finger_right'>
+ <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
+ <inertial>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='finger_right_collision'>
+ <pose frame=''>0 0 0.042 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.15 </size>
+ </box>
+
+ </geometry>
+ </collision>
+
+ <visual name='finger_right_visual'>
+ <pose frame=''>0 0 0 0 0 0 </pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1 </scale>
+ <uri>meshes/l_gripper_tip_scaled.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_finger_right' type='fixed'>
+ <parent>gripper_right</parent>
+ <child>finger_right</child>
+ </joint>
+
+ <link name='finger_left'>
+ <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
+ <inertial>
+ <mass>0.2</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ </inertia>
+ </inertial>
+
+ <collision name='finger_left_collision'>
+ <pose frame=''>0 0 0.042 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.02 0.02 0.15 </size>
+ </box>
+
+ </geometry>
+ </collision>
+
+ <visual name='finger_left_visual'>
+ <pose frame=''>0 0 0 0 0 0 </pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1 </scale>
+ <uri>meshes/l_gripper_tip_scaled.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='gripper_finger_left' type='fixed'>
+ <parent>gripper_left</parent>
+ <child>finger_left</child>
+ </joint>
+ </model>
+</world>
+</sdf>
diff --git a/data/husky/husky.urdf b/data/husky/husky.urdf
new file mode 100644
index 0000000..4fab377
--- /dev/null
+++ b/data/husky/husky.urdf
@@ -0,0 +1,373 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from ../urdf/husky.urdf.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<!--
+Software License Agreement (BSD)
+
+\file husky.urdf.xacro
+\authors Paul Bovbel <pbovbel at clearpathrobotics.com>
+\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+ following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
+ following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics 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 WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, 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.
+-->
+<robot name="husky_robot" xmlns:xacro="http://ros.org/wiki/xacro">
+ <!-- Included URDF/XACRO Files -->
+ <material name="Black">
+ <color rgba="0.1 0.1 0.1 1.0"/>
+ </material>
+ <material name="Blue">
+ <color rgba="0.0 0.0 0.8 1.0"/>
+ </material>
+ <material name="Green">
+ <color rgba="0.0 0.8 0.0 1.0"/>
+ </material>
+ <material name="Grey">
+ <color rgba="0.7 0.7 0.7 1.0"/>
+ </material>
+ <material name="DarkGrey">
+ <color rgba="0.3 0.3 0.3 1.0"/>
+ </material>
+ <material name="Red">
+ <color rgba="0.8 0.0 0.0 1.0"/>
+ </material>
+ <material name="White">
+ <color rgba="1.0 1.0 1.0 1.0"/>
+ </material>
+ <material name="Yellow">
+ <color rgba="0.8 0.8 0.0 1.0"/>
+ </material>
+ <!-- Base Size -->
+ <!-- Wheel Mounting Positions -->
+ <!-- Wheel Properties -->
+ <!-- Base link is on the ground under the robot -->
+ <link name="base_footprint"/>
+ <gazebo reference="base_footprint">
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <joint name="chassis_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0.14493"/>
+ <parent link="base_footprint"/>
+ <child link="base_link"/>
+ </joint>
+ <!-- Chassis link is the center of the robot's bottom plate -->
+ <link name="base_link">
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/base_link.stl"/>
+ </geometry>
+ <material name="Black"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0.12498"/>
+ <geometry>
+ <!-- Make collision box slightly bigger in x and z directions. -->
+ <box size="1.0074 0.5709 0.2675"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="33.455"/>
+ <origin xyz="-0.08748 -0.00085 0.09947"/>
+ <inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296"/>
+ </inertial>
+ </link>
+ <gazebo reference="base_link">
+ </gazebo>
+ <!-- IMU Link is the standard mounting position for the UM6 IMU.-->
+ <!-- Can be modified with environment variables in /etc/ros/setup.bash -->
+ <link name="imu_link"/>
+ <joint name="imu_joint" type="fixed">
+ <origin rpy="$(optenv HUSKY_IMU_RPY 0 -1.5708 3.1416)" xyz="$(optenv HUSKY_IMU_XYZ 0.19 0 0.149)"/>
+ <parent link="base_link"/>
+ <child link="imu_link"/>
+ </joint>
+ <gazebo reference="imu_link">
+ </gazebo>
+ <!-- Husky wheel macros -->
+ <link name="front_left_wheel_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <rolling_friction value="0.0"/>
+ <stiffness value="30000"/>
+ <damping value="1000"/>
+ </contact>
+
+ <inertial>
+ <mass value="2.637"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/wheel.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="1.570795 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1143" radius="0.17775"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="front_left_wheel_link">
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1.0"/>
+ <fdir1 value="1 0 0"/>
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>false</turnGravityOff>
+ </gazebo>
+ <joint name="front_left_wheel" type="continuous">
+ <parent link="base_link"/>
+ <child link="front_left_wheel_link"/>
+ <origin rpy="0 0 0" xyz="0.256 0.2854 0.03282"/>
+ <axis rpy="0 0 0" xyz="0 1 0"/>
+ </joint>
+ <transmission name="front_left_wheel_trans" type="SimpleTransmission">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="front_left_wheel_motor">
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ <joint name="front_left_wheel">
+ <hardwareInterface>VelocityJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <link name="front_right_wheel_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <rolling_friction value="0.0"/>
+ <stiffness value="30000"/>
+ <damping value="1000"/>
+ </contact>
+
+ <inertial>
+ <mass value="2.637"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/wheel.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="1.570795 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1143" radius="0.17775"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="front_right_wheel_link">
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1.0"/>
+ <fdir1 value="1 0 0"/>
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>false</turnGravityOff>
+ </gazebo>
+ <joint name="front_right_wheel" type="continuous">
+ <parent link="base_link"/>
+ <child link="front_right_wheel_link"/>
+ <origin rpy="0 0 0" xyz="0.256 -0.2854 0.03282"/>
+ <axis rpy="0 0 0" xyz="0 1 0"/>
+ </joint>
+ <transmission name="front_right_wheel_trans" type="SimpleTransmission">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="front_right_wheel_motor">
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ <joint name="front_right_wheel">
+ <hardwareInterface>VelocityJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <link name="rear_left_wheel_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <rolling_friction value="0.0"/>
+ <stiffness value="30000"/>
+ <damping value="1000"/>
+ </contact>
+
+ <inertial>
+ <mass value="2.637"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/wheel.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="1.570795 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1143" radius="0.17775"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="rear_left_wheel_link">
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1.0"/>
+ <fdir1 value="1 0 0"/>
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>false</turnGravityOff>
+ </gazebo>
+ <joint name="rear_left_wheel" type="continuous">
+ <parent link="base_link"/>
+ <child link="rear_left_wheel_link"/>
+ <origin rpy="0 0 0" xyz="-0.256 0.2854 0.03282"/>
+ <axis rpy="0 0 0" xyz="0 1 0"/>
+ </joint>
+ <transmission name="rear_left_wheel_trans" type="SimpleTransmission">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="rear_left_wheel_motor">
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ <joint name="rear_left_wheel">
+ <hardwareInterface>VelocityJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <link name="rear_right_wheel_link">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <rolling_friction value="0.0"/>
+ <stiffness value="30000"/>
+ <damping value="1000"/>
+ </contact>
+
+ <inertial>
+ <mass value="2.637"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/wheel.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="1.570795 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1143" radius="0.17775"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="rear_right_wheel_link">
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1.0"/>
+ <fdir1 value="1 0 0"/>
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>false</turnGravityOff>
+ </gazebo>
+ <joint name="rear_right_wheel" type="continuous">
+ <parent link="base_link"/>
+ <child link="rear_right_wheel_link"/>
+ <origin rpy="0 0 0" xyz="-0.256 -0.2854 0.03282"/>
+ <axis rpy="0 0 0" xyz="0 1 0"/>
+ </joint>
+ <transmission name="rear_right_wheel_trans" type="SimpleTransmission">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="rear_right_wheel_motor">
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ <joint name="rear_right_wheel">
+ <hardwareInterface>VelocityJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <link name="top_plate_link">
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/top_plate.stl"/>
+ </geometry>
+ <material name="Yellow"/>
+ </visual>
+ </link>
+ <joint name="top_plate" type="fixed">
+ <parent link="base_link"/>
+ <child link="top_plate_link"/>
+ </joint>
+ <gazebo reference="top_plate_link">
+ <material>Gazebo/Yellow</material>
+ </gazebo>
+ <link name="user_rail_link">
+ <visual>
+ <geometry>
+ <mesh filename="meshes/user_rail.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ </link>
+ <joint name="user_rail" type="fixed">
+ <origin rpy="0 0 0" xyz="0.272 0 0.245"/>
+ <parent link="base_link"/>
+ <child link="user_rail_link"/>
+ </joint>
+ <gazebo reference="user_rail_link">
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <link name="front_bumper_link">
+ <visual>
+ <geometry>
+ <mesh filename="meshes/bumper.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ </link>
+ <joint name="front_bumper" type="fixed">
+ <origin rpy="0 0 0" xyz="0.48 0 0.091"/>
+ <parent link="base_link"/>
+ <child link="front_bumper_link"/>
+ </joint>
+ <gazebo reference="front_bumper_link">
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <link name="rear_bumper_link">
+ <visual>
+ <geometry>
+ <mesh filename="meshes/bumper.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ </link>
+ <joint name="rear_bumper" type="fixed">
+ <origin rpy="0 0 3.14159" xyz="-0.48 0 0.091"/>
+ <parent link="base_link"/>
+ <child link="rear_bumper_link"/>
+ </joint>
+ <gazebo reference="rear_bumper_link">
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+</robot>
+
diff --git a/data/husky/meshes/bumper.stl b/data/husky/meshes/bumper.stl
new file mode 100644
index 0000000..7cccf4b
Binary files /dev/null and b/data/husky/meshes/bumper.stl differ
diff --git a/data/husky/meshes/top_plate.stl b/data/husky/meshes/top_plate.stl
new file mode 100644
index 0000000..ef4d137
Binary files /dev/null and b/data/husky/meshes/top_plate.stl differ
diff --git a/data/husky/meshes/user_rail.stl b/data/husky/meshes/user_rail.stl
new file mode 100644
index 0000000..7542c59
Binary files /dev/null and b/data/husky/meshes/user_rail.stl differ
diff --git a/data/husky/meshes/wheel.stl b/data/husky/meshes/wheel.stl
new file mode 100644
index 0000000..0e76151
Binary files /dev/null and b/data/husky/meshes/wheel.stl differ
diff --git a/data/cube.mtl b/data/jenga/jenga.mtl
similarity index 76%
copy from data/cube.mtl
copy to data/jenga/jenga.mtl
index 935e48a..a89fe6c 100644
--- a/data/cube.mtl
+++ b/data/jenga/jenga.mtl
@@ -1,4 +1,4 @@
-newmtl cube
+newmtl jenga
Ns 10.0000
Ni 1.5000
d 1.0000
@@ -9,6 +9,8 @@ newmtl cube
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
- map_Ka cube.png
- map_Kd cube.png
+ map_Ka jenga.tga
+ map_Kd jenga.png
+
+
diff --git a/data/jenga/jenga.obj b/data/jenga/jenga.obj
new file mode 100644
index 0000000..b90863b
--- /dev/null
+++ b/data/jenga/jenga.obj
@@ -0,0 +1,113 @@
+# jenga.obj
+#
+
+o jenga
+mtllib jenga.mtl
+
+v -0.5 -0.5 0.5
+v 0.5 -0.5 0.5
+v 0.5 0.5 0.5
+v -0.5 0.5 0.5
+
+v -0.5 -0.5 -0.5
+v 0.5 -0.5 -0.5
+v 0.5 0.5 -0.5
+v -0.5 0.5 -0.5
+
+v -0.5 -0.5 -0.5
+v -0.5 0.5 -0.5
+v -0.5 0.5 0.5
+v -0.5 -0.5 0.5
+
+v 0.5 -0.5 -0.5
+v 0.5 0.5 -0.5
+v 0.5 0.5 0.5
+v 0.5 -0.5 0.5
+v -0.5 -0.5 -0.5
+v -0.5 -0.5 0.5
+v 0.5 -0.5 0.5
+v 0.5 -0.5 -0.5
+v -0.5 0.5 -0.5
+v -0.5 0.5 0.5
+v 0.5 0.5 0.5
+v 0.5 0.5 -0.5
+
+vt 0 1
+vt 0 0.75
+vt 0.25 0.75
+vt 0.25 1
+
+vt 0.25 0.5
+vt 0.25 0.75
+vt 0.5 0.75
+vt 0.5 0.5
+
+vt 1 0.75
+vt 0.75 0.75
+vt 0.75 1
+vt 1 1
+
+vt 0.25 0.75
+vt 0.5 0.75
+vt 0.5 1
+vt 0.25 1
+
+vt 0 0.5
+vt 0 0.75
+vt 0.25 0.75
+vt 0.25 0.5
+
+
+vt 0.75 0.75
+vt 0.75 1
+vt 0.5 1
+vt 0.5 0.75
+
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+
+
+g jenga
+usemtl jenga
+s 1
+f 1/1/1 2/2/2 3/3/3
+f 1/1/1 3/3/3 4/4/4
+s 2
+f 7/7/7 6/6/6 5/5/5
+f 8/8/8 7/7/7 5/5/5
+s 3
+f 11/11/11 10/10/10 9/9/9
+f 12/12/12 11/11/11 9/9/9
+s 4
+f 13/13/13 14/14/14 15/15/15
+f 13/13/13 15/15/15 16/16/16
+s 5
+f 19/19/19 18/18/18 17/17/17
+f 20/20/20 19/19/19 17/17/17
+s 6
+f 21/21/21 22/22/22 23/23/23
+f 21/21/21 23/23/23 24/24/24
+
+
\ No newline at end of file
diff --git a/data/jenga/jenga.png b/data/jenga/jenga.png
new file mode 100644
index 0000000..1564309
Binary files /dev/null and b/data/jenga/jenga.png differ
diff --git a/data/jenga/jenga.urdf b/data/jenga/jenga.urdf
new file mode 100644
index 0000000..e78a13d
--- /dev/null
+++ b/data/jenga/jenga.urdf
@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="jenga.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value=".5"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value=".05"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="jenga.obj" scale="0.15 0.05 0.03"/>
+ </geometry>
+ <material name="blue">
+ <color rgba="0.5 0.5 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.15 0.05 0.03"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/kiva_shelf/0_Bullet3Demo.txt b/data/kiva_shelf/0_Bullet3Demo.txt
new file mode 100644
index 0000000..eb396d2
--- /dev/null
+++ b/data/kiva_shelf/0_Bullet3Demo.txt
@@ -0,0 +1,7 @@
+--start_demo_name=R2D2 Grasp
+--mouse_move_multiplier=0.400000
+--mouse_wheel_multiplier=0.010000
+--background_color_red= 0.900000
+--background_color_green= 0.900000
+--background_color_blue= 1.000000
+--fixed_timestep= 0.000000
diff --git a/data/kiva_shelf/meshes/pod_lowres.stl b/data/kiva_shelf/meshes/pod_lowres.stl
new file mode 100644
index 0000000..6e668f1
Binary files /dev/null and b/data/kiva_shelf/meshes/pod_lowres.stl differ
diff --git a/data/kiva_shelf/model.sdf b/data/kiva_shelf/model.sdf
new file mode 100644
index 0000000..501a610
--- /dev/null
+++ b/data/kiva_shelf/model.sdf
@@ -0,0 +1,205 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+ <model name="Amazon Pod">
+ <static>1</static>
+ <pose>0 1 0 0 0 0</pose>
+ <link name="pod_link">
+ <inertial>
+ <pose>0.0 .0 1.2045 0 0 0</pose>
+ <mass>0</mass>
+ <inertia>
+ <ixx>0</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <izz>0.0</izz>
+ <iyz>0.0</iyz>
+ <iyy>0.0</iyy>
+ </inertia>
+ </inertial>
+
+ <visual name="pod_visual">
+ <pose>0 0 0 1.5707 0 0 </pose>
+ <geometry>
+ <mesh>
+ <uri>meshes/pod_lowres.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <diffuse>0.9 0.8 0.5 1</diffuse>
+ </material>
+ </visual>
+
+
+ <collision concave="yes" name="pod_collision">
+ <pose>0 0 1.21515 1.5707 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.875 1.754 0.03</size>
+ </box>
+ </geometry>
+ </collision>
+
+ <collision concave="yes" name="pod_collision">
+ <pose>-0.42 0 1.18 1.5707 0 1.5707 </pose>
+ <geometry>
+ <box>
+ <size>0.875 1.754 0.03</size>
+ </box>
+ </geometry>
+ </collision>
+
+ <collision concave="yes" name="pod_collision">
+ <pose>0.42 0.42 1.17 1.5707 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.03 2.3 0.03</size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>-0.42 0.42 1.17 1.5707 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.03 2.3 0.03</size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>-0.42 -0.42 1.17 1.5707 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.03 2.3 0.03</size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0.42 -0.42 1.17 1.5707 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.03 2.3 0.03</size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0.15 0 1.49 1.5707 0 1.5707 </pose>
+ <geometry>
+ <box>
+ <size>0.875 1.32 0.01</size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>-0.15 0 1.49 1.5707 0 1.5707 </pose>
+ <geometry>
+ <box>
+ <size>0.875 1.32 0.01</size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 0 .57 1.5707 0 1.5707 </pose>
+ <geometry>
+ <box>
+ <size>0.875 0.45 0.01</size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0.42 0 1.18 1.5707 0 1.5707 </pose>
+ <geometry>
+ <box>
+ <size>0.875 1.754 0.03</size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 0 1.06 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.905 0.856 0.028 </size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 0 1.06 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.905 0.856 0.028 </size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 0 0.80 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.905 0.856 0.028 </size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 0.43 0.81 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.856 0.018 0.028 </size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 -0.43 0.81 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.856 0.018 0.028 </size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 0.43 1.08 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.856 0.018 0.028 </size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 -0.43 1.08 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.856 0.018 0.028 </size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 0 0.37 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.905 0.856 0.028 </size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 0 1.29 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.905 0.856 0.028 </size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 0 1.53 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.905 0.856 0.028 </size>
+ </box>
+ </geometry>
+ </collision>
+ <collision concave="yes" name="pod_collision">
+ <pose>0 0 1.78 0 0 0 </pose>
+ <geometry>
+ <box>
+ <size>0.905 0.856 0.028 </size>
+ </box>
+ </geometry>
+ </collision>
+ </link>
+ </model>
+</sdf>
diff --git a/data/kuka_iiwa/kuka_world.sdf b/data/kuka_iiwa/kuka_world.sdf
new file mode 100644
index 0000000..d48d513
--- /dev/null
+++ b/data/kuka_iiwa/kuka_world.sdf
@@ -0,0 +1,414 @@
+<sdf version='1.6'>
+<world name='default'>
+ <model name='lbr_iiwa'>
+ <joint name='fix_to_world' type='fixed'>
+ <parent>world</parent>
+ <child>lbr_iiwa_link_0</child>
+ </joint>
+ <link name='lbr_iiwa_link_0'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>-0.1 0 0.07 0 -0 0</pose>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.05</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.06</iyy>
+ <iyz>0</iyz>
+ <izz>0.03</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_0_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_0.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_0_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_0.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name='lbr_iiwa_link_1'>
+ <pose frame=''>0 0 0.1575 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>0 -0.03 0.12 0 -0 0</pose>
+ <mass>4</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.09</iyy>
+ <iyz>0</iyz>
+ <izz>0.02</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_1_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_1.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_1_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_1.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_1' type='revolute'>
+ <child>lbr_iiwa_link_1</child>
+ <parent>lbr_iiwa_link_0</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_2'>
+ <pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
+ <mass>4</mass>
+ <inertia>
+ <ixx>0.05</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.018</iyy>
+ <iyz>0</iyz>
+ <izz>0.044</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_2_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_2.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_2_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_2.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_2' type='revolute'>
+ <child>lbr_iiwa_link_2</child>
+ <parent>lbr_iiwa_link_1</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_3'>
+ <pose frame=''>0 -0 0.5645 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0.03 0.13 0 -0 0</pose>
+ <mass>3</mass>
+ <inertia>
+ <ixx>0.08</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.075</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_3_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_3.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_3_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_3.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_3' type='revolute'>
+ <child>lbr_iiwa_link_3</child>
+ <parent>lbr_iiwa_link_2</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_4'>
+ <pose frame=''>0 -0 0.78 1.5708 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0.067 0.034 0 -0 0</pose>
+ <mass>2.7</mass>
+ <inertia>
+ <ixx>0.03</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.029</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_4_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_4.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_4_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_4.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_4' type='revolute'>
+ <child>lbr_iiwa_link_4</child>
+ <parent>lbr_iiwa_link_3</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_5'>
+ <pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
+ <mass>1.7</mass>
+ <inertia>
+ <ixx>0.02</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.018</iyy>
+ <iyz>0</iyz>
+ <izz>0.005</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_5_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_5.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_5_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_5.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_5' type='revolute'>
+ <child>lbr_iiwa_link_5</child>
+ <parent>lbr_iiwa_link_4</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_6'>
+ <pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
+ <mass>1.8</mass>
+ <inertia>
+ <ixx>0.005</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.0036</iyy>
+ <iyz>0</iyz>
+ <izz>0.0047</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_6_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_6.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_6_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_6.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_6' type='revolute'>
+ <child>lbr_iiwa_link_6</child>
+ <parent>lbr_iiwa_link_5</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_7'>
+ <pose frame=''>0 0 1.261 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.02 0 -0 0</pose>
+ <mass>0.3</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_7_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_7.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_7_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_7.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_7' type='revolute'>
+ <child>lbr_iiwa_link_7</child>
+ <parent>lbr_iiwa_link_6</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-3.05433</lower>
+ <upper>3.05433</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ </model>
+</world>
+</sdf>
\ No newline at end of file
diff --git a/data/kuka_iiwa/meshes/link_0.stl b/data/kuka_iiwa/meshes/link_0.stl
new file mode 100644
index 0000000..ed14470
Binary files /dev/null and b/data/kuka_iiwa/meshes/link_0.stl differ
diff --git a/data/kuka_iiwa/meshes/link_1.stl b/data/kuka_iiwa/meshes/link_1.stl
new file mode 100644
index 0000000..d9d043e
Binary files /dev/null and b/data/kuka_iiwa/meshes/link_1.stl differ
diff --git a/data/kuka_iiwa/meshes/link_2.stl b/data/kuka_iiwa/meshes/link_2.stl
new file mode 100644
index 0000000..282aa78
Binary files /dev/null and b/data/kuka_iiwa/meshes/link_2.stl differ
diff --git a/data/kuka_iiwa/meshes/link_3.stl b/data/kuka_iiwa/meshes/link_3.stl
new file mode 100644
index 0000000..50d856c
Binary files /dev/null and b/data/kuka_iiwa/meshes/link_3.stl differ
diff --git a/data/kuka_iiwa/meshes/link_4.stl b/data/kuka_iiwa/meshes/link_4.stl
new file mode 100644
index 0000000..ade9b0c
Binary files /dev/null and b/data/kuka_iiwa/meshes/link_4.stl differ
diff --git a/data/kuka_iiwa/meshes/link_5.stl b/data/kuka_iiwa/meshes/link_5.stl
new file mode 100644
index 0000000..663ece5
Binary files /dev/null and b/data/kuka_iiwa/meshes/link_5.stl differ
diff --git a/data/kuka_iiwa/meshes/link_6.stl b/data/kuka_iiwa/meshes/link_6.stl
new file mode 100644
index 0000000..7fb9fca
Binary files /dev/null and b/data/kuka_iiwa/meshes/link_6.stl differ
diff --git a/data/kuka_iiwa/meshes/link_7.stl b/data/kuka_iiwa/meshes/link_7.stl
new file mode 100644
index 0000000..e4e301b
Binary files /dev/null and b/data/kuka_iiwa/meshes/link_7.stl differ
diff --git a/data/kuka_iiwa/model.sdf b/data/kuka_iiwa/model.sdf
new file mode 100644
index 0000000..147da2b
--- /dev/null
+++ b/data/kuka_iiwa/model.sdf
@@ -0,0 +1,459 @@
+<sdf version='1.6'>
+<world name='default'>
+ <model name='lbr_iiwa'>
+ <pose frame=''>0 -2.3 0.7 0 0 0</pose>
+ <link name='lbr_iiwa_link_0'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>-0.1 0 0.07 0 -0 0</pose>
+ <mass>0</mass>
+ <inertia>
+ <ixx>0.05</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.06</iyy>
+ <iyz>0</iyz>
+ <izz>0.03</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_0_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_0.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_0_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_0.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <ambient>1 0 0 1</ambient>
+ <diffuse>0.2 0.2 0.2 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <emissive>0 0 0 0</emissive>
+ </material>
+ </visual>
+ </link>
+ <link name='lbr_iiwa_link_1'>
+ <pose frame=''>0 0 0.1575 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>0 -0.03 0.12 0 -0 0</pose>
+ <mass>4</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.09</iyy>
+ <iyz>0</iyz>
+ <izz>0.02</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_1_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_1.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_1_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_1.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <ambient>1 0 0 1</ambient>
+ <diffuse>0.5 0.7 1.0 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <emissive>0 0 0 0</emissive>
+ </material>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_1' type='revolute'>
+ <child>lbr_iiwa_link_1</child>
+ <parent>lbr_iiwa_link_0</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_2'>
+ <pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
+ <mass>4</mass>
+ <inertia>
+ <ixx>0.05</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.018</iyy>
+ <iyz>0</iyz>
+ <izz>0.044</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_2_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_2.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_2_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_2.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <ambient>1 0 0 1</ambient>
+ <diffuse>0.5 0.7 1.0 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <emissive>0 0 0 0</emissive>
+ </material>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_2' type='revolute'>
+ <child>lbr_iiwa_link_2</child>
+ <parent>lbr_iiwa_link_1</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_3'>
+ <pose frame=''>0 -0 0.5645 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0.03 0.13 0 -0 0</pose>
+ <mass>3</mass>
+ <inertia>
+ <ixx>0.08</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.075</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_3_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_3.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_3_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_3.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <ambient>1 0 0 1</ambient>
+ <diffuse>1.0 0.42 0.04 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <emissive>0 0 0 0</emissive>
+ </material>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_3' type='revolute'>
+ <child>lbr_iiwa_link_3</child>
+ <parent>lbr_iiwa_link_2</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_4'>
+ <pose frame=''>0 -0 0.78 1.5708 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0.067 0.034 0 -0 0</pose>
+ <mass>2.7</mass>
+ <inertia>
+ <ixx>0.03</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.029</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_4_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_4.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_4_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_4.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <ambient>1 0 0 1</ambient>
+ <diffuse>0.5 0.7 1.0 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <emissive>0 0 0 0</emissive>
+ </material>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_4' type='revolute'>
+ <child>lbr_iiwa_link_4</child>
+ <parent>lbr_iiwa_link_3</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_5'>
+ <pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
+ <mass>1.7</mass>
+ <inertia>
+ <ixx>0.02</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.018</iyy>
+ <iyz>0</iyz>
+ <izz>0.005</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_5_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_5.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_5_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_5.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <ambient>1 0 0 1</ambient>
+ <diffuse>0.5 0.7 1.0 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <emissive>0 0 0 0</emissive>
+ </material>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_5' type='revolute'>
+ <child>lbr_iiwa_link_5</child>
+ <parent>lbr_iiwa_link_4</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_6'>
+ <pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
+ <mass>1.8</mass>
+ <inertia>
+ <ixx>0.005</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.0036</iyy>
+ <iyz>0</iyz>
+ <izz>0.0047</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_6_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_6.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_6_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_6.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <ambient>1 0 0 1</ambient>
+ <diffuse>1.0 0.42 0.04 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <emissive>0 0 0 0</emissive>
+ </material>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_6' type='revolute'>
+ <child>lbr_iiwa_link_6</child>
+ <parent>lbr_iiwa_link_5</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_7'>
+ <pose frame=''>0 0 1.261 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.02 0 -0 0</pose>
+ <mass>0.3</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_7_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_7.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_7_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_7.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <ambient>1 0 0 1</ambient>
+ <diffuse>0.2 0.2 0.2 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <emissive>0 0 0 0</emissive>
+ </material>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_7' type='revolute'>
+ <child>lbr_iiwa_link_7</child>
+ <parent>lbr_iiwa_link_6</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-3.05433</lower>
+ <upper>3.05433</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ </model>
+ </world>
+</sdf>
diff --git a/data/kuka_iiwa/model.urdf b/data/kuka_iiwa/model.urdf
new file mode 100644
index 0000000..a6e53d3
--- /dev/null
+++ b/data/kuka_iiwa/model.urdf
@@ -0,0 +1,289 @@
+<?xml version="1.0" ?>
+<!-- ======================================================================= -->
+<!-- | This document was autogenerated by xacro from lbr_iiwa.urdf.xacro | -->
+<!-- | Original xacro: https://github.com/rtkg/lbr_iiwa/archive/master.zip | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- | Changes (kohlhoff): | -->
+<!-- | * Removed gazebo tags. | -->
+<!-- | * Removed unused materials. | -->
+<!-- | * Made mesh paths relative. | -->
+<!-- | * Removed material fields from collision segments. | -->
+<!-- | * Removed the self_collision_checking segment. | -->
+<!-- | * Removed transmission segments, since they didn't match the | -->
+<!-- | convention, will have to added back later. | -->
+<!-- ======================================================================= -->
+<!--LICENSE: -->
+<!--Copyright (c) 2015, Robert Krug & Todor Stoyanov, AASS Research Center, -->
+<!--Orebro University, Sweden -->
+<!--All rights reserved. -->
+<!-- -->
+<!--Redistribution and use in source and binary forms, with or without -->
+<!--modification, are permitted provided that the following conditions are -->
+<!--met: -->
+<!-- -->
+<!--1. Redistributions of source code must retain the above copyright notice,-->
+<!-- this list of conditions and the following disclaimer. -->
+<!-- -->
+<!--2. 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. -->
+<!-- -->
+<!--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 HOLDER 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. -->
+<robot name="lbr_iiwa" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <!-- Import Rviz colors -->
+ <material name="Grey">
+ <color rgba="0.2 0.2 0.2 1.0"/>
+ </material>
+ <material name="Orange">
+ <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+ </material>
+ <material name="Blue">
+ <color rgba="0.5 0.7 1.0 1.0"/>
+</material>
+
+ <!--Import the lbr iiwa macro -->
+ <!--Import Transmissions -->
+ <!--Include Utilities -->
+ <!--The following macros are adapted from the LWR 4 definitions of the RCPRG - https://github.com/RCPRG-ros-pkg/lwr_robot -->
+ <!--Little helper macros to define the inertia matrix needed for links.-->
+ <!--Cuboid-->
+ <!--Cylinder: length is along the y-axis! -->
+ <!--lbr-->
+ <link name="lbr_iiwa_link_0">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
+ <!--Increase mass from 5 Kg original to provide a stable base to carry the
+ arm.-->
+ <mass value="0.0"/>
+ <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_0.stl"/>
+ </geometry>
+ <material name="Grey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_0.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_0 and link_1 -->
+ <joint name="lbr_iiwa_joint_1" type="revolute">
+ <parent link="lbr_iiwa_link_0"/>
+ <child link="lbr_iiwa_link_1"/>
+ <origin rpy="0 0 0" xyz="0 0 0.1575"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_1">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
+ <mass value="4"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_1.stl"/>
+ </geometry>
+ <material name="Blue"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_1.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_1 and link_2 -->
+ <joint name="lbr_iiwa_joint_2" type="revolute">
+ <parent link="lbr_iiwa_link_1"/>
+ <child link="lbr_iiwa_link_2"/>
+ <origin rpy="1.57079632679 0 3.14159265359" xyz="0 0 0.2025"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_2">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
+ <mass value="4"/>
+ <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.044"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_2.stl"/>
+ </geometry>
+ <material name="Blue"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_2.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_2 and link_3 -->
+ <joint name="lbr_iiwa_joint_3" type="revolute">
+ <parent link="lbr_iiwa_link_2"/>
+ <child link="lbr_iiwa_link_3"/>
+ <origin rpy="1.57079632679 0 3.14159265359" xyz="0 0.2045 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_3">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0.03 0.13"/>
+ <mass value="3"/>
+ <inertia ixx="0.08" ixy="0" ixz="0" iyy="0.075" iyz="0" izz="0.01"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_3.stl"/>
+ </geometry>
+ <material name="Orange"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_3.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_3 and link_4 -->
+ <joint name="lbr_iiwa_joint_4" type="revolute">
+ <parent link="lbr_iiwa_link_3"/>
+ <child link="lbr_iiwa_link_4"/>
+ <origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_4">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0.067 0.034"/>
+ <mass value="2.7"/>
+ <inertia ixx="0.03" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.029"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_4.stl"/>
+ </geometry>
+ <material name="Blue"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_4.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_4 and link_5 -->
+ <joint name="lbr_iiwa_joint_5" type="revolute">
+ <parent link="lbr_iiwa_link_4"/>
+ <child link="lbr_iiwa_link_5"/>
+ <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.1845 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_5">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
+ <mass value="1.7"/>
+ <inertia ixx="0.02" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.005"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_5.stl"/>
+ </geometry>
+ <material name="Blue"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_5.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_5 and link_6 -->
+ <joint name="lbr_iiwa_joint_6" type="revolute">
+ <parent link="lbr_iiwa_link_5"/>
+ <child link="lbr_iiwa_link_6"/>
+ <origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_6">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
+ <mass value="1.8"/>
+ <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.0036" iyz="0" izz="0.0047"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_6.stl"/>
+ </geometry>
+ <material name="Orange"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_6.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_6 and link_7 -->
+ <joint name="lbr_iiwa_joint_7" type="revolute">
+ <parent link="lbr_iiwa_link_6"/>
+ <child link="lbr_iiwa_link_7"/>
+ <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.081 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-3.05432619099" upper="3.05432619099" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_7">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0.02"/>
+ <mass value="0.3"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_7.stl"/>
+ </geometry>
+ <material name="Grey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_7.stl"/>
+ </geometry>
+ </collision>
+ </link>
+
+</robot>
+
diff --git a/data/kuka_iiwa/model2.sdf b/data/kuka_iiwa/model2.sdf
new file mode 100644
index 0000000..117357c
--- /dev/null
+++ b/data/kuka_iiwa/model2.sdf
@@ -0,0 +1,818 @@
+<sdf version='1.6'>
+<world name='default'>
+ <model name='lbr_iiwa'>
+ <link name='lbr_iiwa_link_0'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>-0.1 0 0.07 0 -0 0</pose>
+ <mass>0</mass>
+ <inertia>
+ <ixx>0.05</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.06</iyy>
+ <iyz>0</iyz>
+ <izz>0.03</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_0_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_0.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_0_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_0.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name='lbr_iiwa_link_1'>
+ <pose frame=''>0 0 0.1575 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>0 -0.03 0.12 0 -0 0</pose>
+ <mass>4</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.09</iyy>
+ <iyz>0</iyz>
+ <izz>0.02</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_1_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_1.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_1_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_1.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_1' type='revolute'>
+ <child>lbr_iiwa_link_1</child>
+ <parent>lbr_iiwa_link_0</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_2'>
+ <pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
+ <mass>4</mass>
+ <inertia>
+ <ixx>0.05</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.018</iyy>
+ <iyz>0</iyz>
+ <izz>0.044</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_2_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_2.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_2_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_2.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_2' type='revolute'>
+ <child>lbr_iiwa_link_2</child>
+ <parent>lbr_iiwa_link_1</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_3'>
+ <pose frame=''>0 -0 0.5645 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0.03 0.13 0 -0 0</pose>
+ <mass>3</mass>
+ <inertia>
+ <ixx>0.08</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.075</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_3_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_3.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_3_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_3.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_3' type='revolute'>
+ <child>lbr_iiwa_link_3</child>
+ <parent>lbr_iiwa_link_2</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_4'>
+ <pose frame=''>0 -0 0.78 1.5708 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0.067 0.034 0 -0 0</pose>
+ <mass>2.7</mass>
+ <inertia>
+ <ixx>0.03</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.029</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_4_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_4.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_4_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_4.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_4' type='revolute'>
+ <child>lbr_iiwa_link_4</child>
+ <parent>lbr_iiwa_link_3</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_5'>
+ <pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
+ <mass>1.7</mass>
+ <inertia>
+ <ixx>0.02</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.018</iyy>
+ <iyz>0</iyz>
+ <izz>0.005</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_5_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_5.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_5_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_5.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_5' type='revolute'>
+ <child>lbr_iiwa_link_5</child>
+ <parent>lbr_iiwa_link_4</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_6'>
+ <pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
+ <mass>1.8</mass>
+ <inertia>
+ <ixx>0.005</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.0036</iyy>
+ <iyz>0</iyz>
+ <izz>0.0047</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_6_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_6.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_6_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_6.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_6' type='revolute'>
+ <child>lbr_iiwa_link_6</child>
+ <parent>lbr_iiwa_link_5</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_7'>
+ <pose frame=''>0 0 1.261 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.02 0 -0 0</pose>
+ <mass>0.3</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_7_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_7.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_7_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_7.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_7' type='revolute'>
+ <child>lbr_iiwa_link_7</child>
+ <parent>lbr_iiwa_link_6</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-3.05433</lower>
+ <upper>3.05433</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ </model>
+
+ <model name='lbr_iiwa'>
+ <pose frame=''>2 2 0 0 -0 0</pose>
+ <link name='lbr_iiwa_link_0'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>-0.1 0 0.07 0 -0 0</pose>
+ <mass>0</mass>
+ <inertia>
+ <ixx>0.05</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.06</iyy>
+ <iyz>0</iyz>
+ <izz>0.03</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_0_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_0.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_0_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_0.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name='lbr_iiwa_link_1'>
+ <pose frame=''>0 0 0.1575 0 -0 0</pose>
+ <inertial>
+ <pose frame=''>0 -0.03 0.12 0 -0 0</pose>
+ <mass>4</mass>
+ <inertia>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.09</iyy>
+ <iyz>0</iyz>
+ <izz>0.02</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_1_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_1.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_1_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_1.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_1' type='revolute'>
+ <child>lbr_iiwa_link_1</child>
+ <parent>lbr_iiwa_link_0</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_2'>
+ <pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
+ <mass>4</mass>
+ <inertia>
+ <ixx>0.05</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.018</iyy>
+ <iyz>0</iyz>
+ <izz>0.044</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_2_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_2.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_2_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_2.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_2' type='revolute'>
+ <child>lbr_iiwa_link_2</child>
+ <parent>lbr_iiwa_link_1</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_3'>
+ <pose frame=''>0 -0 0.5645 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0.03 0.13 0 -0 0</pose>
+ <mass>3</mass>
+ <inertia>
+ <ixx>0.08</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.075</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_3_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_3.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_3_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_3.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_3' type='revolute'>
+ <child>lbr_iiwa_link_3</child>
+ <parent>lbr_iiwa_link_2</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_4'>
+ <pose frame=''>0 -0 0.78 1.5708 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0.067 0.034 0 -0 0</pose>
+ <mass>2.7</mass>
+ <inertia>
+ <ixx>0.03</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.029</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_4_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_4.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_4_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_4.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_4' type='revolute'>
+ <child>lbr_iiwa_link_4</child>
+ <parent>lbr_iiwa_link_3</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_5'>
+ <pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
+ <mass>1.7</mass>
+ <inertia>
+ <ixx>0.02</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.018</iyy>
+ <iyz>0</iyz>
+ <izz>0.005</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_5_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_5.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_5_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_5.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_5' type='revolute'>
+ <child>lbr_iiwa_link_5</child>
+ <parent>lbr_iiwa_link_4</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.96706</lower>
+ <upper>2.96706</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_6'>
+ <pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
+ <inertial>
+ <pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
+ <mass>1.8</mass>
+ <inertia>
+ <ixx>0.005</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.0036</iyy>
+ <iyz>0</iyz>
+ <izz>0.0047</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_6_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_6.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_6_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_6.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_6' type='revolute'>
+ <child>lbr_iiwa_link_6</child>
+ <parent>lbr_iiwa_link_5</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-2.0944</lower>
+ <upper>2.0944</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ <link name='lbr_iiwa_link_7'>
+ <pose frame=''>0 0 1.261 0 0 0</pose>
+ <inertial>
+ <pose frame=''>0 0 0.02 0 -0 0</pose>
+ <mass>0.3</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+ <collision name='lbr_iiwa_link_7_collision'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/coarse/link_7.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name='lbr_iiwa_link_7_visual'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>meshes/link_7.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <joint name='lbr_iiwa_joint_7' type='revolute'>
+ <child>lbr_iiwa_link_7</child>
+ <parent>lbr_iiwa_link_6</parent>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>-3.05433</lower>
+ <upper>3.05433</upper>
+ <effort>300</effort>
+ <velocity>10</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.5</damping>
+ <friction>0</friction>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ </axis>
+ </joint>
+ </model>
+ </world>
+</sdf>
\ No newline at end of file
diff --git a/data/kuka_iiwa/model_for_sdf.urdf b/data/kuka_iiwa/model_for_sdf.urdf
new file mode 100644
index 0000000..12a1654
--- /dev/null
+++ b/data/kuka_iiwa/model_for_sdf.urdf
@@ -0,0 +1,285 @@
+<?xml version="1.0" ?>
+<!-- ======================================================================= -->
+<!-- | This document was autogenerated by xacro from lbr_iiwa.urdf.xacro | -->
+<!-- | Original xacro: https://github.com/rtkg/lbr_iiwa/archive/master.zip | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- | Changes (kohlhoff): | -->
+<!-- | * Removed gazebo tags. | -->
+<!-- | * Removed unused materials. | -->
+<!-- | * Made mesh paths relative. | -->
+<!-- | * Removed material fields from collision segments. | -->
+<!-- | * Removed the self_collision_checking segment. | -->
+<!-- | * Removed transmission segments, since they didn't match the | -->
+<!-- | convention, will have to added back later. | -->
+<!-- ======================================================================= -->
+<!--LICENSE: -->
+<!--Copyright (c) 2015, Robert Krug & Todor Stoyanov, AASS Research Center, -->
+<!--Orebro University, Sweden -->
+<!--All rights reserved. -->
+<!-- -->
+<!--Redistribution and use in source and binary forms, with or without -->
+<!--modification, are permitted provided that the following conditions are -->
+<!--met: -->
+<!-- -->
+<!--1. Redistributions of source code must retain the above copyright notice,-->
+<!-- this list of conditions and the following disclaimer. -->
+<!-- -->
+<!--2. 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. -->
+<!-- -->
+<!--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 HOLDER 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. -->
+<robot name="lbr_iiwa" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <!-- Import Rviz colors -->
+ <material name="Grey">
+ <color rgba="0.2 0.2 0.2 1.0"/>
+ </material>
+ <material name="Orange">
+ <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+ </material>
+ <!--Import the lbr iiwa macro -->
+ <!--Import Transmissions -->
+ <!--Include Utilities -->
+ <!--The following macros are adapted from the LWR 4 definitions of the RCPRG - https://github.com/RCPRG-ros-pkg/lwr_robot -->
+ <!--Little helper macros to define the inertia matrix needed for links.-->
+ <!--Cuboid-->
+ <!--Cylinder: length is along the y-axis! -->
+ <!--lbr-->
+ <link name="lbr_iiwa_link_0">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
+ <!--Increase mass from 5 Kg original to provide a stable base to carry the
+ arm.-->
+ <mass value="0.01"/>
+ <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_0.stl"/>
+ </geometry>
+ <material name="Grey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/coarse/link_0.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_0 and link_1 -->
+ <joint name="lbr_iiwa_joint_1" type="revolute">
+ <parent link="lbr_iiwa_link_0"/>
+ <child link="lbr_iiwa_link_1"/>
+ <origin rpy="0 0 0" xyz="0 0 0.1575"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_1">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
+ <mass value="4"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_1.stl"/>
+ </geometry>
+ <material name="Orange"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/coarse/link_1.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_1 and link_2 -->
+ <joint name="lbr_iiwa_joint_2" type="revolute">
+ <parent link="lbr_iiwa_link_1"/>
+ <child link="lbr_iiwa_link_2"/>
+ <origin rpy="1.57079632679 0 3.14159265359" xyz="0 0 0.2025"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_2">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
+ <mass value="4"/>
+ <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.044"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_2.stl"/>
+ </geometry>
+ <material name="Orange"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/coarse/link_2.stl"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <!-- joint between link_2 and link_3 -->
+ <joint name="lbr_iiwa_joint_3" type="revolute">
+ <parent link="lbr_iiwa_link_2"/>
+ <child link="lbr_iiwa_link_3"/>
+ <origin rpy="1.57079632679 0 3.14159265359" xyz="0 0.2045 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_3">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0.03 0.13"/>
+ <mass value="3"/>
+ <inertia ixx="0.08" ixy="0" ixz="0" iyy="0.075" iyz="0" izz="0.01"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_3.stl"/>
+ </geometry>
+ <material name="Orange"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/coarse/link_3.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_3 and link_4 -->
+ <joint name="lbr_iiwa_joint_4" type="revolute">
+ <parent link="lbr_iiwa_link_3"/>
+ <child link="lbr_iiwa_link_4"/>
+ <origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_4">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0.067 0.034"/>
+ <mass value="2.7"/>
+ <inertia ixx="0.03" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.029"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_4.stl"/>
+ </geometry>
+ <material name="Orange"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/coarse/link_4.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_4 and link_5 -->
+ <joint name="lbr_iiwa_joint_5" type="revolute">
+ <parent link="lbr_iiwa_link_4"/>
+ <child link="lbr_iiwa_link_5"/>
+ <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.1845 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_5">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
+ <mass value="1.7"/>
+ <inertia ixx="0.02" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.005"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_5.stl"/>
+ </geometry>
+ <material name="Orange"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/coarse/link_5.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_5 and link_6 -->
+ <joint name="lbr_iiwa_joint_6" type="revolute">
+ <parent link="lbr_iiwa_link_5"/>
+ <child link="lbr_iiwa_link_6"/>
+ <origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_6">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
+ <mass value="1.8"/>
+ <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.0036" iyz="0" izz="0.0047"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_6.stl"/>
+ </geometry>
+ <material name="Orange"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/coarse/link_6.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_6 and link_7 -->
+ <joint name="lbr_iiwa_joint_7" type="revolute">
+ <parent link="lbr_iiwa_link_6"/>
+ <child link="lbr_iiwa_link_7"/>
+ <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.081 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-3.05432619099" upper="3.05432619099" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_7">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0.02"/>
+ <mass value="0.3"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_7.stl"/>
+ </geometry>
+ <material name="Grey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/coarse/link_7.stl"/>
+ </geometry>
+ </collision>
+ </link>
+
+</robot>
\ No newline at end of file
diff --git a/data/kuka_iiwa/model_vr_limits.urdf b/data/kuka_iiwa/model_vr_limits.urdf
new file mode 100644
index 0000000..a5bff40
--- /dev/null
+++ b/data/kuka_iiwa/model_vr_limits.urdf
@@ -0,0 +1,289 @@
+<?xml version="1.0" ?>
+<!-- ======================================================================= -->
+<!-- | This document was autogenerated by xacro from lbr_iiwa.urdf.xacro | -->
+<!-- | Original xacro: https://github.com/rtkg/lbr_iiwa/archive/master.zip | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- | Changes (kohlhoff): | -->
+<!-- | * Removed gazebo tags. | -->
+<!-- | * Removed unused materials. | -->
+<!-- | * Made mesh paths relative. | -->
+<!-- | * Removed material fields from collision segments. | -->
+<!-- | * Removed the self_collision_checking segment. | -->
+<!-- | * Removed transmission segments, since they didn't match the | -->
+<!-- | convention, will have to added back later. | -->
+<!-- ======================================================================= -->
+<!--LICENSE: -->
+<!--Copyright (c) 2015, Robert Krug & Todor Stoyanov, AASS Research Center, -->
+<!--Orebro University, Sweden -->
+<!--All rights reserved. -->
+<!-- -->
+<!--Redistribution and use in source and binary forms, with or without -->
+<!--modification, are permitted provided that the following conditions are -->
+<!--met: -->
+<!-- -->
+<!--1. Redistributions of source code must retain the above copyright notice,-->
+<!-- this list of conditions and the following disclaimer. -->
+<!-- -->
+<!--2. 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. -->
+<!-- -->
+<!--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 HOLDER 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. -->
+<robot name="lbr_iiwa" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <!-- Import Rviz colors -->
+ <material name="Grey">
+ <color rgba="0.2 0.2 0.2 1.0"/>
+ </material>
+ <material name="Orange">
+ <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+ </material>
+ <material name="Blue">
+ <color rgba="0.5 0.7 1.0 1.0"/>
+</material>
+
+ <!--Import the lbr iiwa macro -->
+ <!--Import Transmissions -->
+ <!--Include Utilities -->
+ <!--The following macros are adapted from the LWR 4 definitions of the RCPRG - https://github.com/RCPRG-ros-pkg/lwr_robot -->
+ <!--Little helper macros to define the inertia matrix needed for links.-->
+ <!--Cuboid-->
+ <!--Cylinder: length is along the y-axis! -->
+ <!--lbr-->
+ <link name="lbr_iiwa_link_0">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.1 0 0.07"/>
+ <!--Increase mass from 5 Kg original to provide a stable base to carry the
+ arm.-->
+ <mass value="0.0"/>
+ <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.06" iyz="0" izz="0.03"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_0.stl"/>
+ </geometry>
+ <material name="Grey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_0.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_0 and link_1 -->
+ <joint name="lbr_iiwa_joint_1" type="revolute">
+ <parent link="lbr_iiwa_link_0"/>
+ <child link="lbr_iiwa_link_1"/>
+ <origin rpy="0 0 0" xyz="0 0 0.1575"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-.96705972839" upper="0.96705972839" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_1">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
+ <mass value="4"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_1.stl"/>
+ </geometry>
+ <material name="Blue"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_1.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_1 and link_2 -->
+ <joint name="lbr_iiwa_joint_2" type="revolute">
+ <parent link="lbr_iiwa_link_1"/>
+ <child link="lbr_iiwa_link_2"/>
+ <origin rpy="1.57079632679 0 3.14159265359" xyz="0 0 0.2025"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_2">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0003 0.059 0.042"/>
+ <mass value="4"/>
+ <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.044"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_2.stl"/>
+ </geometry>
+ <material name="Blue"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_2.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_2 and link_3 -->
+ <joint name="lbr_iiwa_joint_3" type="revolute">
+ <parent link="lbr_iiwa_link_2"/>
+ <child link="lbr_iiwa_link_3"/>
+ <origin rpy="1.57079632679 0 3.14159265359" xyz="0 0.2045 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_3">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0.03 0.13"/>
+ <mass value="3"/>
+ <inertia ixx="0.08" ixy="0" ixz="0" iyy="0.075" iyz="0" izz="0.01"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_3.stl"/>
+ </geometry>
+ <material name="Orange"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_3.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_3 and link_4 -->
+ <joint name="lbr_iiwa_joint_4" type="revolute">
+ <parent link="lbr_iiwa_link_3"/>
+ <child link="lbr_iiwa_link_4"/>
+ <origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="0.19439510239" upper="2.29439510239" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_4">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0.067 0.034"/>
+ <mass value="2.7"/>
+ <inertia ixx="0.03" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.029"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_4.stl"/>
+ </geometry>
+ <material name="Blue"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_4.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_4 and link_5 -->
+ <joint name="lbr_iiwa_joint_5" type="revolute">
+ <parent link="lbr_iiwa_link_4"/>
+ <child link="lbr_iiwa_link_5"/>
+ <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.1845 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.96705972839" upper="2.96705972839" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_5">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0001 0.021 0.076"/>
+ <mass value="1.7"/>
+ <inertia ixx="0.02" ixy="0" ixz="0" iyy="0.018" iyz="0" izz="0.005"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_5.stl"/>
+ </geometry>
+ <material name="Blue"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_5.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_5 and link_6 -->
+ <joint name="lbr_iiwa_joint_6" type="revolute">
+ <parent link="lbr_iiwa_link_5"/>
+ <child link="lbr_iiwa_link_6"/>
+ <origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_6">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0.0006 0.0004"/>
+ <mass value="1.8"/>
+ <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.0036" iyz="0" izz="0.0047"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_6.stl"/>
+ </geometry>
+ <material name="Orange"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_6.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- joint between link_6 and link_7 -->
+ <joint name="lbr_iiwa_joint_7" type="revolute">
+ <parent link="lbr_iiwa_link_6"/>
+ <child link="lbr_iiwa_link_7"/>
+ <origin rpy="-1.57079632679 3.14159265359 0" xyz="0 0.081 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-3.05432619099" upper="3.05432619099" velocity="10"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="lbr_iiwa_link_7">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0.02"/>
+ <mass value="0.3"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_7.stl"/>
+ </geometry>
+ <material name="Grey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="meshes/link_7.stl"/>
+ </geometry>
+ </collision>
+ </link>
+
+</robot>
+
diff --git a/data/l_finger_collision.stl b/data/l_finger_collision.stl
new file mode 100644
index 0000000..b4e4565
Binary files /dev/null and b/data/l_finger_collision.stl differ
diff --git a/data/lego/lego.obj b/data/lego/lego.obj
new file mode 100644
index 0000000..7dd1a2c
--- /dev/null
+++ b/data/lego/lego.obj
@@ -0,0 +1,3751 @@
+# Blender v2.68 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib lego.mtl
+o Duplo_Stein_2x2_V3
+v 0.000000 0.000000 -0.317500
+v 0.014976 0.000000 -0.302500
+v 0.000000 0.000000 0.000000
+v 0.317000 0.000000 -0.317500
+v 0.302024 0.000000 -0.302500
+v 0.302024 0.000000 -0.015000
+v 0.014976 0.000000 -0.015000
+v 0.317000 0.000000 0.000000
+v 0.317000 0.192500 0.000000
+v 0.000000 0.192500 0.000000
+v 0.000000 0.192500 -0.317500
+v 0.317000 0.192500 -0.317500
+v 0.023969 0.054000 -0.061626
+v 0.019677 -0.000000 -0.059351
+v 0.023969 -0.000000 -0.061626
+v 0.019677 0.054000 -0.059351
+v 0.032082 -0.000000 -0.083415
+v 0.032667 0.054000 -0.078500
+v 0.032667 -0.000000 -0.078500
+v 0.032082 0.054000 -0.083415
+v 0.030359 -0.000000 -0.088041
+v 0.030359 0.054000 -0.088041
+v 0.027601 -0.000000 -0.092107
+v 0.027601 0.054000 -0.092107
+v 0.023969 0.054000 -0.095374
+v 0.023969 -0.000000 -0.095374
+v 0.019677 0.054000 -0.097649
+v 0.019677 -0.000000 -0.097649
+v 0.030359 -0.000000 -0.068959
+v 0.027601 0.054000 -0.064893
+v 0.027601 -0.000000 -0.064893
+v 0.030359 0.054000 -0.068959
+v 0.032082 -0.000000 -0.073585
+v 0.032082 0.054000 -0.073585
+v 0.014976 -0.000000 -0.058200
+v 0.014976 0.054000 -0.058200
+v 0.014976 0.054000 -0.098800
+v 0.014976 -0.000000 -0.098800
+v 0.023969 0.054000 -0.221626
+v 0.019677 -0.000000 -0.219351
+v 0.023969 -0.000000 -0.221626
+v 0.019677 0.054000 -0.219351
+v 0.032082 -0.000000 -0.243415
+v 0.032667 0.054000 -0.238500
+v 0.032667 -0.000000 -0.238500
+v 0.032082 0.054000 -0.243415
+v 0.030359 -0.000000 -0.248041
+v 0.030359 0.054000 -0.248041
+v 0.027601 -0.000000 -0.252107
+v 0.027601 0.054000 -0.252107
+v 0.023969 0.054000 -0.255374
+v 0.023969 -0.000000 -0.255374
+v 0.019677 0.054000 -0.257649
+v 0.019677 -0.000000 -0.257649
+v 0.030359 -0.000000 -0.228959
+v 0.027601 0.054000 -0.224893
+v 0.027601 -0.000000 -0.224893
+v 0.030359 0.054000 -0.228959
+v 0.032082 -0.000000 -0.233585
+v 0.032082 0.054000 -0.233585
+v 0.014976 -0.000000 -0.218200
+v 0.014976 0.054000 -0.218200
+v 0.014976 0.054000 -0.258800
+v 0.014976 -0.000000 -0.258800
+v 0.204383 0.230000 -0.204205
+v 0.207773 0.230000 -0.230221
+v 0.206685 0.230000 -0.238500
+v 0.214412 0.230000 -0.196498
+v 0.210964 0.230000 -0.222506
+v 0.216040 0.230000 -0.215880
+v 0.226091 0.230000 -0.191653
+v 0.222654 0.230000 -0.210797
+v 0.230357 0.230000 -0.207601
+v 0.238624 0.230000 -0.190000
+v 0.238624 0.230000 -0.206511
+v 0.246890 0.230000 -0.207601
+v 0.251157 0.230000 -0.191653
+v 0.254593 0.230000 -0.210797
+v 0.262835 0.230000 -0.196498
+v 0.261208 0.230000 -0.215880
+v 0.266283 0.230000 -0.222506
+v 0.272864 0.230000 -0.204205
+v 0.269474 0.230000 -0.230221
+v 0.270562 0.230000 -0.238500
+v 0.191850 0.230000 -0.225947
+v 0.191850 0.230000 -0.251053
+v 0.190200 0.230000 -0.238500
+v 0.196688 0.230000 -0.214250
+v 0.196688 0.230000 -0.262750
+v 0.204383 0.230000 -0.272795
+v 0.207773 0.230000 -0.246779
+v 0.214412 0.230000 -0.280502
+v 0.210964 0.230000 -0.254494
+v 0.216040 0.230000 -0.261120
+v 0.226091 0.230000 -0.285347
+v 0.222654 0.230000 -0.266203
+v 0.230357 0.230000 -0.269399
+v 0.238624 0.230000 -0.287000
+v 0.238624 0.230000 -0.270489
+v 0.246890 0.230000 -0.269399
+v 0.251157 0.230000 -0.285347
+v 0.254593 0.230000 -0.266203
+v 0.262835 0.230000 -0.280502
+v 0.261208 0.230000 -0.261120
+v 0.266283 0.230000 -0.254494
+v 0.272864 0.230000 -0.272795
+v 0.269474 0.230000 -0.246779
+v 0.280560 0.230000 -0.214250
+v 0.280560 0.230000 -0.262750
+v 0.285397 0.230000 -0.225947
+v 0.285397 0.230000 -0.251053
+v 0.287047 0.230000 -0.238500
+v 0.287047 0.192500 -0.238500
+v 0.285397 0.192500 -0.225947
+v 0.238624 0.192500 -0.287000
+v 0.226091 0.192500 -0.285347
+v 0.251157 0.192500 -0.285347
+v 0.230357 0.192500 -0.269399
+v 0.238624 0.192500 -0.270489
+v 0.222654 0.192500 -0.266203
+v 0.216040 0.192500 -0.261120
+v 0.210964 0.192500 -0.254494
+v 0.207773 0.192500 -0.246779
+v 0.206685 0.192500 -0.238500
+v 0.238624 0.192500 -0.190000
+v 0.251157 0.192500 -0.191653
+v 0.207773 0.192500 -0.230221
+v 0.210964 0.192500 -0.222506
+v 0.216040 0.192500 -0.215880
+v 0.222654 0.192500 -0.210797
+v 0.230357 0.192500 -0.207601
+v 0.272864 0.192500 -0.272795
+v 0.280560 0.192500 -0.262750
+v 0.214412 0.192500 -0.196498
+v 0.226091 0.192500 -0.191653
+v 0.238624 0.192500 -0.206511
+v 0.262835 0.192500 -0.280502
+v 0.204383 0.192500 -0.204205
+v 0.191850 0.192500 -0.251053
+v 0.191850 0.192500 -0.225947
+v 0.190200 0.192500 -0.238500
+v 0.196688 0.192500 -0.262750
+v 0.196688 0.192500 -0.214250
+v 0.204383 0.192500 -0.272795
+v 0.214412 0.192500 -0.280502
+v 0.246890 0.192500 -0.269399
+v 0.254593 0.192500 -0.266203
+v 0.261208 0.192500 -0.261120
+v 0.266283 0.192500 -0.254494
+v 0.269474 0.192500 -0.246779
+v 0.270562 0.192500 -0.238500
+v 0.246890 0.192500 -0.207601
+v 0.254593 0.192500 -0.210797
+v 0.262835 0.192500 -0.196498
+v 0.261208 0.192500 -0.215880
+v 0.266283 0.192500 -0.222506
+v 0.272864 0.192500 -0.204205
+v 0.269474 0.192500 -0.230221
+v 0.280560 0.192500 -0.214250
+v 0.285397 0.192500 -0.251053
+v 0.044635 0.230000 -0.204205
+v 0.048025 0.230000 -0.230221
+v 0.046937 0.230000 -0.238500
+v 0.054664 0.230000 -0.196498
+v 0.051216 0.230000 -0.222506
+v 0.056292 0.230000 -0.215880
+v 0.066343 0.230000 -0.191653
+v 0.062906 0.230000 -0.210797
+v 0.070609 0.230000 -0.207601
+v 0.078876 0.230000 -0.190000
+v 0.078876 0.230000 -0.206511
+v 0.087142 0.230000 -0.207601
+v 0.091409 0.230000 -0.191653
+v 0.094845 0.230000 -0.210797
+v 0.103087 0.230000 -0.196498
+v 0.101459 0.230000 -0.215880
+v 0.106535 0.230000 -0.222506
+v 0.113116 0.230000 -0.204205
+v 0.109726 0.230000 -0.230221
+v 0.110814 0.230000 -0.238500
+v 0.032102 0.230000 -0.225947
+v 0.032102 0.230000 -0.251053
+v 0.030452 0.230000 -0.238500
+v 0.036940 0.230000 -0.214250
+v 0.036940 0.230000 -0.262750
+v 0.044635 0.230000 -0.272795
+v 0.048025 0.230000 -0.246779
+v 0.054664 0.230000 -0.280502
+v 0.051216 0.230000 -0.254494
+v 0.056292 0.230000 -0.261120
+v 0.066343 0.230000 -0.285347
+v 0.062906 0.230000 -0.266203
+v 0.070609 0.230000 -0.269399
+v 0.078876 0.230000 -0.287000
+v 0.078876 0.230000 -0.270489
+v 0.087142 0.230000 -0.269399
+v 0.091409 0.230000 -0.285347
+v 0.094845 0.230000 -0.266203
+v 0.103087 0.230000 -0.280502
+v 0.101459 0.230000 -0.261120
+v 0.106535 0.230000 -0.254494
+v 0.113116 0.230000 -0.272795
+v 0.109726 0.230000 -0.246779
+v 0.120812 0.230000 -0.214250
+v 0.120812 0.230000 -0.262750
+v 0.125649 0.230000 -0.225947
+v 0.125649 0.230000 -0.251053
+v 0.127299 0.230000 -0.238500
+v 0.127299 0.192500 -0.238500
+v 0.125649 0.192500 -0.225947
+v 0.078876 0.192500 -0.287000
+v 0.066343 0.192500 -0.285347
+v 0.091409 0.192500 -0.285347
+v 0.070609 0.192500 -0.269399
+v 0.078876 0.192500 -0.270489
+v 0.062906 0.192500 -0.266203
+v 0.056292 0.192500 -0.261120
+v 0.051216 0.192500 -0.254494
+v 0.048025 0.192500 -0.246779
+v 0.046937 0.192500 -0.238500
+v 0.078876 0.192500 -0.190000
+v 0.091409 0.192500 -0.191653
+v 0.048025 0.192500 -0.230221
+v 0.051216 0.192500 -0.222506
+v 0.056292 0.192500 -0.215880
+v 0.062906 0.192500 -0.210797
+v 0.070609 0.192500 -0.207601
+v 0.113116 0.192500 -0.272795
+v 0.120812 0.192500 -0.262750
+v 0.054664 0.192500 -0.196498
+v 0.066343 0.192500 -0.191653
+v 0.078876 0.192500 -0.206511
+v 0.103087 0.192500 -0.280502
+v 0.044635 0.192500 -0.204205
+v 0.032102 0.192500 -0.251053
+v 0.032102 0.192500 -0.225947
+v 0.030452 0.192500 -0.238500
+v 0.036940 0.192500 -0.262750
+v 0.036940 0.192500 -0.214250
+v 0.044635 0.192500 -0.272795
+v 0.054664 0.192500 -0.280502
+v 0.087142 0.192500 -0.269399
+v 0.094845 0.192500 -0.266203
+v 0.101459 0.192500 -0.261120
+v 0.106535 0.192500 -0.254494
+v 0.109726 0.192500 -0.246779
+v 0.110814 0.192500 -0.238500
+v 0.087142 0.192500 -0.207601
+v 0.094845 0.192500 -0.210797
+v 0.103087 0.192500 -0.196498
+v 0.101459 0.192500 -0.215880
+v 0.106535 0.192500 -0.222506
+v 0.113116 0.192500 -0.204205
+v 0.109726 0.192500 -0.230221
+v 0.120812 0.192500 -0.214250
+v 0.125649 0.192500 -0.251053
+v 0.204383 0.230000 -0.045205
+v 0.207773 0.230000 -0.071221
+v 0.206685 0.230000 -0.079500
+v 0.214412 0.230000 -0.037498
+v 0.210964 0.230000 -0.063506
+v 0.216040 0.230000 -0.056880
+v 0.226091 0.230000 -0.032653
+v 0.222654 0.230000 -0.051797
+v 0.230357 0.230000 -0.048601
+v 0.238624 0.230000 -0.031000
+v 0.238624 0.230000 -0.047511
+v 0.246890 0.230000 -0.048601
+v 0.251157 0.230000 -0.032653
+v 0.254593 0.230000 -0.051797
+v 0.262835 0.230000 -0.037498
+v 0.261208 0.230000 -0.056880
+v 0.266283 0.230000 -0.063506
+v 0.272864 0.230000 -0.045205
+v 0.269474 0.230000 -0.071221
+v 0.270562 0.230000 -0.079500
+v 0.191850 0.230000 -0.066947
+v 0.191850 0.230000 -0.092053
+v 0.190200 0.230000 -0.079500
+v 0.196688 0.230000 -0.055250
+v 0.196688 0.230000 -0.103750
+v 0.204383 0.230000 -0.113795
+v 0.207773 0.230000 -0.087779
+v 0.214412 0.230000 -0.121502
+v 0.210964 0.230000 -0.095494
+v 0.216040 0.230000 -0.102120
+v 0.226091 0.230000 -0.126347
+v 0.222654 0.230000 -0.107203
+v 0.230357 0.230000 -0.110399
+v 0.238624 0.230000 -0.128000
+v 0.238624 0.230000 -0.111489
+v 0.246890 0.230000 -0.110399
+v 0.251157 0.230000 -0.126347
+v 0.254593 0.230000 -0.107203
+v 0.262835 0.230000 -0.121502
+v 0.261208 0.230000 -0.102120
+v 0.266283 0.230000 -0.095494
+v 0.272864 0.230000 -0.113795
+v 0.269474 0.230000 -0.087779
+v 0.280560 0.230000 -0.055250
+v 0.280560 0.230000 -0.103750
+v 0.285397 0.230000 -0.066947
+v 0.285397 0.230000 -0.092053
+v 0.287047 0.230000 -0.079500
+v 0.287047 0.192500 -0.079500
+v 0.285397 0.192500 -0.066947
+v 0.238624 0.192500 -0.128000
+v 0.226091 0.192500 -0.126347
+v 0.251157 0.192500 -0.126347
+v 0.230357 0.192500 -0.110399
+v 0.238624 0.192500 -0.111489
+v 0.222654 0.192500 -0.107203
+v 0.216040 0.192500 -0.102120
+v 0.210964 0.192500 -0.095494
+v 0.207773 0.192500 -0.087779
+v 0.206685 0.192500 -0.079500
+v 0.238624 0.192500 -0.031000
+v 0.251157 0.192500 -0.032653
+v 0.207773 0.192500 -0.071221
+v 0.210964 0.192500 -0.063506
+v 0.216040 0.192500 -0.056880
+v 0.222654 0.192500 -0.051797
+v 0.230357 0.192500 -0.048601
+v 0.272864 0.192500 -0.113795
+v 0.280560 0.192500 -0.103750
+v 0.214412 0.192500 -0.037498
+v 0.226091 0.192500 -0.032653
+v 0.238624 0.192500 -0.047511
+v 0.262835 0.192500 -0.121502
+v 0.204383 0.192500 -0.045205
+v 0.191850 0.192500 -0.092053
+v 0.191850 0.192500 -0.066947
+v 0.190200 0.192500 -0.079500
+v 0.196688 0.192500 -0.103750
+v 0.196688 0.192500 -0.055250
+v 0.204383 0.192500 -0.113795
+v 0.214412 0.192500 -0.121502
+v 0.246890 0.192500 -0.110399
+v 0.254593 0.192500 -0.107203
+v 0.261208 0.192500 -0.102120
+v 0.266283 0.192500 -0.095494
+v 0.269474 0.192500 -0.087779
+v 0.270562 0.192500 -0.079500
+v 0.246890 0.192500 -0.048601
+v 0.254593 0.192500 -0.051797
+v 0.262835 0.192500 -0.037498
+v 0.261208 0.192500 -0.056880
+v 0.266283 0.192500 -0.063506
+v 0.272864 0.192500 -0.045205
+v 0.269474 0.192500 -0.071221
+v 0.280560 0.192500 -0.055250
+v 0.285397 0.192500 -0.092053
+v 0.044635 0.230000 -0.045205
+v 0.048025 0.230000 -0.071221
+v 0.046937 0.230000 -0.079500
+v 0.054664 0.230000 -0.037498
+v 0.051216 0.230000 -0.063506
+v 0.056292 0.230000 -0.056880
+v 0.066343 0.230000 -0.032653
+v 0.062906 0.230000 -0.051797
+v 0.070609 0.230000 -0.048601
+v 0.078876 0.230000 -0.031000
+v 0.078876 0.230000 -0.047511
+v 0.087142 0.230000 -0.048601
+v 0.091409 0.230000 -0.032653
+v 0.094845 0.230000 -0.051797
+v 0.103087 0.230000 -0.037498
+v 0.101459 0.230000 -0.056880
+v 0.106535 0.230000 -0.063506
+v 0.113116 0.230000 -0.045205
+v 0.109726 0.230000 -0.071221
+v 0.110814 0.230000 -0.079500
+v 0.032102 0.230000 -0.066947
+v 0.032102 0.230000 -0.092053
+v 0.030452 0.230000 -0.079500
+v 0.036940 0.230000 -0.055250
+v 0.036940 0.230000 -0.103750
+v 0.044635 0.230000 -0.113795
+v 0.048025 0.230000 -0.087779
+v 0.054664 0.230000 -0.121502
+v 0.051216 0.230000 -0.095494
+v 0.056292 0.230000 -0.102120
+v 0.066343 0.230000 -0.126347
+v 0.062906 0.230000 -0.107203
+v 0.070609 0.230000 -0.110399
+v 0.078876 0.230000 -0.128000
+v 0.078876 0.230000 -0.111489
+v 0.087142 0.230000 -0.110399
+v 0.091409 0.230000 -0.126347
+v 0.094845 0.230000 -0.107203
+v 0.103087 0.230000 -0.121502
+v 0.101459 0.230000 -0.102120
+v 0.106535 0.230000 -0.095494
+v 0.113116 0.230000 -0.113795
+v 0.109726 0.230000 -0.087779
+v 0.120812 0.230000 -0.055250
+v 0.120812 0.230000 -0.103750
+v 0.125649 0.230000 -0.066947
+v 0.125649 0.230000 -0.092053
+v 0.127299 0.230000 -0.079500
+v 0.127299 0.192500 -0.079500
+v 0.125649 0.192500 -0.066947
+v 0.078876 0.192500 -0.128000
+v 0.066343 0.192500 -0.126347
+v 0.091409 0.192500 -0.126347
+v 0.070609 0.192500 -0.110399
+v 0.078876 0.192500 -0.111489
+v 0.062906 0.192500 -0.107203
+v 0.056292 0.192500 -0.102120
+v 0.051216 0.192500 -0.095494
+v 0.048025 0.192500 -0.087779
+v 0.046937 0.192500 -0.079500
+v 0.078876 0.192500 -0.031000
+v 0.091409 0.192500 -0.032653
+v 0.048025 0.192500 -0.071221
+v 0.051216 0.192500 -0.063506
+v 0.056292 0.192500 -0.056880
+v 0.062906 0.192500 -0.051797
+v 0.070609 0.192500 -0.048601
+v 0.113116 0.192500 -0.113795
+v 0.120812 0.192500 -0.103750
+v 0.054664 0.192500 -0.037498
+v 0.066343 0.192500 -0.032653
+v 0.078876 0.192500 -0.047511
+v 0.103087 0.192500 -0.121502
+v 0.044635 0.192500 -0.045205
+v 0.032102 0.192500 -0.092053
+v 0.032102 0.192500 -0.066947
+v 0.030452 0.192500 -0.079500
+v 0.036940 0.192500 -0.103750
+v 0.036940 0.192500 -0.055250
+v 0.044635 0.192500 -0.113795
+v 0.054664 0.192500 -0.121502
+v 0.087142 0.192500 -0.110399
+v 0.094845 0.192500 -0.107203
+v 0.101459 0.192500 -0.102120
+v 0.106535 0.192500 -0.095494
+v 0.109726 0.192500 -0.087779
+v 0.110814 0.192500 -0.079500
+v 0.087142 0.192500 -0.048601
+v 0.094845 0.192500 -0.051797
+v 0.103087 0.192500 -0.037498
+v 0.101459 0.192500 -0.056880
+v 0.106535 0.192500 -0.063506
+v 0.113116 0.192500 -0.045205
+v 0.109726 0.192500 -0.071221
+v 0.120812 0.192500 -0.055250
+v 0.125649 0.192500 -0.092053
+v 0.209182 0.000000 -0.172168
+v 0.222082 0.000000 -0.169980
+v 0.209684 0.000000 -0.168346
+v 0.221365 0.000000 -0.175438
+v 0.203914 0.000000 -0.184905
+v 0.214837 0.000000 -0.191222
+v 0.195535 0.000000 -0.195843
+v 0.204453 0.000000 -0.204776
+v 0.184614 0.000000 -0.204236
+v 0.171897 0.000000 -0.209512
+v 0.190921 0.000000 -0.215176
+v 0.166890 0.000000 -0.210172
+v 0.168739 0.000000 -0.222561
+v 0.175162 0.000000 -0.221714
+v 0.210321 0.072000 -0.163500
+v 0.222934 0.072000 -0.153500
+v 0.210321 0.072000 -0.153500
+v 0.222934 0.072000 -0.163500
+v 0.307016 0.087500 -0.293358
+v 0.307016 0.177500 -0.307500
+v 0.307016 0.177500 -0.293358
+v 0.307016 0.087500 -0.307500
+v 0.292896 0.177500 -0.307500
+v 0.292896 0.087500 -0.307500
+v 0.306517 0.087500 -0.293858
+v 0.247919 0.164278 -0.255382
+v 0.260160 0.164278 -0.253499
+v 0.254979 0.164278 -0.248311
+v 0.253100 0.164278 -0.260570
+v 0.293346 0.095877 -0.300880
+v 0.296132 0.107500 -0.289528
+v 0.289072 0.107500 -0.296600
+v 0.300406 0.095877 -0.293809
+v 0.306517 0.177500 -0.293858
+v 0.293346 0.177500 -0.300880
+v 0.300406 0.177500 -0.293809
+v 0.205207 0.177500 -0.212602
+v 0.205171 0.064876 -0.212566
+v 0.196464 0.177500 -0.210916
+v 0.196464 0.056941 -0.210916
+v 0.204453 0.058366 -0.204776
+v 0.212231 0.064876 -0.205495
+v 0.210584 0.056941 -0.196774
+v 0.178763 -0.000000 -0.226654
+v 0.176796 0.037500 -0.224399
+v 0.178763 0.037500 -0.226654
+v 0.176796 -0.000000 -0.224399
+v 0.189269 0.037500 -0.228100
+v 0.187510 -0.000000 -0.229249
+v 0.187510 0.037500 -0.229249
+v 0.189269 -0.000000 -0.228100
+v 0.190630 0.037500 -0.226303
+v 0.190630 -0.000000 -0.226303
+v 0.191512 0.037500 -0.223964
+v 0.191512 -0.000000 -0.223964
+v 0.191862 -0.000000 -0.221220
+v 0.191862 0.037500 -0.221220
+v 0.191661 -0.000000 -0.218232
+v 0.191661 0.037500 -0.218232
+v 0.183223 0.037500 -0.229376
+v 0.180948 -0.000000 -0.228346
+v 0.180948 0.037500 -0.228346
+v 0.183223 -0.000000 -0.229376
+v 0.185454 0.037500 -0.229683
+v 0.185454 -0.000000 -0.229683
+v 0.175162 0.037500 -0.221714
+v 0.175162 -0.000000 -0.221714
+v 0.190921 -0.000000 -0.215176
+v 0.190921 0.037500 -0.215176
+v 0.221618 0.000000 -0.192474
+v 0.218279 0.037500 -0.192126
+v 0.221618 0.037500 -0.192474
+v 0.218279 0.000000 -0.192126
+v 0.230741 0.037500 -0.186335
+v 0.230356 0.000000 -0.188414
+v 0.230356 0.037500 -0.188414
+v 0.230741 0.000000 -0.186335
+v 0.230291 0.037500 -0.184042
+v 0.230291 0.000000 -0.184042
+v 0.229034 0.037500 -0.181668
+v 0.229034 0.000000 -0.181668
+v 0.227044 0.000000 -0.179355
+v 0.227044 0.037500 -0.179355
+v 0.224436 0.000000 -0.177236
+v 0.224436 0.037500 -0.177236
+v 0.227223 0.037500 -0.191460
+v 0.224659 0.000000 -0.192248
+v 0.224659 0.037500 -0.192248
+v 0.227223 0.000000 -0.191460
+v 0.229160 0.037500 -0.190157
+v 0.229160 0.000000 -0.190157
+v 0.214837 0.037500 -0.191222
+v 0.214837 0.000000 -0.191222
+v 0.221365 0.000000 -0.175438
+v 0.221365 0.037500 -0.175438
+v 0.222082 0.072000 -0.169980
+v 0.209684 0.072000 -0.168346
+v 0.163243 0.072000 -0.210653
+v 0.168739 0.072000 -0.222561
+v 0.166890 0.072000 -0.210172
+v 0.163243 0.072000 -0.223286
+v 0.172549 0.083750 -0.214469
+v 0.163894 0.177500 -0.215610
+v 0.163894 0.083750 -0.215610
+v 0.172549 0.177500 -0.214469
+v 0.186525 0.083750 -0.208855
+v 0.173808 0.177500 -0.214131
+v 0.173808 0.083750 -0.214131
+v 0.186525 0.177500 -0.208855
+v 0.198574 0.083750 -0.199810
+v 0.187653 0.177500 -0.208202
+v 0.187653 0.083750 -0.208202
+v 0.198574 0.177500 -0.199810
+v 0.199495 0.083750 -0.198887
+v 0.207875 0.177500 -0.187949
+v 0.199495 0.177500 -0.198887
+v 0.207875 0.083750 -0.187949
+v 0.208526 0.083750 -0.186819
+v 0.213794 0.177500 -0.174082
+v 0.208526 0.177500 -0.186819
+v 0.213794 0.083750 -0.174082
+v 0.214131 0.083750 -0.172821
+v 0.215270 0.177500 -0.164153
+v 0.214131 0.177500 -0.172821
+v 0.215270 0.083750 -0.164153
+v 0.215313 0.083750 -0.163500
+v 0.215313 0.177500 -0.153500
+v 0.215313 0.177500 -0.163500
+v 0.215313 0.083750 -0.153500
+v 0.210321 0.083750 -0.163500
+v 0.210321 0.083750 -0.153500
+v 0.209182 0.083750 -0.172168
+v 0.203914 0.083750 -0.184905
+v 0.195535 0.083750 -0.195843
+v 0.184614 0.083750 -0.204236
+v 0.171897 0.083750 -0.209512
+v 0.163243 0.083750 -0.210653
+v 0.163243 0.083750 -0.215610
+v 0.163243 0.177500 -0.215610
+v 0.213044 0.099358 -0.220451
+v 0.220104 0.099358 -0.213380
+v 0.225335 0.130500 -0.232761
+v 0.232395 0.130500 -0.225690
+v 0.236627 0.153845 -0.244072
+v 0.243687 0.153845 -0.237000
+v 0.281137 0.147843 -0.274509
+v 0.274077 0.147843 -0.281580
+v 0.271834 0.159131 -0.265192
+v 0.264774 0.159131 -0.272263
+v 0.222934 0.177500 -0.153500
+v 0.163243 0.177500 -0.223286
+v 0.175162 0.177500 -0.221714
+v 0.190921 0.177500 -0.215176
+v 0.221365 0.177500 -0.175438
+v 0.222934 0.177500 -0.163500
+v 0.214837 0.177500 -0.191222
+v 0.210809 0.177500 -0.196479
+v 0.210584 0.177500 -0.196774
+v 0.212231 0.177500 -0.205495
+v 0.144603 0.000000 -0.209512
+v 0.146788 0.000000 -0.222433
+v 0.148420 0.000000 -0.210015
+v 0.141339 0.000000 -0.221714
+v 0.131886 0.000000 -0.204236
+v 0.125580 0.000000 -0.215176
+v 0.120966 0.000000 -0.195843
+v 0.112047 0.000000 -0.204776
+v 0.112587 0.000000 -0.184905
+v 0.107319 0.000000 -0.172168
+v 0.101663 0.000000 -0.191222
+v 0.106660 0.000000 -0.167154
+v 0.094290 0.000000 -0.169005
+v 0.095136 0.000000 -0.175438
+v 0.153258 0.072000 -0.210653
+v 0.163243 0.072000 -0.223286
+v 0.163243 0.072000 -0.210653
+v 0.153258 0.072000 -0.223286
+v 0.023605 0.087500 -0.307500
+v 0.009485 0.177500 -0.307500
+v 0.023605 0.177500 -0.307500
+v 0.009485 0.087500 -0.307500
+v 0.009485 0.177500 -0.293358
+v 0.009485 0.087500 -0.293358
+v 0.023106 0.087500 -0.307000
+v 0.061521 0.164278 -0.248311
+v 0.063401 0.164278 -0.260570
+v 0.068581 0.164278 -0.255382
+v 0.056341 0.164278 -0.253499
+v 0.016095 0.095877 -0.293809
+v 0.027428 0.107500 -0.296600
+v 0.020368 0.107500 -0.289528
+v 0.023155 0.095877 -0.300880
+v 0.023106 0.177500 -0.307000
+v 0.016095 0.177500 -0.293809
+v 0.023155 0.177500 -0.300880
+v 0.104234 0.177500 -0.205531
+v 0.104269 0.064876 -0.205495
+v 0.105917 0.177500 -0.196774
+v 0.105917 0.056941 -0.196774
+v 0.112047 0.058366 -0.204776
+v 0.111329 0.064876 -0.212566
+v 0.120037 0.056941 -0.210916
+v 0.090203 -0.000000 -0.179045
+v 0.092455 0.037500 -0.177075
+v 0.090203 0.037500 -0.179045
+v 0.092455 -0.000000 -0.177075
+v 0.088760 0.037500 -0.189568
+v 0.087613 -0.000000 -0.187805
+v 0.087613 0.037500 -0.187805
+v 0.088760 -0.000000 -0.189568
+v 0.090554 0.037500 -0.190931
+v 0.090554 -0.000000 -0.190931
+v 0.092889 0.037500 -0.191814
+v 0.092889 -0.000000 -0.191814
+v 0.095629 -0.000000 -0.192165
+v 0.095629 0.037500 -0.192165
+v 0.098612 -0.000000 -0.191964
+v 0.098612 0.037500 -0.191964
+v 0.087486 0.037500 -0.183512
+v 0.088514 -0.000000 -0.181234
+v 0.088514 0.037500 -0.181234
+v 0.087486 -0.000000 -0.183512
+v 0.087179 0.037500 -0.185746
+v 0.087179 -0.000000 -0.185746
+v 0.095136 0.037500 -0.175438
+v 0.095136 -0.000000 -0.175438
+v 0.101663 -0.000000 -0.191222
+v 0.101663 0.037500 -0.191222
+v 0.124329 0.000000 -0.221967
+v 0.124678 0.037500 -0.218623
+v 0.124329 0.037500 -0.221967
+v 0.124678 0.000000 -0.218623
+v 0.130459 0.037500 -0.231105
+v 0.128383 0.000000 -0.230720
+v 0.128383 0.037500 -0.230720
+v 0.130459 0.000000 -0.231105
+v 0.132749 0.037500 -0.230655
+v 0.132749 0.000000 -0.230655
+v 0.135118 0.037500 -0.229396
+v 0.135118 0.000000 -0.229396
+v 0.137429 0.000000 -0.227402
+v 0.137429 0.037500 -0.227402
+v 0.139543 0.000000 -0.224790
+v 0.139543 0.037500 -0.224790
+v 0.125342 0.037500 -0.227582
+v 0.124555 0.000000 -0.225013
+v 0.124555 0.037500 -0.225013
+v 0.125342 0.000000 -0.227582
+v 0.126643 0.037500 -0.229522
+v 0.126643 0.000000 -0.229522
+v 0.125580 0.037500 -0.215176
+v 0.125580 0.000000 -0.215176
+v 0.141339 0.000000 -0.221714
+v 0.141339 0.037500 -0.221714
+v 0.146788 0.072000 -0.222433
+v 0.148420 0.072000 -0.210015
+v 0.106180 0.072000 -0.163500
+v 0.094290 0.072000 -0.169005
+v 0.106660 0.072000 -0.167154
+v 0.093567 0.072000 -0.163500
+v 0.102370 0.083750 -0.172821
+v 0.101230 0.177500 -0.164153
+v 0.101230 0.083750 -0.164153
+v 0.102370 0.177500 -0.172821
+v 0.107975 0.083750 -0.186819
+v 0.102707 0.177500 -0.174082
+v 0.102707 0.083750 -0.174082
+v 0.107975 0.177500 -0.186819
+v 0.117006 0.083750 -0.198887
+v 0.108626 0.177500 -0.187949
+v 0.108626 0.083750 -0.187949
+v 0.117006 0.177500 -0.198887
+v 0.117927 0.083750 -0.199810
+v 0.128847 0.177500 -0.208202
+v 0.117927 0.177500 -0.199810
+v 0.128847 0.083750 -0.208202
+v 0.129976 0.083750 -0.208855
+v 0.142693 0.177500 -0.214131
+v 0.129976 0.177500 -0.208855
+v 0.142693 0.083750 -0.214131
+v 0.143952 0.083750 -0.214469
+v 0.152607 0.177500 -0.215610
+v 0.143952 0.177500 -0.214469
+v 0.152607 0.083750 -0.215610
+v 0.153258 0.083750 -0.215653
+v 0.163243 0.177500 -0.215653
+v 0.153258 0.177500 -0.215653
+v 0.163243 0.083750 -0.215653
+v 0.153258 0.083750 -0.210653
+v 0.163243 0.083750 -0.210653
+v 0.144603 0.083750 -0.209512
+v 0.131886 0.083750 -0.204236
+v 0.120966 0.083750 -0.195843
+v 0.112587 0.083750 -0.184905
+v 0.107319 0.083750 -0.172168
+v 0.106180 0.083750 -0.163500
+v 0.101230 0.083750 -0.163500
+v 0.101230 0.177500 -0.163500
+v 0.096397 0.099358 -0.213380
+v 0.103457 0.099358 -0.220451
+v 0.084106 0.130500 -0.225690
+v 0.091166 0.130500 -0.232761
+v 0.072814 0.153845 -0.237000
+v 0.079874 0.153845 -0.244072
+v 0.042424 0.147843 -0.281580
+v 0.035364 0.147843 -0.274509
+v 0.051726 0.159131 -0.272263
+v 0.044667 0.159131 -0.265192
+v 0.163243 0.177500 -0.223286
+v 0.093567 0.177500 -0.163500
+v 0.095136 0.177500 -0.175438
+v 0.101663 0.177500 -0.191222
+v 0.141339 0.177500 -0.221714
+v 0.153258 0.177500 -0.223286
+v 0.125580 0.177500 -0.215176
+v 0.120331 0.177500 -0.211142
+v 0.120037 0.177500 -0.210916
+v 0.111329 0.177500 -0.212566
+v 0.107319 0.000000 -0.144832
+v 0.094419 0.000000 -0.147020
+v 0.106817 0.000000 -0.148654
+v 0.095136 0.000000 -0.141562
+v 0.112587 0.000000 -0.132094
+v 0.101663 0.000000 -0.125778
+v 0.120966 0.000000 -0.121157
+v 0.112047 0.000000 -0.112224
+v 0.131886 0.000000 -0.112764
+v 0.144603 0.000000 -0.107488
+v 0.125580 0.000000 -0.101824
+v 0.149610 0.000000 -0.106828
+v 0.147762 0.000000 -0.094439
+v 0.141339 0.000000 -0.095286
+v 0.106180 0.072000 -0.153500
+v 0.093567 0.072000 -0.163500
+v 0.106180 0.072000 -0.163500
+v 0.093567 0.072000 -0.153500
+v 0.009485 0.087500 -0.023642
+v 0.009485 0.177500 -0.009500
+v 0.009485 0.177500 -0.023642
+v 0.009485 0.087500 -0.009500
+v 0.023605 0.177500 -0.009500
+v 0.023605 0.087500 -0.009500
+v 0.009984 0.087500 -0.023142
+v 0.068581 0.164278 -0.061618
+v 0.056341 0.164278 -0.063501
+v 0.061521 0.164278 -0.068689
+v 0.063401 0.164278 -0.056430
+v 0.023155 0.095877 -0.016120
+v 0.020368 0.107500 -0.027472
+v 0.027428 0.107500 -0.020400
+v 0.016095 0.095877 -0.023191
+v 0.009984 0.177500 -0.023142
+v 0.023155 0.177500 -0.016120
+v 0.016095 0.177500 -0.023191
+v 0.111293 0.177500 -0.104398
+v 0.111329 0.064876 -0.104434
+v 0.120037 0.177500 -0.106084
+v 0.120037 0.056941 -0.106084
+v 0.112047 0.058366 -0.112224
+v 0.104269 0.064876 -0.111505
+v 0.105917 0.056941 -0.120226
+v 0.137738 -0.000000 -0.090346
+v 0.139705 0.037500 -0.092601
+v 0.137738 0.037500 -0.090346
+v 0.139705 -0.000000 -0.092601
+v 0.127231 0.037500 -0.088900
+v 0.128991 -0.000000 -0.087751
+v 0.128991 0.037500 -0.087751
+v 0.127231 -0.000000 -0.088900
+v 0.125871 0.037500 -0.090697
+v 0.125871 -0.000000 -0.090697
+v 0.124989 0.037500 -0.093036
+v 0.124989 -0.000000 -0.093036
+v 0.124639 -0.000000 -0.095780
+v 0.124639 0.037500 -0.095780
+v 0.124839 -0.000000 -0.098768
+v 0.124839 0.037500 -0.098768
+v 0.133278 0.037500 -0.087624
+v 0.135552 -0.000000 -0.088654
+v 0.135552 0.037500 -0.088654
+v 0.133278 -0.000000 -0.087624
+v 0.131047 0.037500 -0.087317
+v 0.131047 -0.000000 -0.087317
+v 0.141339 0.037500 -0.095286
+v 0.141339 -0.000000 -0.095286
+v 0.125580 -0.000000 -0.101824
+v 0.125580 0.037500 -0.101824
+v 0.094883 0.000000 -0.124526
+v 0.098222 0.037500 -0.124874
+v 0.094883 0.037500 -0.124526
+v 0.098222 0.000000 -0.124874
+v 0.085760 0.037500 -0.130665
+v 0.086145 0.000000 -0.128586
+v 0.086145 0.037500 -0.128586
+v 0.085760 0.000000 -0.130665
+v 0.086209 0.037500 -0.132958
+v 0.086209 0.000000 -0.132958
+v 0.087466 0.037500 -0.135332
+v 0.087466 0.000000 -0.135332
+v 0.089457 0.000000 -0.137645
+v 0.089457 0.037500 -0.137645
+v 0.092065 0.000000 -0.139764
+v 0.092065 0.037500 -0.139764
+v 0.089277 0.037500 -0.125540
+v 0.091842 0.000000 -0.124752
+v 0.091842 0.037500 -0.124752
+v 0.089277 0.000000 -0.125540
+v 0.087340 0.037500 -0.126843
+v 0.087340 0.000000 -0.126843
+v 0.101663 0.037500 -0.125778
+v 0.101663 0.000000 -0.125778
+v 0.095136 0.000000 -0.141562
+v 0.095136 0.037500 -0.141562
+v 0.094419 0.072000 -0.147020
+v 0.106817 0.072000 -0.148654
+v 0.153258 0.072000 -0.106347
+v 0.147762 0.072000 -0.094439
+v 0.149610 0.072000 -0.106828
+v 0.153258 0.072000 -0.093714
+v 0.143952 0.083750 -0.102531
+v 0.152607 0.177500 -0.101390
+v 0.152607 0.083750 -0.101390
+v 0.143952 0.177500 -0.102531
+v 0.129976 0.083750 -0.108145
+v 0.142693 0.177500 -0.102869
+v 0.142693 0.083750 -0.102869
+v 0.129976 0.177500 -0.108145
+v 0.117927 0.083750 -0.117190
+v 0.128847 0.177500 -0.108798
+v 0.128847 0.083750 -0.108798
+v 0.117927 0.177500 -0.117190
+v 0.117006 0.083750 -0.118113
+v 0.108626 0.177500 -0.129051
+v 0.117006 0.177500 -0.118113
+v 0.108626 0.083750 -0.129051
+v 0.107975 0.083750 -0.130181
+v 0.102707 0.177500 -0.142918
+v 0.107975 0.177500 -0.130181
+v 0.102707 0.083750 -0.142918
+v 0.102370 0.083750 -0.144179
+v 0.101230 0.177500 -0.152847
+v 0.102370 0.177500 -0.144179
+v 0.101230 0.083750 -0.152847
+v 0.101188 0.083750 -0.153500
+v 0.101188 0.177500 -0.163500
+v 0.101188 0.177500 -0.153500
+v 0.101188 0.083750 -0.163500
+v 0.106180 0.083750 -0.153500
+v 0.106180 0.083750 -0.163500
+v 0.107319 0.083750 -0.144832
+v 0.112587 0.083750 -0.132094
+v 0.120966 0.083750 -0.121157
+v 0.131886 0.083750 -0.112764
+v 0.144603 0.083750 -0.107488
+v 0.153258 0.083750 -0.106347
+v 0.153258 0.083750 -0.101390
+v 0.153258 0.177500 -0.101390
+v 0.103457 0.099358 -0.096549
+v 0.096397 0.099358 -0.103620
+v 0.091166 0.130500 -0.084238
+v 0.084106 0.130500 -0.091310
+v 0.079874 0.153845 -0.072928
+v 0.072814 0.153845 -0.080000
+v 0.035364 0.147843 -0.042491
+v 0.042424 0.147843 -0.035420
+v 0.044667 0.159131 -0.051808
+v 0.051726 0.159131 -0.044737
+v 0.093567 0.177500 -0.163500
+v 0.153258 0.177500 -0.093714
+v 0.141339 0.177500 -0.095286
+v 0.125580 0.177500 -0.101824
+v 0.095136 0.177500 -0.141562
+v 0.093567 0.177500 -0.153500
+v 0.101663 0.177500 -0.125778
+v 0.105691 0.177500 -0.120521
+v 0.105917 0.177500 -0.120226
+v 0.104269 0.177500 -0.111505
+v 0.171897 0.000000 -0.107488
+v 0.169713 0.000000 -0.094567
+v 0.168081 0.000000 -0.106985
+v 0.175162 0.000000 -0.095286
+v 0.184614 0.000000 -0.112764
+v 0.190921 0.000000 -0.101824
+v 0.195535 0.000000 -0.121157
+v 0.204453 0.000000 -0.112224
+v 0.203914 0.000000 -0.132094
+v 0.209182 0.000000 -0.144832
+v 0.214837 0.000000 -0.125778
+v 0.209841 0.000000 -0.149846
+v 0.222210 0.000000 -0.147995
+v 0.221365 0.000000 -0.141562
+v 0.163243 0.072000 -0.106347
+v 0.153258 0.072000 -0.093714
+v 0.153258 0.072000 -0.106347
+v 0.163243 0.072000 -0.093714
+v 0.292896 0.087500 -0.009500
+v 0.307016 0.177500 -0.009500
+v 0.292896 0.177500 -0.009500
+v 0.307016 0.087500 -0.009500
+v 0.307016 0.177500 -0.023642
+v 0.307016 0.087500 -0.023642
+v 0.293395 0.087500 -0.010000
+v 0.254979 0.164278 -0.068689
+v 0.253100 0.164278 -0.056430
+v 0.247919 0.164278 -0.061618
+v 0.260160 0.164278 -0.063501
+v 0.300406 0.095877 -0.023191
+v 0.289072 0.107500 -0.020400
+v 0.296132 0.107500 -0.027472
+v 0.293346 0.095877 -0.016120
+v 0.293395 0.177500 -0.010000
+v 0.300406 0.177500 -0.023191
+v 0.293346 0.177500 -0.016120
+v 0.212267 0.177500 -0.111469
+v 0.212231 0.064876 -0.111505
+v 0.210584 0.177500 -0.120226
+v 0.210584 0.056941 -0.120226
+v 0.204453 0.058366 -0.112224
+v 0.205171 0.064876 -0.104434
+v 0.196464 0.056941 -0.106084
+v 0.226297 -0.000000 -0.137955
+v 0.224046 0.037500 -0.139925
+v 0.226297 0.037500 -0.137955
+v 0.224046 -0.000000 -0.139925
+v 0.227741 0.037500 -0.127432
+v 0.228888 -0.000000 -0.129195
+v 0.228888 0.037500 -0.129195
+v 0.227741 -0.000000 -0.127432
+v 0.225947 0.037500 -0.126069
+v 0.225947 -0.000000 -0.126069
+v 0.223611 0.037500 -0.125186
+v 0.223611 -0.000000 -0.125186
+v 0.220872 -0.000000 -0.124835
+v 0.220872 0.037500 -0.124835
+v 0.217889 -0.000000 -0.125036
+v 0.217889 0.037500 -0.125036
+v 0.229015 0.037500 -0.133488
+v 0.227987 -0.000000 -0.135766
+v 0.227987 0.037500 -0.135766
+v 0.229015 -0.000000 -0.133488
+v 0.229321 0.037500 -0.131254
+v 0.229321 -0.000000 -0.131254
+v 0.221365 0.037500 -0.141562
+v 0.221365 -0.000000 -0.141562
+v 0.214837 -0.000000 -0.125778
+v 0.214837 0.037500 -0.125778
+v 0.192171 0.000000 -0.095033
+v 0.191823 0.037500 -0.098377
+v 0.192171 0.037500 -0.095033
+v 0.191823 0.000000 -0.098377
+v 0.186042 0.037500 -0.085895
+v 0.188118 0.000000 -0.086280
+v 0.188118 0.037500 -0.086280
+v 0.186042 0.000000 -0.085895
+v 0.183752 0.037500 -0.086345
+v 0.183752 0.000000 -0.086345
+v 0.181382 0.037500 -0.087604
+v 0.181382 0.000000 -0.087604
+v 0.179072 0.000000 -0.089598
+v 0.179072 0.037500 -0.089598
+v 0.176957 0.000000 -0.092210
+v 0.176957 0.037500 -0.092210
+v 0.191159 0.037500 -0.089418
+v 0.191945 0.000000 -0.091987
+v 0.191945 0.037500 -0.091987
+v 0.191159 0.000000 -0.089418
+v 0.189857 0.037500 -0.087478
+v 0.189857 0.000000 -0.087478
+v 0.190921 0.037500 -0.101824
+v 0.190921 0.000000 -0.101824
+v 0.175162 0.000000 -0.095286
+v 0.175162 0.037500 -0.095286
+v 0.169713 0.072000 -0.094567
+v 0.168081 0.072000 -0.106985
+v 0.210321 0.072000 -0.153500
+v 0.222210 0.072000 -0.147995
+v 0.209841 0.072000 -0.149846
+v 0.222934 0.072000 -0.153500
+v 0.214131 0.083750 -0.144179
+v 0.215270 0.177500 -0.152847
+v 0.215270 0.083750 -0.152847
+v 0.214131 0.177500 -0.144179
+v 0.208526 0.083750 -0.130181
+v 0.213794 0.177500 -0.142918
+v 0.213794 0.083750 -0.142918
+v 0.208526 0.177500 -0.130181
+v 0.199495 0.083750 -0.118113
+v 0.207875 0.177500 -0.129051
+v 0.207875 0.083750 -0.129051
+v 0.199495 0.177500 -0.118113
+v 0.198574 0.083750 -0.117190
+v 0.187653 0.177500 -0.108798
+v 0.198574 0.177500 -0.117190
+v 0.187653 0.083750 -0.108798
+v 0.186525 0.083750 -0.108145
+v 0.173808 0.177500 -0.102869
+v 0.186525 0.177500 -0.108145
+v 0.173808 0.083750 -0.102869
+v 0.172549 0.083750 -0.102531
+v 0.163894 0.177500 -0.101390
+v 0.172549 0.177500 -0.102531
+v 0.163894 0.083750 -0.101390
+v 0.163243 0.083750 -0.101347
+v 0.153258 0.177500 -0.101347
+v 0.163243 0.177500 -0.101347
+v 0.153258 0.083750 -0.101347
+v 0.163243 0.083750 -0.106347
+v 0.153258 0.083750 -0.106347
+v 0.171897 0.083750 -0.107488
+v 0.184614 0.083750 -0.112764
+v 0.195535 0.083750 -0.121157
+v 0.203914 0.083750 -0.132094
+v 0.209182 0.083750 -0.144832
+v 0.210321 0.083750 -0.153500
+v 0.215270 0.083750 -0.153500
+v 0.215270 0.177500 -0.153500
+v 0.220104 0.099358 -0.103620
+v 0.213044 0.099358 -0.096549
+v 0.232395 0.130500 -0.091310
+v 0.225335 0.130500 -0.084238
+v 0.243687 0.153845 -0.080000
+v 0.236627 0.153845 -0.072928
+v 0.274077 0.147843 -0.035420
+v 0.281137 0.147843 -0.042491
+v 0.264774 0.159131 -0.044737
+v 0.271834 0.159131 -0.051808
+v 0.153258 0.177500 -0.093714
+v 0.222934 0.177500 -0.153500
+v 0.221365 0.177500 -0.141562
+v 0.214837 0.177500 -0.125778
+v 0.175162 0.177500 -0.095286
+v 0.163243 0.177500 -0.093714
+v 0.190921 0.177500 -0.101824
+v 0.196170 0.177500 -0.105858
+v 0.196464 0.177500 -0.106084
+v 0.205171 0.177500 -0.104434
+v 0.095723 0.054000 -0.024007
+v 0.097995 -0.000000 -0.019708
+v 0.095723 -0.000000 -0.024007
+v 0.097995 0.054000 -0.019708
+v 0.073968 -0.000000 -0.032132
+v 0.078876 0.054000 -0.032719
+v 0.078876 -0.000000 -0.032719
+v 0.073968 0.054000 -0.032132
+v 0.069349 -0.000000 -0.030407
+v 0.069349 0.054000 -0.030407
+v 0.065290 -0.000000 -0.027644
+v 0.065290 0.054000 -0.027644
+v 0.062029 0.054000 -0.024007
+v 0.062029 -0.000000 -0.024007
+v 0.059757 0.054000 -0.019708
+v 0.059757 -0.000000 -0.019708
+v 0.088402 -0.000000 -0.030407
+v 0.092461 0.054000 -0.027644
+v 0.092461 -0.000000 -0.027644
+v 0.088402 0.054000 -0.030407
+v 0.083783 -0.000000 -0.032132
+v 0.083783 0.054000 -0.032132
+v 0.099144 -0.000000 -0.015000
+v 0.099144 0.054000 -0.015000
+v 0.058608 0.054000 -0.015000
+v 0.058608 -0.000000 -0.015000
+v 0.062029 0.054000 -0.293493
+v 0.059757 -0.000000 -0.297792
+v 0.062029 -0.000000 -0.293493
+v 0.059757 0.054000 -0.297792
+v 0.083783 -0.000000 -0.285368
+v 0.078876 0.054000 -0.284781
+v 0.078876 -0.000000 -0.284781
+v 0.083783 0.054000 -0.285368
+v 0.088402 -0.000000 -0.287093
+v 0.088402 0.054000 -0.287093
+v 0.092461 -0.000000 -0.289856
+v 0.092461 0.054000 -0.289856
+v 0.095723 0.054000 -0.293493
+v 0.095723 -0.000000 -0.293493
+v 0.097995 0.054000 -0.297792
+v 0.097995 -0.000000 -0.297792
+v 0.069349 -0.000000 -0.287093
+v 0.065290 0.054000 -0.289856
+v 0.065290 -0.000000 -0.289856
+v 0.069349 0.054000 -0.287093
+v 0.073968 -0.000000 -0.285368
+v 0.073968 0.054000 -0.285368
+v 0.058608 -0.000000 -0.302500
+v 0.058608 0.054000 -0.302500
+v 0.099144 0.054000 -0.302500
+v 0.099144 -0.000000 -0.302500
+v 0.255471 0.054000 -0.024007
+v 0.257743 -0.000000 -0.019708
+v 0.255471 -0.000000 -0.024007
+v 0.257743 0.054000 -0.019708
+v 0.233716 -0.000000 -0.032132
+v 0.238624 0.054000 -0.032719
+v 0.238624 -0.000000 -0.032719
+v 0.233716 0.054000 -0.032132
+v 0.229097 -0.000000 -0.030407
+v 0.229097 0.054000 -0.030407
+v 0.225038 -0.000000 -0.027644
+v 0.225038 0.054000 -0.027644
+v 0.221777 0.054000 -0.024007
+v 0.221777 -0.000000 -0.024007
+v 0.219505 0.054000 -0.019708
+v 0.219505 -0.000000 -0.019708
+v 0.248150 -0.000000 -0.030407
+v 0.252209 0.054000 -0.027644
+v 0.252209 -0.000000 -0.027644
+v 0.248150 0.054000 -0.030407
+v 0.243531 -0.000000 -0.032132
+v 0.243531 0.054000 -0.032132
+v 0.258892 -0.000000 -0.015000
+v 0.258892 0.054000 -0.015000
+v 0.218356 0.054000 -0.015000
+v 0.218356 -0.000000 -0.015000
+v 0.221777 0.054000 -0.293493
+v 0.219505 -0.000000 -0.297792
+v 0.221777 -0.000000 -0.293493
+v 0.219505 0.054000 -0.297792
+v 0.243531 -0.000000 -0.285368
+v 0.238624 0.054000 -0.284781
+v 0.238624 -0.000000 -0.284781
+v 0.243531 0.054000 -0.285368
+v 0.248150 -0.000000 -0.287093
+v 0.248150 0.054000 -0.287093
+v 0.252209 -0.000000 -0.289856
+v 0.252209 0.054000 -0.289856
+v 0.255471 0.054000 -0.293493
+v 0.255471 -0.000000 -0.293493
+v 0.257743 0.054000 -0.297792
+v 0.257743 -0.000000 -0.297792
+v 0.229097 -0.000000 -0.287093
+v 0.225038 0.054000 -0.289856
+v 0.225038 -0.000000 -0.289856
+v 0.229097 0.054000 -0.287093
+v 0.233716 -0.000000 -0.285368
+v 0.233716 0.054000 -0.285368
+v 0.218356 -0.000000 -0.302500
+v 0.218356 0.054000 -0.302500
+v 0.258892 0.054000 -0.302500
+v 0.258892 -0.000000 -0.302500
+v 0.293031 0.054000 -0.095374
+v 0.297323 -0.000000 -0.097649
+v 0.293031 -0.000000 -0.095374
+v 0.297323 0.054000 -0.097649
+v 0.284918 -0.000000 -0.073585
+v 0.284333 0.054000 -0.078500
+v 0.284333 -0.000000 -0.078500
+v 0.284918 0.054000 -0.073585
+v 0.286641 -0.000000 -0.068959
+v 0.286641 0.054000 -0.068959
+v 0.289399 -0.000000 -0.064893
+v 0.289399 0.054000 -0.064893
+v 0.293031 0.054000 -0.061626
+v 0.293031 -0.000000 -0.061626
+v 0.297323 0.054000 -0.059351
+v 0.297323 -0.000000 -0.059351
+v 0.286641 -0.000000 -0.088041
+v 0.289399 0.054000 -0.092107
+v 0.289399 -0.000000 -0.092107
+v 0.286641 0.054000 -0.088041
+v 0.284918 -0.000000 -0.083415
+v 0.284918 0.054000 -0.083415
+v 0.302024 -0.000000 -0.098800
+v 0.302024 0.054000 -0.098800
+v 0.302024 0.054000 -0.058200
+v 0.302024 -0.000000 -0.058200
+v 0.293031 0.054000 -0.255374
+v 0.297323 -0.000000 -0.257649
+v 0.293031 -0.000000 -0.255374
+v 0.297323 0.054000 -0.257649
+v 0.284918 -0.000000 -0.233585
+v 0.284333 0.054000 -0.238500
+v 0.284333 -0.000000 -0.238500
+v 0.284918 0.054000 -0.233585
+v 0.286641 -0.000000 -0.228959
+v 0.286641 0.054000 -0.228959
+v 0.289399 -0.000000 -0.224893
+v 0.289399 0.054000 -0.224893
+v 0.293031 0.054000 -0.221626
+v 0.293031 -0.000000 -0.221626
+v 0.297323 0.054000 -0.219351
+v 0.297323 -0.000000 -0.219351
+v 0.286641 -0.000000 -0.248041
+v 0.289399 0.054000 -0.252107
+v 0.289399 -0.000000 -0.252107
+v 0.286641 0.054000 -0.248041
+v 0.284918 -0.000000 -0.243415
+v 0.284918 0.054000 -0.243415
+v 0.302024 -0.000000 -0.258800
+v 0.302024 0.054000 -0.258800
+v 0.302024 0.054000 -0.218200
+v 0.302024 -0.000000 -0.218200
+v 0.009984 0.177500 -0.010000
+v 0.009984 0.177500 -0.307500
+v 0.307016 0.177500 -0.010000
+v 0.307016 0.177500 -0.023642
+v 0.302024 0.057500 -0.302500
+v 0.302024 0.057500 -0.015000
+v 0.307016 0.057500 -0.010000
+v 0.307016 0.057500 -0.307500
+v 0.302024 0.057500 -0.010000
+v 0.302024 0.057500 -0.307500
+v 0.014976 0.057500 -0.015000
+v 0.009984 0.057500 -0.010000
+v 0.009984 0.057500 -0.015000
+v 0.014976 0.057500 -0.302500
+v 0.009984 0.057500 -0.307500
+v 0.014976 0.057500 -0.307500
+v 0.300406 0.177500 -0.023191
+v 0.307016 0.159664 -0.023642
+v 0.289044 0.177500 -0.023642
+v 0.307016 0.147500 -0.023642
+v 0.307016 0.177500 -0.023642
+v 0.307016 0.147500 -0.302500
+v 0.307016 0.177500 -0.302500
+v 0.289044 0.177500 -0.302500
+v 0.293065 0.177500 -0.289761
+v 0.293065 0.147500 -0.307761
+v 0.293065 0.177500 -0.307761
+v 0.014646 0.147500 -0.307761
+v 0.014646 0.177500 -0.307761
+v 0.014646 0.177500 -0.289761
+v 0.027956 0.177500 -0.293392
+v 0.009984 0.147500 -0.293392
+v 0.009984 0.177500 -0.293392
+v 0.009984 0.147500 -0.014534
+v 0.009984 0.177500 -0.014534
+v 0.027956 0.177500 -0.014534
+v 0.023571 0.177500 -0.028000
+v 0.023571 0.147500 -0.010000
+v 0.023571 0.177500 -0.010000
+v 0.301990 0.147500 -0.010000
+v 0.301990 0.177500 -0.010000
+v 0.301990 0.177500 -0.028000
+usemtl None
+s off
+f 1 2 3
+f 2 1 4
+f 2 4 5
+f 5 4 6
+f 3 7 8
+f 7 3 2
+f 8 7 6
+f 8 6 4
+f 9 3 8
+f 3 9 10
+f 11 4 1
+f 4 11 12
+f 11 3 10
+f 3 11 1
+f 4 9 8
+f 9 4 12
+f 13 14 15
+f 14 13 16
+f 17 18 19
+f 18 17 20
+f 21 20 17
+f 20 21 22
+f 23 22 21
+f 22 23 24
+f 25 23 26
+f 23 25 24
+f 27 26 28
+f 26 27 25
+f 29 30 31
+f 30 29 32
+f 30 15 31
+f 15 30 13
+f 33 32 29
+f 32 33 34
+f 19 34 33
+f 34 19 18
+f 16 35 14
+f 35 16 36
+f 37 28 38
+f 28 37 27
+f 28 35 38
+f 35 28 14
+f 14 28 26
+f 14 26 15
+f 15 26 23
+f 15 23 31
+f 31 23 29
+f 29 23 21
+f 29 21 17
+f 29 17 33
+f 33 17 19
+f 37 35 36
+f 35 37 38
+f 36 27 37
+f 27 36 16
+f 27 16 25
+f 25 16 13
+f 25 13 30
+f 25 30 24
+f 24 30 32
+f 24 32 22
+f 22 32 34
+f 22 34 20
+f 20 34 18
+f 39 40 41
+f 40 39 42
+f 43 44 45
+f 44 43 46
+f 47 46 43
+f 46 47 48
+f 49 48 47
+f 48 49 50
+f 51 49 52
+f 49 51 50
+f 53 52 54
+f 52 53 51
+f 55 56 57
+f 56 55 58
+f 56 41 57
+f 41 56 39
+f 59 58 55
+f 58 59 60
+f 45 60 59
+f 60 45 44
+f 42 61 40
+f 61 42 62
+f 63 54 64
+f 54 63 53
+f 54 61 64
+f 61 54 40
+f 40 54 52
+f 40 52 41
+f 41 52 49
+f 41 49 57
+f 57 49 55
+f 55 49 47
+f 55 47 43
+f 55 43 59
+f 59 43 45
+f 63 61 62
+f 61 63 64
+f 62 53 63
+f 53 62 42
+f 53 42 51
+f 51 42 39
+f 51 39 56
+f 51 56 50
+f 50 56 58
+f 50 58 48
+f 48 58 60
+f 48 60 46
+f 46 60 44
+f 65 66 67
+f 66 65 68
+f 66 68 69
+f 69 68 70
+f 70 68 71
+f 70 71 72
+f 72 71 73
+f 73 71 74
+f 73 74 75
+f 75 74 76
+f 76 74 77
+f 76 77 78
+f 78 77 79
+f 78 79 80
+f 80 79 81
+f 81 79 82
+f 81 82 83
+f 83 82 84
+f 85 86 87
+f 86 85 88
+f 86 88 89
+f 89 88 90
+f 90 88 65
+f 90 65 67
+f 90 67 91
+f 90 91 92
+f 92 91 93
+f 92 93 94
+f 92 94 95
+f 95 94 96
+f 95 96 97
+f 95 97 98
+f 98 97 99
+f 98 99 100
+f 98 100 101
+f 101 100 102
+f 101 102 103
+f 103 102 104
+f 103 104 105
+f 103 105 106
+f 106 105 107
+f 106 107 84
+f 106 84 82
+f 106 82 108
+f 106 108 109
+f 109 108 110
+f 109 110 111
+f 111 110 112
+f 113 110 114
+f 110 113 112
+f 95 115 116
+f 115 95 98
+f 98 117 115
+f 117 98 101
+f 99 118 119
+f 118 99 97
+f 97 120 118
+f 120 97 96
+f 96 121 120
+f 121 96 94
+f 121 93 122
+f 93 121 94
+f 122 91 123
+f 91 122 93
+f 123 67 124
+f 67 123 91
+f 77 125 126
+f 125 77 74
+f 124 66 127
+f 66 124 67
+f 127 69 128
+f 69 127 66
+f 128 70 129
+f 70 128 69
+f 70 130 129
+f 130 70 72
+f 72 131 130
+f 131 72 73
+f 132 109 133
+f 109 132 106
+f 71 134 135
+f 134 71 68
+f 73 136 131
+f 136 73 75
+f 101 137 117
+f 137 101 103
+f 68 138 134
+f 138 68 65
+f 139 140 141
+f 140 139 142
+f 140 142 143
+f 143 142 144
+f 143 144 138
+f 138 144 124
+f 124 144 123
+f 123 144 145
+f 123 145 122
+f 122 145 121
+f 121 145 116
+f 121 116 120
+f 120 116 118
+f 118 116 115
+f 118 115 119
+f 119 115 117
+f 119 117 146
+f 146 117 147
+f 147 117 137
+f 147 137 148
+f 148 137 149
+f 149 137 132
+f 149 132 150
+f 150 132 151
+f 138 127 134
+f 127 138 124
+f 134 127 128
+f 134 128 129
+f 134 129 135
+f 135 129 130
+f 135 130 131
+f 135 131 125
+f 125 131 136
+f 125 136 152
+f 125 152 126
+f 126 152 153
+f 126 153 154
+f 154 153 155
+f 154 155 156
+f 154 156 157
+f 157 156 158
+f 157 158 151
+f 157 151 132
+f 157 132 133
+f 157 133 159
+f 159 133 160
+f 159 160 114
+f 114 160 113
+f 114 108 159
+f 108 114 110
+f 159 82 157
+f 82 159 108
+f 74 135 125
+f 135 74 71
+f 85 143 88
+f 143 85 140
+f 90 142 89
+f 142 90 144
+f 90 145 144
+f 145 90 92
+f 92 116 145
+f 116 92 95
+f 103 132 137
+f 132 103 106
+f 133 111 160
+f 111 133 109
+f 82 154 157
+f 154 82 79
+f 160 112 113
+f 112 160 111
+f 79 126 154
+f 126 79 77
+f 88 138 65
+f 138 88 143
+f 87 140 85
+f 140 87 141
+f 86 141 87
+f 141 86 139
+f 89 139 86
+f 139 89 142
+f 75 152 136
+f 152 75 76
+f 84 158 83
+f 158 84 151
+f 105 150 107
+f 150 105 149
+f 83 156 81
+f 156 83 158
+f 104 149 105
+f 149 104 148
+f 81 155 80
+f 155 81 156
+f 100 119 146
+f 119 100 99
+f 78 155 153
+f 155 78 80
+f 107 151 84
+f 151 107 150
+f 102 146 147
+f 146 102 100
+f 104 147 148
+f 147 104 102
+f 76 153 152
+f 153 76 78
+f 161 162 163
+f 162 161 164
+f 162 164 165
+f 165 164 166
+f 166 164 167
+f 166 167 168
+f 168 167 169
+f 169 167 170
+f 169 170 171
+f 171 170 172
+f 172 170 173
+f 172 173 174
+f 174 173 175
+f 174 175 176
+f 176 175 177
+f 177 175 178
+f 177 178 179
+f 179 178 180
+f 181 182 183
+f 182 181 184
+f 182 184 185
+f 185 184 186
+f 186 184 161
+f 186 161 163
+f 186 163 187
+f 186 187 188
+f 188 187 189
+f 188 189 190
+f 188 190 191
+f 191 190 192
+f 191 192 193
+f 191 193 194
+f 194 193 195
+f 194 195 196
+f 194 196 197
+f 197 196 198
+f 197 198 199
+f 199 198 200
+f 199 200 201
+f 199 201 202
+f 202 201 203
+f 202 203 180
+f 202 180 178
+f 202 178 204
+f 202 204 205
+f 205 204 206
+f 205 206 207
+f 207 206 208
+f 209 206 210
+f 206 209 208
+f 191 211 212
+f 211 191 194
+f 194 213 211
+f 213 194 197
+f 195 214 215
+f 214 195 193
+f 193 216 214
+f 216 193 192
+f 192 217 216
+f 217 192 190
+f 217 189 218
+f 189 217 190
+f 218 187 219
+f 187 218 189
+f 219 163 220
+f 163 219 187
+f 173 221 222
+f 221 173 170
+f 220 162 223
+f 162 220 163
+f 223 165 224
+f 165 223 162
+f 224 166 225
+f 166 224 165
+f 166 226 225
+f 226 166 168
+f 168 227 226
+f 227 168 169
+f 228 205 229
+f 205 228 202
+f 167 230 231
+f 230 167 164
+f 169 232 227
+f 232 169 171
+f 197 233 213
+f 233 197 199
+f 164 234 230
+f 234 164 161
+f 235 236 237
+f 236 235 238
+f 236 238 239
+f 239 238 240
+f 239 240 234
+f 234 240 220
+f 220 240 219
+f 219 240 241
+f 219 241 218
+f 218 241 217
+f 217 241 212
+f 217 212 216
+f 216 212 214
+f 214 212 211
+f 214 211 215
+f 215 211 213
+f 215 213 242
+f 242 213 243
+f 243 213 233
+f 243 233 244
+f 244 233 245
+f 245 233 228
+f 245 228 246
+f 246 228 247
+f 234 223 230
+f 223 234 220
+f 230 223 224
+f 230 224 225
+f 230 225 231
+f 231 225 226
+f 231 226 227
+f 231 227 221
+f 221 227 232
+f 221 232 248
+f 221 248 222
+f 222 248 249
+f 222 249 250
+f 250 249 251
+f 250 251 252
+f 250 252 253
+f 253 252 254
+f 253 254 247
+f 253 247 228
+f 253 228 229
+f 253 229 255
+f 255 229 256
+f 255 256 210
+f 210 256 209
+f 210 204 255
+f 204 210 206
+f 255 178 253
+f 178 255 204
+f 170 231 221
+f 231 170 167
+f 181 239 184
+f 239 181 236
+f 186 238 185
+f 238 186 240
+f 186 241 240
+f 241 186 188
+f 188 212 241
+f 212 188 191
+f 199 228 233
+f 228 199 202
+f 229 207 256
+f 207 229 205
+f 178 250 253
+f 250 178 175
+f 256 208 209
+f 208 256 207
+f 175 222 250
+f 222 175 173
+f 184 234 161
+f 234 184 239
+f 183 236 181
+f 236 183 237
+f 182 237 183
+f 237 182 235
+f 185 235 182
+f 235 185 238
+f 171 248 232
+f 248 171 172
+f 180 254 179
+f 254 180 247
+f 201 246 203
+f 246 201 245
+f 179 252 177
+f 252 179 254
+f 200 245 201
+f 245 200 244
+f 177 251 176
+f 251 177 252
+f 196 215 242
+f 215 196 195
+f 174 251 249
+f 251 174 176
+f 203 247 180
+f 247 203 246
+f 198 242 243
+f 242 198 196
+f 200 243 244
+f 243 200 198
+f 172 249 248
+f 249 172 174
+f 257 258 259
+f 258 257 260
+f 258 260 261
+f 261 260 262
+f 262 260 263
+f 262 263 264
+f 264 263 265
+f 265 263 266
+f 265 266 267
+f 267 266 268
+f 268 266 269
+f 268 269 270
+f 270 269 271
+f 270 271 272
+f 272 271 273
+f 273 271 274
+f 273 274 275
+f 275 274 276
+f 277 278 279
+f 278 277 280
+f 278 280 281
+f 281 280 282
+f 282 280 257
+f 282 257 259
+f 282 259 283
+f 282 283 284
+f 284 283 285
+f 284 285 286
+f 284 286 287
+f 287 286 288
+f 287 288 289
+f 287 289 290
+f 290 289 291
+f 290 291 292
+f 290 292 293
+f 293 292 294
+f 293 294 295
+f 295 294 296
+f 295 296 297
+f 295 297 298
+f 298 297 299
+f 298 299 276
+f 298 276 274
+f 298 274 300
+f 298 300 301
+f 301 300 302
+f 301 302 303
+f 303 302 304
+f 305 302 306
+f 302 305 304
+f 287 307 308
+f 307 287 290
+f 290 309 307
+f 309 290 293
+f 291 310 311
+f 310 291 289
+f 289 312 310
+f 312 289 288
+f 288 313 312
+f 313 288 286
+f 313 285 314
+f 285 313 286
+f 314 283 315
+f 283 314 285
+f 315 259 316
+f 259 315 283
+f 269 317 318
+f 317 269 266
+f 316 258 319
+f 258 316 259
+f 319 261 320
+f 261 319 258
+f 320 262 321
+f 262 320 261
+f 262 322 321
+f 322 262 264
+f 264 323 322
+f 323 264 265
+f 324 301 325
+f 301 324 298
+f 263 326 327
+f 326 263 260
+f 265 328 323
+f 328 265 267
+f 293 329 309
+f 329 293 295
+f 260 330 326
+f 330 260 257
+f 331 332 333
+f 332 331 334
+f 332 334 335
+f 335 334 336
+f 335 336 330
+f 330 336 316
+f 316 336 315
+f 315 336 337
+f 315 337 314
+f 314 337 313
+f 313 337 308
+f 313 308 312
+f 312 308 310
+f 310 308 307
+f 310 307 311
+f 311 307 309
+f 311 309 338
+f 338 309 339
+f 339 309 329
+f 339 329 340
+f 340 329 341
+f 341 329 324
+f 341 324 342
+f 342 324 343
+f 330 319 326
+f 319 330 316
+f 326 319 320
+f 326 320 321
+f 326 321 327
+f 327 321 322
+f 327 322 323
+f 327 323 317
+f 317 323 328
+f 317 328 344
+f 317 344 318
+f 318 344 345
+f 318 345 346
+f 346 345 347
+f 346 347 348
+f 346 348 349
+f 349 348 350
+f 349 350 343
+f 349 343 324
+f 349 324 325
+f 349 325 351
+f 351 325 352
+f 351 352 306
+f 306 352 305
+f 306 300 351
+f 300 306 302
+f 351 274 349
+f 274 351 300
+f 266 327 317
+f 327 266 263
+f 277 335 280
+f 335 277 332
+f 282 334 281
+f 334 282 336
+f 282 337 336
+f 337 282 284
+f 284 308 337
+f 308 284 287
+f 295 324 329
+f 324 295 298
+f 325 303 352
+f 303 325 301
+f 274 346 349
+f 346 274 271
+f 352 304 305
+f 304 352 303
+f 271 318 346
+f 318 271 269
+f 280 330 257
+f 330 280 335
+f 279 332 277
+f 332 279 333
+f 278 333 279
+f 333 278 331
+f 281 331 278
+f 331 281 334
+f 267 344 328
+f 344 267 268
+f 276 350 275
+f 350 276 343
+f 297 342 299
+f 342 297 341
+f 275 348 273
+f 348 275 350
+f 296 341 297
+f 341 296 340
+f 273 347 272
+f 347 273 348
+f 292 311 338
+f 311 292 291
+f 270 347 345
+f 347 270 272
+f 299 343 276
+f 343 299 342
+f 294 338 339
+f 338 294 292
+f 296 339 340
+f 339 296 294
+f 268 345 344
+f 345 268 270
+f 353 354 355
+f 354 353 356
+f 354 356 357
+f 357 356 358
+f 358 356 359
+f 358 359 360
+f 360 359 361
+f 361 359 362
+f 361 362 363
+f 363 362 364
+f 364 362 365
+f 364 365 366
+f 366 365 367
+f 366 367 368
+f 368 367 369
+f 369 367 370
+f 369 370 371
+f 371 370 372
+f 373 374 375
+f 374 373 376
+f 374 376 377
+f 377 376 378
+f 378 376 353
+f 378 353 355
+f 378 355 379
+f 378 379 380
+f 380 379 381
+f 380 381 382
+f 380 382 383
+f 383 382 384
+f 383 384 385
+f 383 385 386
+f 386 385 387
+f 386 387 388
+f 386 388 389
+f 389 388 390
+f 389 390 391
+f 391 390 392
+f 391 392 393
+f 391 393 394
+f 394 393 395
+f 394 395 372
+f 394 372 370
+f 394 370 396
+f 394 396 397
+f 397 396 398
+f 397 398 399
+f 399 398 400
+f 401 398 402
+f 398 401 400
+f 383 403 404
+f 403 383 386
+f 386 405 403
+f 405 386 389
+f 387 406 407
+f 406 387 385
+f 385 408 406
+f 408 385 384
+f 384 409 408
+f 409 384 382
+f 409 381 410
+f 381 409 382
+f 410 379 411
+f 379 410 381
+f 411 355 412
+f 355 411 379
+f 365 413 414
+f 413 365 362
+f 412 354 415
+f 354 412 355
+f 415 357 416
+f 357 415 354
+f 416 358 417
+f 358 416 357
+f 358 418 417
+f 418 358 360
+f 360 419 418
+f 419 360 361
+f 420 397 421
+f 397 420 394
+f 359 422 423
+f 422 359 356
+f 361 424 419
+f 424 361 363
+f 389 425 405
+f 425 389 391
+f 356 426 422
+f 426 356 353
+f 427 428 429
+f 428 427 430
+f 428 430 431
+f 431 430 432
+f 431 432 426
+f 426 432 412
+f 412 432 411
+f 411 432 433
+f 411 433 410
+f 410 433 409
+f 409 433 404
+f 409 404 408
+f 408 404 406
+f 406 404 403
+f 406 403 407
+f 407 403 405
+f 407 405 434
+f 434 405 435
+f 435 405 425
+f 435 425 436
+f 436 425 437
+f 437 425 420
+f 437 420 438
+f 438 420 439
+f 426 415 422
+f 415 426 412
+f 422 415 416
+f 422 416 417
+f 422 417 423
+f 423 417 418
+f 423 418 419
+f 423 419 413
+f 413 419 424
+f 413 424 440
+f 413 440 414
+f 414 440 441
+f 414 441 442
+f 442 441 443
+f 442 443 444
+f 442 444 445
+f 445 444 446
+f 445 446 439
+f 445 439 420
+f 445 420 421
+f 445 421 447
+f 447 421 448
+f 447 448 402
+f 402 448 401
+f 402 396 447
+f 396 402 398
+f 447 370 445
+f 370 447 396
+f 362 423 413
+f 423 362 359
+f 373 431 376
+f 431 373 428
+f 378 430 377
+f 430 378 432
+f 378 433 432
+f 433 378 380
+f 380 404 433
+f 404 380 383
+f 391 420 425
+f 420 391 394
+f 421 399 448
+f 399 421 397
+f 370 442 445
+f 442 370 367
+f 448 400 401
+f 400 448 399
+f 367 414 442
+f 414 367 365
+f 376 426 353
+f 426 376 431
+f 375 428 373
+f 428 375 429
+f 374 429 375
+f 429 374 427
+f 377 427 374
+f 427 377 430
+f 363 440 424
+f 440 363 364
+f 372 446 371
+f 446 372 439
+f 393 438 395
+f 438 393 437
+f 371 444 369
+f 444 371 446
+f 392 437 393
+f 437 392 436
+f 369 443 368
+f 443 369 444
+f 388 407 434
+f 407 388 387
+f 366 443 441
+f 443 366 368
+f 395 439 372
+f 439 395 438
+f 390 434 435
+f 434 390 388
+f 392 435 436
+f 435 392 390
+f 364 441 440
+f 441 364 366
+f 449 450 451
+f 450 449 452
+f 452 449 453
+f 452 453 454
+f 454 453 455
+f 454 455 456
+f 456 455 457
+f 456 457 458
+f 456 458 459
+f 459 458 460
+f 459 460 461
+f 459 461 462
+f 463 464 465
+f 464 463 466
+f 467 468 469
+f 468 467 470
+f 471 470 472
+f 470 471 468
+f 473 470 467
+f 470 473 472
+f 474 475 476
+f 475 474 477
+f 478 479 480
+f 479 478 481
+f 473 469 482
+f 469 473 467
+f 472 483 471
+f 483 472 478
+f 473 478 472
+f 478 473 481
+f 473 484 481
+f 484 473 482
+f 485 486 487
+f 486 488 487
+f 489 490 491
+f 490 489 488
+f 490 488 486
+f 492 493 494
+f 493 492 495
+f 496 497 498
+f 497 496 499
+f 500 499 496
+f 499 500 501
+f 502 501 500
+f 501 502 503
+f 504 502 505
+f 502 504 503
+f 506 505 507
+f 505 506 504
+f 508 509 510
+f 509 508 511
+f 509 494 510
+f 494 509 492
+f 512 511 508
+f 511 512 513
+f 498 513 512
+f 513 498 497
+f 495 514 493
+f 514 495 515
+f 516 507 517
+f 507 516 506
+f 507 514 517
+f 514 507 493
+f 493 507 505
+f 493 505 494
+f 494 505 502
+f 494 502 510
+f 510 502 508
+f 508 502 500
+f 508 500 496
+f 508 496 512
+f 512 496 498
+f 516 514 515
+f 514 516 517
+f 515 506 516
+f 506 515 495
+f 506 495 504
+f 504 495 492
+f 504 492 509
+f 504 509 503
+f 503 509 511
+f 503 511 501
+f 501 511 513
+f 501 513 499
+f 499 513 497
+f 518 519 520
+f 519 518 521
+f 522 523 524
+f 523 522 525
+f 526 525 522
+f 525 526 527
+f 528 527 526
+f 527 528 529
+f 530 528 531
+f 528 530 529
+f 532 531 533
+f 531 532 530
+f 534 535 536
+f 535 534 537
+f 535 520 536
+f 520 535 518
+f 538 537 534
+f 537 538 539
+f 524 539 538
+f 539 524 523
+f 521 540 519
+f 540 521 541
+f 542 533 543
+f 533 542 532
+f 533 540 543
+f 540 533 519
+f 519 533 531
+f 519 531 520
+f 520 531 528
+f 520 528 536
+f 536 528 534
+f 534 528 526
+f 534 526 522
+f 534 522 538
+f 538 522 524
+f 542 540 541
+f 540 542 543
+f 541 532 542
+f 532 541 521
+f 532 521 530
+f 530 521 518
+f 530 518 535
+f 530 535 529
+f 529 535 537
+f 529 537 527
+f 527 537 539
+f 527 539 525
+f 525 539 523
+f 463 544 466
+f 544 463 545
+f 450 545 451
+f 545 450 544
+f 546 547 548
+f 547 546 549
+f 461 548 547
+f 548 461 460
+f 550 551 552
+f 551 550 553
+f 554 555 556
+f 555 554 557
+f 558 559 560
+f 559 558 561
+f 562 563 564
+f 563 562 565
+f 566 567 568
+f 567 566 569
+f 570 571 572
+f 571 570 573
+f 574 575 576
+f 575 574 577
+f 577 578 579
+f 578 577 574
+f 578 574 580
+f 580 574 573
+f 580 573 570
+f 580 570 581
+f 581 570 569
+f 581 569 566
+f 581 566 582
+f 582 566 565
+f 582 565 562
+f 582 562 583
+f 583 562 558
+f 583 558 560
+f 583 560 584
+f 584 560 554
+f 584 554 556
+f 584 556 585
+f 585 556 586
+f 586 556 550
+f 586 550 552
+f 552 587 586
+f 587 552 551
+f 556 553 550
+f 553 556 555
+f 560 557 554
+f 557 560 559
+f 562 561 558
+f 561 562 564
+f 565 568 563
+f 568 565 566
+f 569 572 567
+f 572 569 570
+f 573 576 571
+f 576 573 574
+f 490 588 589
+f 588 490 486
+f 590 589 588
+f 589 590 591
+f 591 592 593
+f 592 591 590
+f 592 476 593
+f 476 592 474
+f 480 594 595
+f 594 480 479
+f 477 596 475
+f 596 477 597
+f 597 594 596
+f 594 597 595
+f 455 583 457
+f 583 455 582
+f 458 548 460
+f 548 585 546
+f 585 548 584
+f 584 548 458
+f 457 584 458
+f 584 457 583
+f 580 545 578
+f 545 580 449
+f 545 449 451
+f 463 578 545
+f 453 580 581
+f 580 453 449
+f 455 581 582
+f 581 455 453
+f 464 579 465
+f 579 464 577
+f 577 464 575
+f 575 464 598
+f 463 579 578
+f 579 463 465
+f 549 587 599
+f 587 549 586
+f 586 549 585
+f 585 549 546
+f 600 549 599
+f 549 600 547
+f 547 600 461
+f 461 600 462
+f 487 459 601
+f 459 487 488
+f 459 488 456
+f 456 488 489
+f 601 462 600
+f 462 601 459
+f 485 588 486
+f 588 485 590
+f 590 485 592
+f 592 485 474
+f 474 485 483
+f 474 483 477
+f 477 483 597
+f 597 483 595
+f 595 483 480
+f 480 483 478
+f 466 602 603
+f 602 466 544
+f 602 544 452
+f 452 544 450
+f 452 604 602
+f 604 452 454
+f 454 605 604
+f 605 454 606
+f 606 454 491
+f 491 456 489
+f 456 491 454
+f 491 607 606
+f 607 491 490
+f 607 476 484
+f 476 607 593
+f 593 607 591
+f 591 607 589
+f 589 607 490
+f 484 476 475
+f 484 475 596
+f 484 596 594
+f 484 594 479
+f 484 479 481
+f 464 603 598
+f 603 464 466
+f 468 482 469
+f 482 468 483
+f 483 468 471
+f 603 575 598
+f 575 603 576
+f 576 603 602
+f 576 602 571
+f 571 602 572
+f 572 602 567
+f 567 602 568
+f 568 602 604
+f 568 604 563
+f 563 604 564
+f 564 604 605
+f 564 605 606
+f 564 606 607
+f 564 607 561
+f 561 607 559
+f 559 607 485
+f 485 607 484
+f 559 485 557
+f 557 485 487
+f 485 484 483
+f 483 484 482
+f 487 555 557
+f 555 487 601
+f 555 601 553
+f 553 601 551
+f 551 601 600
+f 551 600 587
+f 587 600 599
+f 608 609 610
+f 609 608 611
+f 611 608 612
+f 611 612 613
+f 613 612 614
+f 613 614 615
+f 615 614 616
+f 615 616 617
+f 615 617 618
+f 618 617 619
+f 618 619 620
+f 618 620 621
+f 622 623 624
+f 623 622 625
+f 626 627 628
+f 627 626 629
+f 630 629 631
+f 629 630 627
+f 632 629 626
+f 629 632 631
+f 633 634 635
+f 634 633 636
+f 637 638 639
+f 638 637 640
+f 632 628 641
+f 628 632 626
+f 631 642 630
+f 642 631 637
+f 632 637 631
+f 637 632 640
+f 632 643 640
+f 643 632 641
+f 644 645 646
+f 645 647 646
+f 648 649 650
+f 649 648 647
+f 649 647 645
+f 651 652 653
+f 652 651 654
+f 655 656 657
+f 656 655 658
+f 659 658 655
+f 658 659 660
+f 661 660 659
+f 660 661 662
+f 663 661 664
+f 661 663 662
+f 665 664 666
+f 664 665 663
+f 667 668 669
+f 668 667 670
+f 668 653 669
+f 653 668 651
+f 671 670 667
+f 670 671 672
+f 657 672 671
+f 672 657 656
+f 654 673 652
+f 673 654 674
+f 675 666 676
+f 666 675 665
+f 666 673 676
+f 673 666 652
+f 652 666 664
+f 652 664 653
+f 653 664 661
+f 653 661 669
+f 669 661 667
+f 667 661 659
+f 667 659 655
+f 667 655 671
+f 671 655 657
+f 675 673 674
+f 673 675 676
+f 674 665 675
+f 665 674 654
+f 665 654 663
+f 663 654 651
+f 663 651 668
+f 663 668 662
+f 662 668 670
+f 662 670 660
+f 660 670 672
+f 660 672 658
+f 658 672 656
+f 677 678 679
+f 678 677 680
+f 681 682 683
+f 682 681 684
+f 685 684 681
+f 684 685 686
+f 687 686 685
+f 686 687 688
+f 689 687 690
+f 687 689 688
+f 691 690 692
+f 690 691 689
+f 693 694 695
+f 694 693 696
+f 694 679 695
+f 679 694 677
+f 697 696 693
+f 696 697 698
+f 683 698 697
+f 698 683 682
+f 680 699 678
+f 699 680 700
+f 701 692 702
+f 692 701 691
+f 692 699 702
+f 699 692 678
+f 678 692 690
+f 678 690 679
+f 679 690 687
+f 679 687 695
+f 695 687 693
+f 693 687 685
+f 693 685 681
+f 693 681 697
+f 697 681 683
+f 701 699 700
+f 699 701 702
+f 700 691 701
+f 691 700 680
+f 691 680 689
+f 689 680 677
+f 689 677 694
+f 689 694 688
+f 688 694 696
+f 688 696 686
+f 686 696 698
+f 686 698 684
+f 684 698 682
+f 622 703 625
+f 703 622 704
+f 609 704 610
+f 704 609 703
+f 705 706 707
+f 706 705 708
+f 620 707 706
+f 707 620 619
+f 709 710 711
+f 710 709 712
+f 713 714 715
+f 714 713 716
+f 717 718 719
+f 718 717 720
+f 721 722 723
+f 722 721 724
+f 725 726 727
+f 726 725 728
+f 729 730 731
+f 730 729 732
+f 733 734 735
+f 734 733 736
+f 736 737 738
+f 737 736 733
+f 737 733 739
+f 739 733 732
+f 739 732 729
+f 739 729 740
+f 740 729 728
+f 740 728 725
+f 740 725 741
+f 741 725 724
+f 741 724 721
+f 741 721 742
+f 742 721 717
+f 742 717 719
+f 742 719 743
+f 743 719 713
+f 743 713 715
+f 743 715 744
+f 744 715 745
+f 745 715 709
+f 745 709 711
+f 711 746 745
+f 746 711 710
+f 715 712 709
+f 712 715 714
+f 719 716 713
+f 716 719 718
+f 721 720 717
+f 720 721 723
+f 724 727 722
+f 727 724 725
+f 728 731 726
+f 731 728 729
+f 732 735 730
+f 735 732 733
+f 649 747 748
+f 747 649 645
+f 749 748 747
+f 748 749 750
+f 750 751 752
+f 751 750 749
+f 751 635 752
+f 635 751 633
+f 639 753 754
+f 753 639 638
+f 636 755 634
+f 755 636 756
+f 756 753 755
+f 753 756 754
+f 614 742 616
+f 742 614 741
+f 617 707 619
+f 707 744 705
+f 744 707 743
+f 743 707 617
+f 616 743 617
+f 743 616 742
+f 739 704 737
+f 704 739 608
+f 704 608 610
+f 622 737 704
+f 612 739 740
+f 739 612 608
+f 614 740 741
+f 740 614 612
+f 623 738 624
+f 738 623 736
+f 736 623 734
+f 734 623 757
+f 622 738 737
+f 738 622 624
+f 708 746 758
+f 746 708 745
+f 745 708 744
+f 744 708 705
+f 759 708 758
+f 708 759 706
+f 706 759 620
+f 620 759 621
+f 646 618 760
+f 618 646 647
+f 618 647 615
+f 615 647 648
+f 760 621 759
+f 621 760 618
+f 644 747 645
+f 747 644 749
+f 749 644 751
+f 751 644 633
+f 633 644 642
+f 633 642 636
+f 636 642 756
+f 756 642 754
+f 754 642 639
+f 639 642 637
+f 625 761 762
+f 761 625 703
+f 761 703 611
+f 611 703 609
+f 611 763 761
+f 763 611 613
+f 613 764 763
+f 764 613 765
+f 765 613 650
+f 650 615 648
+f 615 650 613
+f 650 766 765
+f 766 650 649
+f 766 635 643
+f 635 766 752
+f 752 766 750
+f 750 766 748
+f 748 766 649
+f 643 635 634
+f 643 634 755
+f 643 755 753
+f 643 753 638
+f 643 638 640
+f 623 762 757
+f 762 623 625
+f 627 641 628
+f 641 627 642
+f 642 627 630
+f 762 734 757
+f 734 762 735
+f 735 762 761
+f 735 761 730
+f 730 761 731
+f 731 761 726
+f 726 761 727
+f 727 761 763
+f 727 763 722
+f 722 763 723
+f 723 763 764
+f 723 764 765
+f 723 765 766
+f 723 766 720
+f 720 766 718
+f 718 766 644
+f 644 766 643
+f 718 644 716
+f 716 644 646
+f 644 643 642
+f 642 643 641
+f 646 714 716
+f 714 646 760
+f 714 760 712
+f 712 760 710
+f 710 760 759
+f 710 759 746
+f 746 759 758
+f 767 768 769
+f 768 767 770
+f 770 767 771
+f 770 771 772
+f 772 771 773
+f 772 773 774
+f 774 773 775
+f 774 775 776
+f 774 776 777
+f 777 776 778
+f 777 778 779
+f 777 779 780
+f 781 782 783
+f 782 781 784
+f 785 786 787
+f 786 785 788
+f 789 788 790
+f 788 789 786
+f 791 788 785
+f 788 791 790
+f 792 793 794
+f 793 792 795
+f 796 797 798
+f 797 796 799
+f 791 787 800
+f 787 791 785
+f 790 801 789
+f 801 790 796
+f 791 796 790
+f 796 791 799
+f 791 802 799
+f 802 791 800
+f 803 804 805
+f 804 806 805
+f 807 808 809
+f 808 807 806
+f 808 806 804
+f 810 811 812
+f 811 810 813
+f 814 815 816
+f 815 814 817
+f 818 817 814
+f 817 818 819
+f 820 819 818
+f 819 820 821
+f 822 820 823
+f 820 822 821
+f 824 823 825
+f 823 824 822
+f 826 827 828
+f 827 826 829
+f 827 812 828
+f 812 827 810
+f 830 829 826
+f 829 830 831
+f 816 831 830
+f 831 816 815
+f 813 832 811
+f 832 813 833
+f 834 825 835
+f 825 834 824
+f 825 832 835
+f 832 825 811
+f 811 825 823
+f 811 823 812
+f 812 823 820
+f 812 820 828
+f 828 820 826
+f 826 820 818
+f 826 818 814
+f 826 814 830
+f 830 814 816
+f 834 832 833
+f 832 834 835
+f 833 824 834
+f 824 833 813
+f 824 813 822
+f 822 813 810
+f 822 810 827
+f 822 827 821
+f 821 827 829
+f 821 829 819
+f 819 829 831
+f 819 831 817
+f 817 831 815
+f 836 837 838
+f 837 836 839
+f 840 841 842
+f 841 840 843
+f 844 843 840
+f 843 844 845
+f 846 845 844
+f 845 846 847
+f 848 846 849
+f 846 848 847
+f 850 849 851
+f 849 850 848
+f 852 853 854
+f 853 852 855
+f 853 838 854
+f 838 853 836
+f 856 855 852
+f 855 856 857
+f 842 857 856
+f 857 842 841
+f 839 858 837
+f 858 839 859
+f 860 851 861
+f 851 860 850
+f 851 858 861
+f 858 851 837
+f 837 851 849
+f 837 849 838
+f 838 849 846
+f 838 846 854
+f 854 846 852
+f 852 846 844
+f 852 844 840
+f 852 840 856
+f 856 840 842
+f 860 858 859
+f 858 860 861
+f 859 850 860
+f 850 859 839
+f 850 839 848
+f 848 839 836
+f 848 836 853
+f 848 853 847
+f 847 853 855
+f 847 855 845
+f 845 855 857
+f 845 857 843
+f 843 857 841
+f 781 862 784
+f 862 781 863
+f 768 863 769
+f 863 768 862
+f 864 865 866
+f 865 864 867
+f 779 866 865
+f 866 779 778
+f 868 869 870
+f 869 868 871
+f 872 873 874
+f 873 872 875
+f 876 877 878
+f 877 876 879
+f 880 881 882
+f 881 880 883
+f 884 885 886
+f 885 884 887
+f 888 889 890
+f 889 888 891
+f 892 893 894
+f 893 892 895
+f 895 896 897
+f 896 895 892
+f 896 892 898
+f 898 892 891
+f 898 891 888
+f 898 888 899
+f 899 888 887
+f 899 887 884
+f 899 884 900
+f 900 884 883
+f 900 883 880
+f 900 880 901
+f 901 880 876
+f 901 876 878
+f 901 878 902
+f 902 878 872
+f 902 872 874
+f 902 874 903
+f 903 874 904
+f 904 874 868
+f 904 868 870
+f 870 905 904
+f 905 870 869
+f 874 871 868
+f 871 874 873
+f 878 875 872
+f 875 878 877
+f 880 879 876
+f 879 880 882
+f 883 886 881
+f 886 883 884
+f 887 890 885
+f 890 887 888
+f 891 894 889
+f 894 891 892
+f 808 906 907
+f 906 808 804
+f 908 907 906
+f 907 908 909
+f 909 910 911
+f 910 909 908
+f 910 794 911
+f 794 910 792
+f 798 912 913
+f 912 798 797
+f 795 914 793
+f 914 795 915
+f 915 912 914
+f 912 915 913
+f 773 901 775
+f 901 773 900
+f 776 866 778
+f 866 903 864
+f 903 866 902
+f 902 866 776
+f 775 902 776
+f 902 775 901
+f 898 863 896
+f 863 898 767
+f 863 767 769
+f 781 896 863
+f 771 898 899
+f 898 771 767
+f 773 899 900
+f 899 773 771
+f 782 897 783
+f 897 782 895
+f 895 782 893
+f 893 782 916
+f 781 897 896
+f 897 781 783
+f 867 905 917
+f 905 867 904
+f 904 867 903
+f 903 867 864
+f 918 867 917
+f 867 918 865
+f 865 918 779
+f 779 918 780
+f 805 777 919
+f 777 805 806
+f 777 806 774
+f 774 806 807
+f 919 780 918
+f 780 919 777
+f 803 906 804
+f 906 803 908
+f 908 803 910
+f 910 803 792
+f 792 803 801
+f 792 801 795
+f 795 801 915
+f 915 801 913
+f 913 801 798
+f 798 801 796
+f 784 920 921
+f 920 784 862
+f 920 862 770
+f 770 862 768
+f 770 922 920
+f 922 770 772
+f 772 923 922
+f 923 772 924
+f 924 772 809
+f 809 774 807
+f 774 809 772
+f 809 925 924
+f 925 809 808
+f 925 794 802
+f 794 925 911
+f 911 925 909
+f 909 925 907
+f 907 925 808
+f 802 794 793
+f 802 793 914
+f 802 914 912
+f 802 912 797
+f 802 797 799
+f 782 921 916
+f 921 782 784
+f 786 800 787
+f 800 786 801
+f 801 786 789
+f 921 893 916
+f 893 921 894
+f 894 921 920
+f 894 920 889
+f 889 920 890
+f 890 920 885
+f 885 920 886
+f 886 920 922
+f 886 922 881
+f 881 922 882
+f 882 922 923
+f 882 923 924
+f 882 924 925
+f 882 925 879
+f 879 925 877
+f 877 925 803
+f 803 925 802
+f 877 803 875
+f 875 803 805
+f 803 802 801
+f 801 802 800
+f 805 873 875
+f 873 805 919
+f 873 919 871
+f 871 919 869
+f 869 919 918
+f 869 918 905
+f 905 918 917
+f 926 927 928
+f 927 926 929
+f 929 926 930
+f 929 930 931
+f 931 930 932
+f 931 932 933
+f 933 932 934
+f 933 934 935
+f 933 935 936
+f 936 935 937
+f 936 937 938
+f 936 938 939
+f 940 941 942
+f 941 940 943
+f 944 945 946
+f 945 944 947
+f 948 947 949
+f 947 948 945
+f 950 947 944
+f 947 950 949
+f 951 952 953
+f 952 951 954
+f 955 956 957
+f 956 955 958
+f 950 946 959
+f 946 950 944
+f 949 960 948
+f 960 949 955
+f 950 955 949
+f 955 950 958
+f 950 961 958
+f 961 950 959
+f 962 963 964
+f 963 965 964
+f 966 967 968
+f 967 966 965
+f 967 965 963
+f 969 970 971
+f 970 969 972
+f 973 974 975
+f 974 973 976
+f 977 976 973
+f 976 977 978
+f 979 978 977
+f 978 979 980
+f 981 979 982
+f 979 981 980
+f 983 982 984
+f 982 983 981
+f 985 986 987
+f 986 985 988
+f 986 971 987
+f 971 986 969
+f 989 988 985
+f 988 989 990
+f 975 990 989
+f 990 975 974
+f 972 991 970
+f 991 972 992
+f 993 984 994
+f 984 993 983
+f 984 991 994
+f 991 984 970
+f 970 984 982
+f 970 982 971
+f 971 982 979
+f 971 979 987
+f 987 979 985
+f 985 979 977
+f 985 977 973
+f 985 973 989
+f 989 973 975
+f 993 991 992
+f 991 993 994
+f 992 983 993
+f 983 992 972
+f 983 972 981
+f 981 972 969
+f 981 969 986
+f 981 986 980
+f 980 986 988
+f 980 988 978
+f 978 988 990
+f 978 990 976
+f 976 990 974
+f 995 996 997
+f 996 995 998
+f 999 1000 1001
+f 1000 999 1002
+f 1003 1002 999
+f 1002 1003 1004
+f 1005 1004 1003
+f 1004 1005 1006
+f 1007 1005 1008
+f 1005 1007 1006
+f 1009 1008 1010
+f 1008 1009 1007
+f 1011 1012 1013
+f 1012 1011 1014
+f 1012 997 1013
+f 997 1012 995
+f 1015 1014 1011
+f 1014 1015 1016
+f 1001 1016 1015
+f 1016 1001 1000
+f 998 1017 996
+f 1017 998 1018
+f 1019 1010 1020
+f 1010 1019 1009
+f 1010 1017 1020
+f 1017 1010 996
+f 996 1010 1008
+f 996 1008 997
+f 997 1008 1005
+f 997 1005 1013
+f 1013 1005 1011
+f 1011 1005 1003
+f 1011 1003 999
+f 1011 999 1015
+f 1015 999 1001
+f 1019 1017 1018
+f 1017 1019 1020
+f 1018 1009 1019
+f 1009 1018 998
+f 1009 998 1007
+f 1007 998 995
+f 1007 995 1012
+f 1007 1012 1006
+f 1006 1012 1014
+f 1006 1014 1004
+f 1004 1014 1016
+f 1004 1016 1002
+f 1002 1016 1000
+f 940 1021 943
+f 1021 940 1022
+f 927 1022 928
+f 1022 927 1021
+f 1023 1024 1025
+f 1024 1023 1026
+f 938 1025 1024
+f 1025 938 937
+f 1027 1028 1029
+f 1028 1027 1030
+f 1031 1032 1033
+f 1032 1031 1034
+f 1035 1036 1037
+f 1036 1035 1038
+f 1039 1040 1041
+f 1040 1039 1042
+f 1043 1044 1045
+f 1044 1043 1046
+f 1047 1048 1049
+f 1048 1047 1050
+f 1051 1052 1053
+f 1052 1051 1054
+f 1054 1055 1056
+f 1055 1054 1051
+f 1055 1051 1057
+f 1057 1051 1050
+f 1057 1050 1047
+f 1057 1047 1058
+f 1058 1047 1046
+f 1058 1046 1043
+f 1058 1043 1059
+f 1059 1043 1042
+f 1059 1042 1039
+f 1059 1039 1060
+f 1060 1039 1035
+f 1060 1035 1037
+f 1060 1037 1061
+f 1061 1037 1031
+f 1061 1031 1033
+f 1061 1033 1062
+f 1062 1033 1063
+f 1063 1033 1027
+f 1063 1027 1029
+f 1029 1064 1063
+f 1064 1029 1028
+f 1033 1030 1027
+f 1030 1033 1032
+f 1037 1034 1031
+f 1034 1037 1036
+f 1039 1038 1035
+f 1038 1039 1041
+f 1042 1045 1040
+f 1045 1042 1043
+f 1046 1049 1044
+f 1049 1046 1047
+f 1050 1053 1048
+f 1053 1050 1051
+f 967 1065 1066
+f 1065 967 963
+f 1067 1066 1065
+f 1066 1067 1068
+f 1068 1069 1070
+f 1069 1068 1067
+f 1069 953 1070
+f 953 1069 951
+f 957 1071 1072
+f 1071 957 956
+f 954 1073 952
+f 1073 954 1074
+f 1074 1071 1073
+f 1071 1074 1072
+f 932 1060 934
+f 1060 932 1059
+f 935 1025 937
+f 1025 1062 1023
+f 1062 1025 1061
+f 1061 1025 935
+f 934 1061 935
+f 1061 934 1060
+f 1057 1022 1055
+f 1022 1057 926
+f 1022 926 928
+f 940 1055 1022
+f 930 1057 1058
+f 1057 930 926
+f 932 1058 1059
+f 1058 932 930
+f 941 1056 942
+f 1056 941 1054
+f 1054 941 1052
+f 1052 941 1075
+f 940 1056 1055
+f 1056 940 942
+f 1026 1064 1076
+f 1064 1026 1063
+f 1063 1026 1062
+f 1062 1026 1023
+f 1077 1026 1076
+f 1026 1077 1024
+f 1024 1077 938
+f 938 1077 939
+f 964 936 1078
+f 936 964 965
+f 936 965 933
+f 933 965 966
+f 1078 939 1077
+f 939 1078 936
+f 962 1065 963
+f 1065 962 1067
+f 1067 962 1069
+f 1069 962 951
+f 951 962 960
+f 951 960 954
+f 954 960 1074
+f 1074 960 1072
+f 1072 960 957
+f 957 960 955
+f 943 1079 1080
+f 1079 943 1021
+f 1079 1021 929
+f 929 1021 927
+f 929 1081 1079
+f 1081 929 931
+f 931 1082 1081
+f 1082 931 1083
+f 1083 931 968
+f 968 933 966
+f 933 968 931
+f 968 1084 1083
+f 1084 968 967
+f 1084 953 961
+f 953 1084 1070
+f 1070 1084 1068
+f 1068 1084 1066
+f 1066 1084 967
+f 961 953 952
+f 961 952 1073
+f 961 1073 1071
+f 961 1071 956
+f 961 956 958
+f 941 1080 1075
+f 1080 941 943
+f 945 959 946
+f 959 945 960
+f 960 945 948
+f 1080 1052 1075
+f 1052 1080 1053
+f 1053 1080 1079
+f 1053 1079 1048
+f 1048 1079 1049
+f 1049 1079 1044
+f 1044 1079 1045
+f 1045 1079 1081
+f 1045 1081 1040
+f 1040 1081 1041
+f 1041 1081 1082
+f 1041 1082 1083
+f 1041 1083 1084
+f 1041 1084 1038
+f 1038 1084 1036
+f 1036 1084 962
+f 962 1084 961
+f 1036 962 1034
+f 1034 962 964
+f 962 961 960
+f 960 961 959
+f 964 1032 1034
+f 1032 964 1078
+f 1032 1078 1030
+f 1030 1078 1028
+f 1028 1078 1077
+f 1028 1077 1064
+f 1064 1077 1076
+f 1085 1086 1087
+f 1086 1085 1088
+f 1089 1090 1091
+f 1090 1089 1092
+f 1093 1092 1089
+f 1092 1093 1094
+f 1095 1094 1093
+f 1094 1095 1096
+f 1097 1095 1098
+f 1095 1097 1096
+f 1099 1098 1100
+f 1098 1099 1097
+f 1101 1102 1103
+f 1102 1101 1104
+f 1102 1087 1103
+f 1087 1102 1085
+f 1105 1104 1101
+f 1104 1105 1106
+f 1091 1106 1105
+f 1106 1091 1090
+f 1088 1107 1086
+f 1107 1088 1108
+f 1109 1100 1110
+f 1100 1109 1099
+f 1100 1107 1110
+f 1107 1100 1086
+f 1086 1100 1098
+f 1086 1098 1087
+f 1087 1098 1095
+f 1087 1095 1103
+f 1103 1095 1101
+f 1101 1095 1093
+f 1101 1093 1089
+f 1101 1089 1105
+f 1105 1089 1091
+f 1109 1107 1108
+f 1107 1109 1110
+f 1108 1099 1109
+f 1099 1108 1088
+f 1099 1088 1097
+f 1097 1088 1085
+f 1097 1085 1102
+f 1097 1102 1096
+f 1096 1102 1104
+f 1096 1104 1094
+f 1094 1104 1106
+f 1094 1106 1092
+f 1092 1106 1090
+f 1111 1112 1113
+f 1112 1111 1114
+f 1115 1116 1117
+f 1116 1115 1118
+f 1119 1118 1115
+f 1118 1119 1120
+f 1121 1120 1119
+f 1120 1121 1122
+f 1123 1121 1124
+f 1121 1123 1122
+f 1125 1124 1126
+f 1124 1125 1123
+f 1127 1128 1129
+f 1128 1127 1130
+f 1128 1113 1129
+f 1113 1128 1111
+f 1131 1130 1127
+f 1130 1131 1132
+f 1117 1132 1131
+f 1132 1117 1116
+f 1114 1133 1112
+f 1133 1114 1134
+f 1135 1126 1136
+f 1126 1135 1125
+f 1126 1133 1136
+f 1133 1126 1112
+f 1112 1126 1124
+f 1112 1124 1113
+f 1113 1124 1121
+f 1113 1121 1129
+f 1129 1121 1127
+f 1127 1121 1119
+f 1127 1119 1115
+f 1127 1115 1131
+f 1131 1115 1117
+f 1135 1133 1134
+f 1133 1135 1136
+f 1134 1125 1135
+f 1125 1134 1114
+f 1125 1114 1123
+f 1123 1114 1111
+f 1123 1111 1128
+f 1123 1128 1122
+f 1122 1128 1130
+f 1122 1130 1120
+f 1120 1130 1132
+f 1120 1132 1118
+f 1118 1132 1116
+f 1137 1138 1139
+f 1138 1137 1140
+f 1141 1142 1143
+f 1142 1141 1144
+f 1145 1144 1141
+f 1144 1145 1146
+f 1147 1146 1145
+f 1146 1147 1148
+f 1149 1147 1150
+f 1147 1149 1148
+f 1151 1150 1152
+f 1150 1151 1149
+f 1153 1154 1155
+f 1154 1153 1156
+f 1154 1139 1155
+f 1139 1154 1137
+f 1157 1156 1153
+f 1156 1157 1158
+f 1143 1158 1157
+f 1158 1143 1142
+f 1140 1159 1138
+f 1159 1140 1160
+f 1161 1152 1162
+f 1152 1161 1151
+f 1152 1159 1162
+f 1159 1152 1138
+f 1138 1152 1150
+f 1138 1150 1139
+f 1139 1150 1147
+f 1139 1147 1155
+f 1155 1147 1153
+f 1153 1147 1145
+f 1153 1145 1141
+f 1153 1141 1157
+f 1157 1141 1143
+f 1161 1159 1160
+f 1159 1161 1162
+f 1160 1151 1161
+f 1151 1160 1140
+f 1151 1140 1149
+f 1149 1140 1137
+f 1149 1137 1154
+f 1149 1154 1148
+f 1148 1154 1156
+f 1148 1156 1146
+f 1146 1156 1158
+f 1146 1158 1144
+f 1144 1158 1142
+f 1163 1164 1165
+f 1164 1163 1166
+f 1167 1168 1169
+f 1168 1167 1170
+f 1171 1170 1167
+f 1170 1171 1172
+f 1173 1172 1171
+f 1172 1173 1174
+f 1175 1173 1176
+f 1173 1175 1174
+f 1177 1176 1178
+f 1176 1177 1175
+f 1179 1180 1181
+f 1180 1179 1182
+f 1180 1165 1181
+f 1165 1180 1163
+f 1183 1182 1179
+f 1182 1183 1184
+f 1169 1184 1183
+f 1184 1169 1168
+f 1166 1185 1164
+f 1185 1166 1186
+f 1187 1178 1188
+f 1178 1187 1177
+f 1178 1185 1188
+f 1185 1178 1164
+f 1164 1178 1176
+f 1164 1176 1165
+f 1165 1176 1173
+f 1165 1173 1181
+f 1181 1173 1179
+f 1179 1173 1171
+f 1179 1171 1167
+f 1179 1167 1183
+f 1183 1167 1169
+f 1187 1185 1186
+f 1185 1187 1188
+f 1186 1177 1187
+f 1177 1186 1166
+f 1177 1166 1175
+f 1175 1166 1163
+f 1175 1163 1180
+f 1175 1180 1174
+f 1174 1180 1182
+f 1174 1182 1172
+f 1172 1182 1184
+f 1172 1184 1170
+f 1170 1184 1168
+f 1189 1190 1191
+f 1190 1189 1192
+f 1193 1194 1195
+f 1194 1193 1196
+f 1197 1196 1193
+f 1196 1197 1198
+f 1199 1198 1197
+f 1198 1199 1200
+f 1201 1199 1202
+f 1199 1201 1200
+f 1203 1202 1204
+f 1202 1203 1201
+f 1205 1206 1207
+f 1206 1205 1208
+f 1206 1191 1207
+f 1191 1206 1189
+f 1209 1208 1205
+f 1208 1209 1210
+f 1195 1210 1209
+f 1210 1195 1194
+f 1192 1211 1190
+f 1211 1192 1212
+f 1213 1204 1214
+f 1204 1213 1203
+f 1204 1211 1214
+f 1211 1204 1190
+f 1190 1204 1202
+f 1190 1202 1191
+f 1191 1202 1199
+f 1191 1199 1207
+f 1207 1199 1205
+f 1205 1199 1197
+f 1205 1197 1193
+f 1205 1193 1209
+f 1209 1193 1195
+f 1213 1211 1212
+f 1211 1213 1214
+f 1212 1203 1213
+f 1203 1212 1192
+f 1203 1192 1201
+f 1201 1192 1189
+f 1201 1189 1206
+f 1201 1206 1200
+f 1200 1206 1208
+f 1200 1208 1198
+f 1198 1208 1210
+f 1198 1210 1196
+f 1196 1210 1194
+f 1215 1216 1217
+f 1216 1215 1218
+f 1219 1220 1221
+f 1220 1219 1222
+f 1223 1222 1219
+f 1222 1223 1224
+f 1225 1224 1223
+f 1224 1225 1226
+f 1227 1225 1228
+f 1225 1227 1226
+f 1229 1228 1230
+f 1228 1229 1227
+f 1231 1232 1233
+f 1232 1231 1234
+f 1232 1217 1233
+f 1217 1232 1215
+f 1235 1234 1231
+f 1234 1235 1236
+f 1221 1236 1235
+f 1236 1221 1220
+f 1218 1237 1216
+f 1237 1218 1238
+f 1239 1230 1240
+f 1230 1239 1229
+f 1230 1237 1240
+f 1237 1230 1216
+f 1216 1230 1228
+f 1216 1228 1217
+f 1217 1228 1225
+f 1217 1225 1233
+f 1233 1225 1231
+f 1231 1225 1223
+f 1231 1223 1219
+f 1231 1219 1235
+f 1235 1219 1221
+f 1239 1237 1238
+f 1237 1239 1240
+f 1238 1229 1239
+f 1229 1238 1218
+f 1229 1218 1227
+f 1227 1218 1215
+f 1227 1215 1232
+f 1227 1232 1226
+f 1226 1232 1234
+f 1226 1234 1224
+f 1224 1234 1236
+f 1224 1236 1222
+f 1222 1236 1220
+f 468 1241 1242
+f 1241 468 1243
+f 1243 468 1244
+f 1245 6 1246
+f 6 1245 5
+f 9 11 10
+f 11 9 12
+f 1244 1247 1243
+f 1247 1244 1248
+f 1248 1244 468
+f 1247 1246 1249
+f 1246 1247 1245
+f 1245 1247 1250
+f 1250 1247 1248
+f 1251 6 7
+f 6 1251 1246
+f 1241 1249 1252
+f 1249 1241 1247
+f 1247 1241 1243
+f 1249 1253 1252
+f 1253 1249 1251
+f 1251 1249 1246
+f 2 1251 7
+f 1251 2 1254
+f 1253 1241 1252
+f 1241 1253 1242
+f 1242 1253 1255
+f 1251 1255 1253
+f 1255 1251 1256
+f 1256 1251 1254
+f 1245 2 5
+f 2 1245 1254
+f 468 1250 1248
+f 1250 468 1256
+f 1256 468 1255
+f 1255 468 1242
+f 1245 1256 1254
+f 1256 1245 1250
+f 1257 1258 1244
+f 1259 1260 1261
+f 1262 1261 1260
+f 1261 1262 1263
+f 1264 1260 1259
+f 1260 1264 1262
+f 1263 1262 1264
+f 1261 1264 1259
+f 1264 1261 1263
+f 1265 1266 1267
+f 1268 1267 1266
+f 1267 1268 1269
+f 1270 1266 1265
+f 1266 1270 1268
+f 1269 1268 1270
+f 1267 1270 1265
+f 1270 1267 1269
+f 1271 1272 1273
+f 1274 1273 1272
+f 1273 1274 1275
+f 1276 1272 1271
+f 1272 1276 1274
+f 1275 1274 1276
+f 1273 1276 1271
+f 1276 1273 1275
+f 1277 1278 1279
+f 1280 1279 1278
+f 1279 1280 1281
+f 1282 1278 1277
+f 1278 1282 1280
+f 1281 1280 1282
+f 1279 1282 1277
+f 1282 1279 1281
diff --git a/data/lego/lego.urdf b/data/lego/lego.urdf
new file mode 100644
index 0000000..634a5e6
--- /dev/null
+++ b/data/lego/lego.urdf
@@ -0,0 +1,32 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="legobrick">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <rolling_friction value="0.0"/>
+ <contact_cfm value="0.0"/>
+ <contact_erp value="1.0"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+ <mass value=".1"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="1.570796 0 0" xyz="-0.016 -0.016 -0.0115"/>
+ <geometry>
+ <mesh filename="lego.obj" scale=".1 .1 .1"/>
+ </geometry>
+ <material name="yellow">
+ <color rgba="1 1 0.4 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="1.570796 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.032 0.023 0.032"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/lego/lego_vhacd.obj b/data/lego/lego_vhacd.obj
new file mode 100644
index 0000000..b136bb5
--- /dev/null
+++ b/data/lego/lego_vhacd.obj
@@ -0,0 +1,3072 @@
+o convex_0
+v -0.001024 0.146464 -0.318524
+v 0.054282 0.193568 -0.160829
+v -0.001024 0.193568 -0.160829
+v 0.054282 0.193568 -0.318524
+v 0.011270 0.146464 -0.160829
+v 0.054282 0.146464 -0.318524
+v -0.001024 0.193568 -0.318524
+v 0.054282 0.177181 -0.160829
+v -0.001024 0.146464 -0.160829
+v 0.054282 0.146464 -0.306205
+f 6 8 10
+f 3 2 4
+f 2 3 5
+f 4 2 6
+f 1 4 6
+f 5 1 6
+f 1 3 7
+f 3 4 7
+f 4 1 7
+f 2 5 8
+f 6 2 8
+f 3 1 9
+f 1 5 9
+f 5 3 9
+f 5 6 10
+f 8 5 10
+o convex_1
+v 0.304179 0.148508 -0.160783
+v 0.318523 0.193568 0.001024
+v 0.261169 0.193568 0.001024
+v 0.318523 0.148508 0.001024
+v 0.318523 0.193568 -0.160783
+v 0.261169 0.177182 -0.160783
+v 0.261169 0.148508 0.001024
+v 0.261169 0.193568 -0.160783
+v 0.318523 0.148508 -0.160783
+v 0.261169 0.148508 -0.013341
+f 17 16 20
+f 12 13 14
+f 13 12 15
+f 12 14 15
+f 15 11 16
+f 14 13 17
+f 11 14 17
+f 13 16 17
+f 13 15 18
+f 16 13 18
+f 15 16 18
+f 14 11 19
+f 11 15 19
+f 15 14 19
+f 16 11 20
+f 11 17 20
+o convex_2
+v 0.084994 0.230440 -0.029703
+v 0.031750 0.193576 -0.068621
+v 0.031750 0.193576 -0.062472
+v 0.125962 0.193576 -0.068621
+v 0.031750 0.230440 -0.068621
+v 0.125962 0.230440 -0.068621
+v 0.095232 0.193576 -0.031753
+v 0.052253 0.193576 -0.035852
+v 0.042006 0.230440 -0.044046
+v 0.115725 0.230440 -0.044042
+v 0.121867 0.193576 -0.052232
+v 0.062490 0.230440 -0.031753
+v 0.072728 0.193576 -0.029703
+v 0.105478 0.193576 -0.035852
+v 0.105478 0.230440 -0.035852
+v 0.042006 0.193576 -0.044046
+v 0.033807 0.230440 -0.056328
+v 0.123915 0.230440 -0.056328
+f 31 24 38
+f 23 22 24
+f 22 23 25
+f 24 22 25
+f 25 21 26
+f 24 25 26
+f 23 24 27
+f 23 27 28
+f 21 25 29
+f 26 21 30
+f 27 24 31
+f 21 29 32
+f 29 28 32
+f 27 21 33
+f 28 27 33
+f 21 32 33
+f 32 28 33
+f 27 31 34
+f 31 30 34
+f 21 27 35
+f 30 21 35
+f 27 34 35
+f 34 30 35
+f 23 28 36
+f 28 29 36
+f 36 29 37
+f 25 23 37
+f 29 25 37
+f 23 36 37
+f 24 26 38
+f 26 30 38
+f 30 31 38
+o convex_3
+v 0.244764 0.230440 -0.029703
+v 0.191551 0.193576 -0.068621
+v 0.191551 0.193576 -0.062472
+v 0.285742 0.193576 -0.068621
+v 0.191551 0.230440 -0.068621
+v 0.285742 0.230440 -0.068621
+v 0.255009 0.193576 -0.031753
+v 0.212021 0.193576 -0.035852
+v 0.201777 0.230440 -0.044046
+v 0.275497 0.230440 -0.044042
+v 0.281648 0.193576 -0.052232
+v 0.222266 0.230440 -0.031753
+v 0.232501 0.193576 -0.029703
+v 0.265244 0.193576 -0.035852
+v 0.265244 0.230440 -0.035852
+v 0.195636 0.193576 -0.052232
+v 0.193589 0.230440 -0.056328
+v 0.283695 0.230440 -0.056328
+f 49 42 56
+f 41 40 42
+f 40 41 43
+f 42 40 43
+f 43 39 44
+f 42 43 44
+f 41 42 45
+f 41 45 46
+f 39 43 47
+f 44 39 48
+f 45 42 49
+f 39 47 50
+f 47 46 50
+f 45 39 51
+f 46 45 51
+f 39 50 51
+f 50 46 51
+f 45 49 52
+f 49 48 52
+f 39 45 53
+f 48 39 53
+f 45 52 53
+f 52 48 53
+f 41 46 54
+f 46 47 54
+f 54 47 55
+f 43 41 55
+f 47 43 55
+f 41 54 55
+f 42 44 56
+f 44 48 56
+f 48 49 56
+o convex_4
+v 0.041996 0.193576 -0.273455
+v 0.074766 0.230440 -0.269356
+v 0.050191 0.230440 -0.228401
+v 0.035855 0.230440 -0.265257
+v 0.050191 0.193576 -0.228401
+v 0.074766 0.193576 -0.287793
+v 0.029702 0.193576 -0.228401
+v 0.060425 0.230440 -0.285740
+v 0.029702 0.230440 -0.228401
+v 0.074766 0.193576 -0.269356
+v 0.029702 0.193576 -0.248884
+v 0.074766 0.230440 -0.287793
+v 0.060425 0.193576 -0.285740
+v 0.029702 0.230440 -0.248884
+v 0.044042 0.230440 -0.275501
+v 0.031753 0.193576 -0.257076
+f 67 70 72
+f 59 58 60
+f 58 59 61
+f 61 57 62
+f 57 61 63
+f 61 59 63
+f 60 58 64
+f 59 60 65
+f 63 59 65
+f 58 61 66
+f 62 58 66
+f 61 62 66
+f 57 63 67
+f 63 65 67
+f 58 62 68
+f 64 58 68
+f 62 64 68
+f 62 57 69
+f 64 62 69
+f 65 60 70
+f 67 65 70
+f 57 60 71
+f 60 64 71
+f 69 57 71
+f 64 69 71
+f 60 57 72
+f 57 67 72
+f 70 60 72
+o convex_5
+v 0.113677 0.193576 -0.275501
+v 0.128024 0.230440 -0.228401
+v 0.128024 0.193576 -0.228401
+v 0.074766 0.230440 -0.269356
+v 0.117780 0.230440 -0.271402
+v 0.074766 0.193576 -0.287793
+v 0.107531 0.193576 -0.228401
+v 0.089113 0.230440 -0.287793
+v 0.107531 0.230440 -0.228401
+v 0.074766 0.193576 -0.269356
+v 0.125970 0.193576 -0.257076
+v 0.074766 0.230440 -0.287793
+v 0.128024 0.230440 -0.248884
+v 0.097303 0.193576 -0.285740
+v 0.105488 0.230440 -0.281647
+f 73 86 87
+f 76 74 77
+f 73 75 78
+f 75 74 79
+f 78 75 79
+f 76 77 80
+f 74 76 81
+f 79 74 81
+f 76 79 81
+f 76 78 82
+f 79 76 82
+f 78 79 82
+f 75 73 83
+f 73 77 83
+f 78 76 84
+f 76 80 84
+f 80 78 84
+f 74 75 85
+f 77 74 85
+f 75 83 85
+f 83 77 85
+f 73 78 86
+f 78 80 86
+f 86 80 87
+f 77 73 87
+f 80 77 87
+o convex_6
+v 0.056328 0.230440 -0.193578
+v 0.031750 0.193576 -0.228391
+v 0.031750 0.193576 -0.220198
+v 0.072718 0.193576 -0.189476
+v 0.050189 0.230440 -0.228391
+v 0.072718 0.193576 -0.207916
+v 0.072718 0.230440 -0.207916
+v 0.031750 0.230440 -0.220198
+v 0.046090 0.193576 -0.199723
+v 0.050189 0.193576 -0.228391
+v 0.072718 0.230440 -0.189476
+v 0.039947 0.230440 -0.205867
+v 0.031750 0.230440 -0.228391
+v 0.066570 0.193576 -0.189476
+f 91 98 101
+f 90 89 91
+f 91 89 93
+f 92 88 94
+f 93 92 94
+f 91 93 94
+f 89 90 95
+f 88 92 95
+f 90 91 96
+f 89 92 97
+f 93 89 97
+f 92 93 97
+f 94 88 98
+f 91 94 98
+f 88 95 99
+f 95 90 99
+f 90 96 99
+f 96 88 99
+f 92 89 100
+f 89 95 100
+f 95 92 100
+f 96 91 101
+f 88 96 101
+f 98 88 101
+o convex_7
+v 0.115732 0.230440 -0.203818
+v 0.107537 0.193576 -0.228391
+v 0.072718 0.193576 -0.207916
+v 0.105483 0.193576 -0.195628
+v 0.072718 0.230440 -0.207916
+v 0.072718 0.230440 -0.189476
+v 0.125976 0.230440 -0.228391
+v 0.125976 0.193576 -0.220198
+v 0.072718 0.193576 -0.189476
+v 0.091157 0.230440 -0.189476
+v 0.107537 0.230440 -0.228391
+v 0.125976 0.193576 -0.228391
+v 0.091157 0.193576 -0.189476
+v 0.117775 0.193576 -0.205867
+v 0.125976 0.230440 -0.220198
+f 102 115 116
+f 104 103 105
+f 103 104 106
+f 106 104 107
+f 102 106 107
+f 106 102 108
+f 105 103 109
+f 104 105 110
+f 107 104 110
+f 107 110 111
+f 105 102 111
+f 102 107 111
+f 103 106 112
+f 108 103 112
+f 106 108 112
+f 103 108 113
+f 109 103 113
+f 108 109 113
+f 110 105 114
+f 111 110 114
+f 105 111 114
+f 102 105 115
+f 105 109 115
+f 115 109 116
+f 108 102 116
+f 109 108 116
+o convex_8
+v 0.199724 0.193576 -0.271402
+v 0.232492 0.230440 -0.269356
+v 0.209963 0.230440 -0.228401
+v 0.195627 0.230440 -0.265257
+v 0.209963 0.193576 -0.228401
+v 0.232492 0.193576 -0.287793
+v 0.189484 0.193576 -0.228401
+v 0.220198 0.230440 -0.285740
+v 0.189484 0.230440 -0.228401
+v 0.232492 0.193576 -0.269356
+v 0.189484 0.193576 -0.248884
+v 0.232492 0.230440 -0.287793
+v 0.212005 0.193576 -0.281647
+v 0.189484 0.230440 -0.248884
+v 0.203820 0.230440 -0.275501
+f 124 129 131
+f 119 118 120
+f 118 119 121
+f 121 117 122
+f 117 121 123
+f 121 119 123
+f 120 118 124
+f 119 120 125
+f 123 119 125
+f 118 121 126
+f 122 118 126
+f 121 122 126
+f 120 117 127
+f 117 123 127
+f 123 125 127
+f 118 122 128
+f 124 118 128
+f 122 124 128
+f 122 117 129
+f 124 122 129
+f 125 120 130
+f 120 127 130
+f 127 125 130
+f 117 120 131
+f 120 124 131
+f 129 117 131
+o convex_9
+v 0.216100 0.230440 -0.193578
+v 0.191532 0.193576 -0.228391
+v 0.191532 0.193576 -0.220198
+v 0.232492 0.193576 -0.207916
+v 0.209965 0.230440 -0.228391
+v 0.191532 0.230440 -0.220198
+v 0.226341 0.193576 -0.189476
+v 0.232492 0.230440 -0.189476
+v 0.232492 0.230440 -0.207916
+v 0.205867 0.193576 -0.199723
+v 0.209965 0.193576 -0.228391
+v 0.199724 0.230440 -0.205867
+v 0.191532 0.230440 -0.228391
+v 0.232492 0.193576 -0.189476
+f 139 138 145
+f 134 133 135
+f 133 134 137
+f 132 136 137
+f 134 135 138
+f 136 132 139
+f 132 138 139
+f 136 139 140
+f 135 136 140
+f 139 135 140
+f 138 132 141
+f 134 138 141
+f 135 133 142
+f 136 135 142
+f 133 136 142
+f 132 137 143
+f 137 134 143
+f 141 132 143
+f 134 141 143
+f 136 133 144
+f 133 137 144
+f 137 136 144
+f 138 135 145
+f 135 139 145
+o convex_10
+v 0.275502 0.230440 -0.203818
+v 0.267308 0.193576 -0.228391
+v 0.232492 0.193576 -0.207916
+v 0.265259 0.193576 -0.195628
+v 0.232492 0.230440 -0.207916
+v 0.232492 0.230440 -0.189476
+v 0.285740 0.230440 -0.228391
+v 0.285740 0.193576 -0.220198
+v 0.232492 0.193576 -0.189476
+v 0.250934 0.230440 -0.189476
+v 0.267308 0.230440 -0.228391
+v 0.285740 0.193576 -0.228391
+v 0.250934 0.193576 -0.189476
+v 0.277550 0.193576 -0.205867
+v 0.285740 0.230440 -0.220198
+f 146 159 160
+f 148 147 149
+f 147 148 150
+f 150 148 151
+f 146 150 151
+f 150 146 152
+f 149 147 153
+f 148 149 154
+f 151 148 154
+f 151 154 155
+f 149 146 155
+f 146 151 155
+f 147 150 156
+f 152 147 156
+f 150 152 156
+f 147 152 157
+f 153 147 157
+f 152 153 157
+f 154 149 158
+f 155 154 158
+f 149 155 158
+f 146 149 159
+f 149 153 159
+f 159 153 160
+f 152 146 160
+f 153 152 160
+o convex_11
+v 0.025611 0.146460 -0.300092
+v -0.001024 -0.001010 -0.318524
+v 0.054282 -0.001010 -0.318524
+v -0.001024 -0.001010 -0.300092
+v -0.001024 0.146460 -0.318524
+v 0.054282 0.146460 -0.318524
+v 0.054282 -0.001010 -0.302140
+v -0.001024 0.146460 -0.300092
+v 0.054282 0.146460 -0.306236
+v 0.054282 0.058397 -0.302140
+v 0.015363 -0.001010 -0.300092
+f 167 161 171
+f 162 163 164
+f 163 162 165
+f 162 164 165
+f 165 161 166
+f 163 165 166
+f 164 163 167
+f 163 166 167
+f 164 161 168
+f 161 165 168
+f 165 164 168
+f 166 161 169
+f 167 166 169
+f 167 169 170
+f 161 167 170
+f 169 161 170
+f 161 164 171
+f 164 167 171
+o convex_12
+v 0.015361 -0.001010 -0.271390
+v 0.011265 0.146460 -0.160809
+v -0.001024 0.146460 -0.160809
+v 0.015361 -0.001010 -0.160809
+v -0.001024 -0.001010 -0.160809
+v -0.001024 0.146460 -0.271390
+v -0.001024 -0.001010 -0.271390
+v 0.011265 0.146460 -0.271390
+v 0.017411 0.054282 -0.259103
+v 0.015361 0.058382 -0.160809
+v 0.017411 -0.001010 -0.218151
+v 0.017411 0.054282 -0.218151
+v 0.017411 -0.001010 -0.259103
+v 0.015361 0.058382 -0.271390
+f 179 180 185
+f 173 174 175
+f 172 175 176
+f 175 174 176
+f 176 174 177
+f 174 173 177
+f 177 172 178
+f 172 176 178
+f 176 177 178
+f 177 173 179
+f 172 177 179
+f 179 173 180
+f 173 175 181
+f 175 172 182
+f 181 175 182
+f 180 173 183
+f 173 181 183
+f 182 180 183
+f 181 182 183
+f 172 180 184
+f 180 182 184
+f 182 172 184
+f 172 179 185
+f 180 172 185
+o convex_13
+v 0.031747 -0.001024 -0.250921
+v 0.021509 0.054282 -0.218157
+v 0.017413 0.054282 -0.218157
+v 0.017413 0.054282 -0.259121
+v 0.017413 -0.001024 -0.218157
+v 0.033797 0.054282 -0.232496
+v 0.017413 -0.001024 -0.259121
+v 0.031747 -0.001024 -0.226353
+v 0.031747 0.054282 -0.250921
+v 0.033797 -0.001024 -0.244778
+v 0.025604 -0.001024 -0.257068
+v 0.031747 0.054282 -0.226353
+v 0.025604 -0.001024 -0.220206
+v 0.021509 0.054282 -0.259121
+f 194 196 199
+f 188 187 189
+f 187 188 190
+f 188 189 190
+f 189 187 191
+f 186 190 192
+f 190 189 192
+f 190 186 193
+f 189 191 194
+f 194 191 195
+f 193 186 195
+f 191 193 195
+f 186 194 195
+f 186 192 196
+f 194 186 196
+f 191 187 197
+f 193 191 197
+f 193 197 198
+f 187 190 198
+f 190 193 198
+f 197 187 198
+f 192 189 199
+f 189 194 199
+f 196 192 199
+o convex_14
+v 0.054282 0.128024 -0.318521
+v 0.273452 0.193560 -0.281657
+v 0.273452 0.177174 -0.281657
+v 0.273452 0.193560 -0.318521
+v 0.054282 0.193560 -0.281657
+v 0.273452 0.128024 -0.318521
+v 0.054282 0.193560 -0.318521
+v 0.054282 0.128024 -0.306231
+v 0.273452 0.128024 -0.306231
+v 0.054282 0.177174 -0.281657
+f 207 202 209
+f 201 202 203
+f 202 201 204
+f 201 203 204
+f 203 202 205
+f 200 203 205
+f 203 200 206
+f 200 204 206
+f 204 203 206
+f 204 200 207
+f 200 205 207
+f 207 205 208
+f 205 202 208
+f 202 207 208
+f 202 204 209
+f 204 207 209
+o convex_15
+v 0.281659 0.130084 -0.281657
+v 0.273464 0.128024 -0.318521
+v 0.318524 0.128024 -0.318521
+v 0.273464 0.193560 -0.318521
+v 0.318524 0.193560 -0.281657
+v 0.273464 0.193560 -0.281657
+v 0.318524 0.193560 -0.318521
+v 0.318524 0.128024 -0.281657
+v 0.273464 0.142370 -0.281657
+v 0.279613 0.128024 -0.285753
+f 210 218 219
+f 212 211 213
+f 213 211 215
+f 210 214 215
+f 214 213 215
+f 212 213 216
+f 213 214 216
+f 214 212 216
+f 211 212 217
+f 214 210 217
+f 212 214 217
+f 215 211 218
+f 210 215 218
+f 211 217 219
+f 217 210 219
+f 218 211 219
+o convex_16
+v 0.265263 0.230440 -0.281649
+v 0.232492 0.193576 -0.287795
+v 0.232492 0.193576 -0.269366
+v 0.232492 0.230440 -0.287795
+v 0.263213 0.230440 -0.255024
+v 0.265263 0.193576 -0.255024
+v 0.265263 0.193576 -0.281649
+v 0.232492 0.230440 -0.269366
+v 0.248877 0.193576 -0.287795
+v 0.248877 0.230440 -0.287795
+f 220 228 229
+f 221 222 223
+f 220 223 224
+f 224 222 225
+f 222 221 225
+f 220 224 225
+f 220 225 226
+f 225 221 226
+f 223 222 227
+f 224 223 227
+f 222 224 227
+f 221 223 228
+f 220 226 228
+f 226 221 228
+f 223 220 229
+f 228 223 229
+o convex_17
+v 0.285747 0.230440 -0.257065
+v 0.265266 0.193576 -0.279595
+v 0.265266 0.230440 -0.279595
+v 0.267317 0.230440 -0.228395
+v 0.287796 0.193576 -0.228395
+v 0.267317 0.193576 -0.228395
+v 0.281650 0.193576 -0.265260
+v 0.287796 0.230440 -0.228395
+v 0.287796 0.193576 -0.248885
+v 0.275510 0.230440 -0.273455
+v 0.265266 0.193576 -0.250935
+f 235 233 240
+f 230 232 233
+f 231 234 235
+f 234 233 235
+f 234 231 236
+f 230 233 237
+f 233 234 237
+f 237 234 238
+f 236 230 238
+f 234 236 238
+f 230 237 238
+f 232 230 239
+f 231 232 239
+f 230 236 239
+f 236 231 239
+f 232 231 240
+f 233 232 240
+f 231 235 240
+o convex_18
+v 0.044040 0.146473 -0.035847
+v -0.001024 0.119838 -0.035847
+v 0.011270 0.119838 -0.035847
+v 0.044040 0.119838 0.001024
+v -0.001024 0.193573 0.001024
+v 0.044040 0.193573 -0.035847
+v -0.001024 0.119838 0.001024
+v -0.001024 0.193573 -0.035847
+v 0.044040 0.193573 0.001024
+v 0.033798 0.119838 -0.027650
+v 0.044040 0.119838 -0.011269
+v 0.029695 0.123931 -0.035847
+f 250 243 252
+f 243 242 241
+f 242 243 244
+f 241 242 246
+f 244 241 246
+f 242 244 247
+f 244 245 247
+f 245 242 247
+f 242 245 248
+f 246 242 248
+f 245 246 248
+f 245 244 249
+f 246 245 249
+f 244 246 249
+f 244 243 250
+f 244 250 251
+f 241 244 251
+f 250 241 251
+f 243 241 252
+f 241 250 252
+o convex_19
+v 0.044053 0.177173 -0.035847
+v 0.261169 0.193573 0.001024
+v 0.261169 0.119838 0.001024
+v 0.261169 0.193573 -0.035847
+v 0.044053 0.193573 0.001024
+v 0.044053 0.119838 -0.011269
+v 0.261169 0.119838 -0.011269
+v 0.044053 0.119838 0.001024
+v 0.044053 0.193573 -0.035847
+v 0.261169 0.177173 -0.035847
+f 259 253 262
+f 254 255 256
+f 255 254 257
+f 254 256 257
+f 257 253 258
+f 256 255 259
+f 255 258 259
+f 258 253 259
+f 255 257 260
+f 258 255 260
+f 257 258 260
+f 256 253 261
+f 253 257 261
+f 257 256 261
+f 253 256 262
+f 256 259 262
+o convex_20
+v 0.287802 0.054274 -0.062482
+v 0.300092 -0.001024 -0.160798
+v 0.318524 -0.001024 -0.160798
+v 0.318524 -0.001024 -0.046089
+v 0.318524 0.060421 -0.160798
+v 0.318524 0.060421 -0.046089
+v 0.283705 -0.001024 -0.070679
+v 0.300092 0.058376 -0.160798
+v 0.283705 0.054274 -0.087061
+v 0.300092 -0.001024 -0.046089
+v 0.306230 0.060421 -0.046089
+v 0.283705 -0.001024 -0.087061
+v 0.300092 0.058376 -0.046089
+v 0.283705 0.054274 -0.070679
+v 0.306230 0.060421 -0.160798
+f 271 273 277
+f 264 265 266
+f 265 264 267
+f 266 265 267
+f 266 267 268
+f 264 266 269
+f 267 264 270
+f 270 264 271
+f 266 268 272
+f 263 269 272
+f 269 266 272
+f 268 267 273
+f 272 268 273
+f 264 269 274
+f 271 264 274
+f 269 271 274
+f 263 272 275
+f 272 273 275
+f 275 273 276
+f 269 263 276
+f 271 269 276
+f 273 271 276
+f 263 275 276
+f 267 270 277
+f 270 271 277
+f 273 267 277
+o convex_21
+v 0.318523 0.148508 -0.046089
+v 0.306234 0.060436 -0.160798
+v 0.318523 0.060436 -0.160798
+v 0.306234 0.060436 -0.046089
+v 0.306234 0.148508 -0.160798
+v 0.318523 0.148508 -0.160798
+v 0.318523 0.060436 -0.046089
+v 0.306234 0.148508 -0.046089
+f 282 281 285
+f 279 280 281
+f 280 279 282
+f 279 281 282
+f 278 280 283
+f 282 278 283
+f 280 282 283
+f 280 278 284
+f 281 280 284
+f 278 281 284
+f 281 278 285
+f 278 282 285
+o convex_22
+v 0.318519 0.148508 -0.017411
+v 0.261169 -0.001024 -0.015359
+v 0.261169 -0.001024 0.001024
+v 0.318519 -0.001024 0.001024
+v 0.261169 0.148508 0.001024
+v 0.318519 -0.001024 -0.017411
+v 0.318519 0.148508 0.001024
+v 0.261169 0.148508 -0.011263
+v 0.291887 0.148508 -0.017411
+v 0.261169 0.058402 -0.015359
+v 0.300083 -0.001024 -0.017411
+f 294 291 296
+f 288 287 289
+f 287 288 290
+f 288 289 290
+f 289 287 291
+f 286 289 291
+f 289 286 292
+f 286 290 292
+f 290 289 292
+f 290 286 293
+f 287 290 293
+f 286 291 294
+f 293 286 294
+f 293 294 295
+f 287 293 295
+f 294 287 295
+f 291 287 296
+f 287 294 296
+o convex_23
+v 0.037897 0.193576 -0.109586
+v 0.080906 0.230440 -0.109586
+v 0.050188 0.230440 -0.068621
+v 0.080906 0.193576 -0.109586
+v 0.054289 0.230440 -0.123919
+v 0.029702 0.193576 -0.068621
+v 0.029702 0.230440 -0.091159
+v 0.080906 0.193576 -0.128024
+v 0.050188 0.193576 -0.068621
+v 0.080906 0.230440 -0.128024
+v 0.029702 0.230440 -0.068621
+v 0.064515 0.193576 -0.128024
+v 0.029702 0.193576 -0.091159
+v 0.037897 0.230440 -0.109586
+v 0.048138 0.193576 -0.119825
+v 0.064515 0.230440 -0.128024
+f 306 308 312
+f 298 299 300
+f 299 298 301
+f 297 300 302
+f 299 301 303
+f 298 300 304
+f 300 297 304
+f 300 299 305
+f 299 302 305
+f 302 300 305
+f 301 298 306
+f 298 304 306
+f 302 299 307
+f 299 303 307
+f 303 302 307
+f 304 297 308
+f 306 304 308
+f 297 302 309
+f 303 297 309
+f 302 303 309
+f 297 303 310
+f 303 301 310
+f 308 297 311
+f 301 308 311
+f 297 310 311
+f 310 301 311
+f 301 306 312
+f 308 301 312
+o convex_24
+v 0.103446 0.193576 -0.123919
+v 0.128020 0.230440 -0.068621
+v 0.128020 0.193576 -0.068621
+v 0.080911 0.230440 -0.109586
+v 0.119825 0.230440 -0.109586
+v 0.107537 0.193576 -0.068621
+v 0.080911 0.193576 -0.128024
+v 0.093209 0.230440 -0.128024
+v 0.107537 0.230440 -0.068621
+v 0.125968 0.193576 -0.097305
+v 0.080911 0.193576 -0.109586
+v 0.128020 0.230440 -0.091159
+v 0.080911 0.230440 -0.128024
+v 0.119825 0.193576 -0.109586
+v 0.109584 0.230440 -0.119825
+f 317 326 327
+f 316 314 317
+f 313 315 318
+f 315 314 318
+f 313 318 319
+f 316 317 320
+f 313 319 320
+f 314 316 321
+f 318 314 321
+f 316 318 321
+f 315 313 322
+f 318 316 323
+f 316 319 323
+f 319 318 323
+f 314 315 324
+f 317 314 324
+f 315 322 324
+f 322 317 324
+f 319 316 325
+f 316 320 325
+f 320 319 325
+f 322 313 326
+f 317 322 326
+f 313 320 327
+f 320 317 327
+f 326 313 327
+o convex_25
+v 0.117777 0.058382 -0.103447
+v 0.089105 0.111629 -0.099351
+v 0.089105 0.111629 -0.097303
+v 0.119828 0.117782 -0.117777
+v 0.103447 0.058382 -0.119828
+v 0.099351 0.117782 -0.089105
+v 0.103447 0.117782 -0.119828
+v 0.119828 0.056331 -0.119828
+v 0.119828 0.117782 -0.105489
+v 0.107537 0.064529 -0.103447
+v 0.093207 0.117782 -0.089105
+f 330 337 338
+f 330 329 332
+f 333 331 334
+f 332 329 334
+f 328 332 335
+f 334 331 335
+f 332 334 335
+f 333 328 336
+f 331 333 336
+f 328 335 336
+f 335 331 336
+f 332 328 337
+f 330 332 337
+f 329 330 338
+f 328 333 338
+f 333 334 338
+f 334 329 338
+f 337 328 338
+o convex_26
+v 0.197669 0.175135 -0.199720
+v 0.144413 0.156703 -0.224298
+v 0.144413 0.175135 -0.224298
+v 0.123927 0.175135 -0.203817
+v 0.195612 0.156703 -0.199720
+v 0.185377 0.156703 -0.220199
+v 0.123927 0.156703 -0.203817
+v 0.193569 0.175135 -0.216102
+v 0.123927 0.156703 -0.216102
+v 0.173084 0.175135 -0.224298
+v 0.125985 0.175135 -0.218151
+v 0.197669 0.156703 -0.212009
+f 344 346 350
+f 339 341 342
+f 339 342 343
+f 343 340 344
+f 343 342 345
+f 340 343 345
+f 341 339 346
+f 340 345 347
+f 345 342 347
+f 340 341 348
+f 344 340 348
+f 341 346 348
+f 346 344 348
+f 341 340 349
+f 342 341 349
+f 340 347 349
+f 347 342 349
+f 339 343 350
+f 343 344 350
+f 346 339 350
+o convex_27
+v 0.197676 0.193576 -0.109586
+v 0.240680 0.230440 -0.109586
+v 0.209966 0.230440 -0.068621
+v 0.240680 0.193576 -0.109586
+v 0.214061 0.230440 -0.123919
+v 0.189481 0.193576 -0.068621
+v 0.189481 0.230440 -0.091159
+v 0.240680 0.193576 -0.128024
+v 0.209966 0.193576 -0.068621
+v 0.240680 0.230440 -0.128024
+v 0.189481 0.230440 -0.068621
+v 0.224291 0.193576 -0.128024
+v 0.189481 0.193576 -0.091159
+v 0.197676 0.230440 -0.109586
+v 0.207916 0.193576 -0.119825
+v 0.224291 0.230440 -0.128024
+f 360 362 366
+f 352 353 354
+f 353 352 355
+f 351 354 356
+f 353 355 357
+f 352 354 358
+f 354 351 358
+f 354 353 359
+f 353 356 359
+f 356 354 359
+f 355 352 360
+f 352 358 360
+f 356 353 361
+f 353 357 361
+f 357 356 361
+f 358 351 362
+f 360 358 362
+f 351 356 363
+f 357 351 363
+f 356 357 363
+f 351 357 364
+f 357 355 364
+f 362 351 365
+f 355 362 365
+f 351 364 365
+f 364 355 365
+f 355 360 366
+f 362 355 366
+o convex_28
+v 0.263221 0.193576 -0.123919
+v 0.287789 0.230440 -0.068621
+v 0.287789 0.193576 -0.068621
+v 0.240685 0.230440 -0.109586
+v 0.279600 0.230440 -0.109586
+v 0.267311 0.193576 -0.068621
+v 0.240685 0.193576 -0.128024
+v 0.252984 0.230440 -0.128024
+v 0.267311 0.230440 -0.068621
+v 0.285742 0.193576 -0.097305
+v 0.240685 0.193576 -0.109586
+v 0.287789 0.230440 -0.091159
+v 0.240685 0.230440 -0.128024
+v 0.279600 0.193576 -0.109586
+v 0.269358 0.230440 -0.119825
+f 371 380 381
+f 370 368 371
+f 367 369 372
+f 369 368 372
+f 367 372 373
+f 370 371 374
+f 367 373 374
+f 368 370 375
+f 372 368 375
+f 370 372 375
+f 369 367 376
+f 372 370 377
+f 370 373 377
+f 373 372 377
+f 368 369 378
+f 371 368 378
+f 369 376 378
+f 376 371 378
+f 373 370 379
+f 370 374 379
+f 374 373 379
+f 376 367 380
+f 371 376 380
+f 367 374 381
+f 374 371 381
+f 380 367 381
+o convex_29
+v 0.191518 0.117782 -0.099351
+v 0.156702 0.070673 -0.107539
+v 0.193568 0.054282 -0.121876
+v 0.175142 0.054282 -0.093202
+v 0.156702 0.117782 -0.101398
+v 0.193568 0.117782 -0.115729
+v 0.195617 0.054282 -0.103447
+v 0.156702 0.117782 -0.093202
+v 0.156702 0.070673 -0.093202
+v 0.193568 0.085019 -0.121876
+v 0.166949 0.054282 -0.107539
+v 0.175142 0.117782 -0.093202
+v 0.195617 0.117782 -0.115729
+v 0.156702 0.085019 -0.107539
+v 0.195617 0.117782 -0.103447
+v 0.195617 0.054282 -0.121876
+v 0.191518 0.054282 -0.099351
+f 388 382 398
+f 386 382 387
+f 385 384 388
+f 382 386 389
+f 386 383 389
+f 389 383 390
+f 385 389 390
+f 384 383 391
+f 386 387 391
+f 383 384 392
+f 384 385 392
+f 390 383 392
+f 385 390 392
+f 385 382 393
+f 389 385 393
+f 382 389 393
+f 387 382 394
+f 391 387 394
+f 383 386 395
+f 391 383 395
+f 386 391 395
+f 382 388 396
+f 394 382 396
+f 388 394 396
+f 388 384 397
+f 384 391 397
+f 394 388 397
+f 391 394 397
+f 382 385 398
+f 385 388 398
+o convex_30
+v 0.156690 0.117778 -0.093202
+v 0.113689 0.117778 -0.115731
+v 0.119831 0.056331 -0.119828
+v 0.121886 0.056331 -0.101401
+v 0.156690 0.070680 -0.107537
+v 0.123931 0.154646 -0.115731
+v 0.113689 0.154646 -0.103447
+v 0.154649 0.154646 -0.101399
+v 0.140307 0.056331 -0.093202
+v 0.140307 0.154646 -0.093202
+v 0.156690 0.070680 -0.093202
+v 0.125981 0.085019 -0.119828
+v 0.150548 0.056331 -0.107537
+v 0.113689 0.154646 -0.115731
+v 0.113689 0.117778 -0.103447
+v 0.156690 0.085019 -0.107537
+v 0.154649 0.154646 -0.093202
+v 0.125981 0.056331 -0.119828
+v 0.152599 0.154646 -0.103447
+f 414 404 417
+f 404 405 406
+f 402 401 407
+f 405 407 408
+f 406 405 408
+f 407 399 408
+f 403 399 409
+f 399 407 409
+f 407 401 411
+f 403 409 411
+f 409 407 411
+f 401 400 412
+f 405 404 412
+f 400 405 412
+f 410 401 412
+f 404 410 412
+f 400 401 413
+f 401 402 413
+f 405 400 413
+f 402 407 413
+f 407 405 413
+f 399 403 414
+f 406 399 414
+f 403 410 414
+f 410 404 414
+f 399 406 415
+f 408 399 415
+f 406 408 415
+f 401 410 416
+f 410 403 416
+f 411 401 416
+f 403 411 416
+f 404 406 417
+f 406 414 417
+o convex_31
+v 0.015361 -0.001010 -0.300086
+v 0.011265 0.146460 -0.271411
+v 0.017411 0.146460 -0.291893
+v -0.001024 0.146460 -0.300086
+v -0.001024 -0.001010 -0.271411
+v 0.015361 -0.001010 -0.271411
+v -0.001024 -0.001010 -0.300086
+v -0.001024 0.146460 -0.271411
+v 0.017411 0.146460 -0.300086
+v 0.015361 0.058397 -0.271411
+v 0.017411 0.087039 -0.300086
+f 426 420 428
+f 419 420 421
+f 419 422 423
+f 422 418 423
+f 421 418 424
+f 418 422 424
+f 422 421 424
+f 419 421 425
+f 421 422 425
+f 422 419 425
+f 421 420 426
+f 418 421 426
+f 420 419 427
+f 419 423 427
+f 423 420 427
+f 423 418 428
+f 420 423 428
+f 418 426 428
+o convex_32
+v 0.025606 0.097306 -0.300089
+v 0.035842 0.146460 -0.273460
+v 0.033797 0.146460 -0.273460
+v 0.017411 0.146460 -0.300089
+v 0.017411 0.099355 -0.289842
+v 0.041992 0.146460 -0.285755
+v 0.017411 0.146460 -0.289842
+v 0.027654 0.146460 -0.300089
+v 0.017411 0.089110 -0.300089
+v 0.041992 0.142356 -0.279609
+f 429 434 438
+f 431 430 432
+f 430 431 433
+f 432 430 434
+f 431 432 435
+f 432 433 435
+f 433 431 435
+f 429 432 436
+f 434 429 436
+f 432 434 436
+f 432 429 437
+f 429 433 437
+f 433 432 437
+f 433 429 438
+f 430 433 438
+f 434 430 438
+o convex_33
+v 0.054282 -0.001024 -0.318524
+v 0.205863 0.128024 -0.306237
+v 0.205863 0.058384 -0.302139
+v 0.054282 0.128024 -0.306237
+v 0.205863 0.128024 -0.318524
+v 0.205863 -0.001024 -0.318524
+v 0.054282 -0.001024 -0.302139
+v 0.054282 0.128024 -0.318524
+v 0.205863 -0.001024 -0.302139
+v 0.054282 0.058384 -0.302139
+f 442 445 448
+f 441 440 442
+f 440 441 443
+f 442 440 443
+f 439 443 444
+f 443 441 444
+f 442 439 445
+f 439 444 445
+f 439 442 446
+f 443 439 446
+f 442 443 446
+f 444 441 447
+f 441 445 447
+f 445 444 447
+f 441 442 448
+f 445 441 448
+o convex_34
+v 0.085004 0.054282 -0.283705
+v 0.058387 -0.001024 -0.302135
+v 0.058387 -0.001024 -0.295990
+v 0.099347 -0.001024 -0.302135
+v 0.058387 0.054282 -0.302135
+v 0.099347 0.054282 -0.302135
+v 0.091147 -0.001024 -0.285753
+v 0.066579 0.054282 -0.285753
+v 0.072722 -0.001024 -0.283705
+v 0.097294 0.054282 -0.291897
+v 0.099347 -0.001024 -0.295990
+v 0.060432 0.054282 -0.291897
+v 0.066579 -0.001024 -0.285753
+f 460 451 461
+f 451 450 452
+f 450 451 453
+f 452 450 453
+f 453 449 454
+f 452 453 454
+f 451 452 455
+f 449 453 456
+f 455 449 457
+f 451 455 457
+f 449 456 457
+f 454 449 458
+f 449 455 458
+f 458 455 459
+f 452 454 459
+f 455 452 459
+f 454 458 459
+f 453 451 460
+f 456 453 460
+f 456 460 461
+f 451 457 461
+f 457 456 461
+o convex_35
+v 0.300092 0.058376 -0.160810
+v 0.300092 -0.001024 -0.281653
+v 0.318524 -0.001024 -0.281653
+v 0.318524 0.060421 -0.281653
+v 0.318524 -0.001024 -0.160810
+v 0.283705 -0.001024 -0.246817
+v 0.283705 0.054274 -0.246817
+v 0.318524 0.060421 -0.160810
+v 0.300092 -0.001024 -0.160810
+v 0.300092 0.058376 -0.281653
+v 0.283705 -0.001024 -0.230435
+v 0.283705 0.054274 -0.230435
+v 0.306230 0.060421 -0.160810
+v 0.306230 0.060421 -0.281653
+f 468 474 475
+f 464 463 465
+f 463 464 466
+f 464 465 466
+f 463 466 467
+f 463 467 468
+f 462 466 469
+f 466 465 469
+f 466 462 470
+f 467 466 470
+f 465 463 471
+f 463 468 471
+f 468 467 472
+f 470 462 472
+f 467 470 472
+f 472 462 473
+f 468 472 473
+f 462 469 474
+f 469 465 474
+f 473 462 474
+f 468 473 474
+f 465 471 475
+f 471 468 475
+f 474 465 475
+o convex_36
+v 0.318523 0.125976 -0.160810
+v 0.306234 0.060434 -0.281653
+v 0.318523 0.060434 -0.281653
+v 0.306234 0.060434 -0.160810
+v 0.306234 0.125976 -0.281653
+v 0.318523 0.125976 -0.281653
+v 0.318523 0.060434 -0.160810
+v 0.306234 0.125976 -0.160810
+f 480 479 483
+f 477 478 479
+f 478 477 480
+f 477 479 480
+f 476 478 481
+f 480 476 481
+f 478 480 481
+f 478 476 482
+f 479 478 482
+f 476 479 482
+f 479 476 483
+f 476 480 483
+o convex_37
+v 0.289847 0.193573 -0.160810
+v 0.306230 0.125982 -0.281653
+v 0.318521 0.125982 -0.281653
+v 0.289847 0.193573 -0.281653
+v 0.318521 0.193573 -0.281653
+v 0.318521 0.125982 -0.160810
+v 0.318521 0.193573 -0.160810
+v 0.289847 0.171031 -0.160810
+v 0.306230 0.125982 -0.160810
+v 0.289847 0.171031 -0.281653
+f 491 487 493
+f 486 485 487
+f 487 484 488
+f 486 487 488
+f 486 488 489
+f 485 486 489
+f 484 489 490
+f 488 484 490
+f 489 488 490
+f 484 487 491
+f 489 484 491
+f 489 491 492
+f 485 489 492
+f 491 485 492
+f 487 485 493
+f 485 491 493
+o convex_38
+v 0.015361 0.058382 -0.035847
+v 0.015361 -0.001024 -0.099338
+v 0.017411 -0.001024 -0.099338
+v -0.001024 0.121879 -0.160798
+v -0.001024 -0.001012 -0.035847
+v -0.001024 -0.001012 -0.160798
+v -0.001024 0.121879 -0.035847
+v 0.011265 0.121879 -0.160798
+v 0.015361 -0.001012 -0.160798
+v 0.011265 0.121879 -0.035847
+v 0.015361 -0.001012 -0.035847
+v 0.017411 0.054280 -0.099338
+v 0.017411 0.054280 -0.056334
+v 0.015361 0.058382 -0.160798
+v 0.017411 -0.001024 -0.056334
+f 496 506 508
+f 495 498 499
+f 496 495 499
+f 498 497 499
+f 498 494 500
+f 497 498 500
+f 499 497 501
+f 497 500 501
+f 496 499 502
+f 499 501 502
+f 500 494 503
+f 501 500 503
+f 494 498 504
+f 496 502 505
+f 501 503 505
+f 505 503 506
+f 503 494 506
+f 494 504 506
+f 496 505 506
+f 502 501 507
+f 505 502 507
+f 501 505 507
+f 495 496 508
+f 498 495 508
+f 504 498 508
+f 506 504 508
+o convex_39
+v 0.029699 -0.001024 -0.093196
+v 0.023557 0.054282 -0.058379
+v 0.017413 0.054282 -0.058379
+v 0.017413 0.054282 -0.099339
+v 0.017413 -0.001024 -0.058379
+v 0.033797 0.054282 -0.070673
+v 0.017413 -0.001024 -0.099339
+v 0.033797 -0.001024 -0.070673
+v 0.033797 0.054282 -0.085004
+v 0.025604 0.054282 -0.097294
+v 0.023557 -0.001024 -0.058379
+v 0.033797 -0.001024 -0.085004
+f 509 517 520
+f 511 510 512
+f 510 511 513
+f 511 512 513
+f 512 510 514
+f 509 513 515
+f 513 512 515
+f 513 509 516
+f 514 510 516
+f 514 516 517
+f 512 514 517
+f 512 517 518
+f 509 515 518
+f 515 512 518
+f 517 509 518
+f 510 513 519
+f 513 516 519
+f 516 510 519
+f 516 509 520
+f 517 516 520
+o convex_40
+v 0.027653 0.193566 -0.035847
+v -0.001021 0.121886 -0.160798
+v 0.011270 0.121886 -0.160798
+v -0.001021 0.121886 -0.035847
+v -0.001021 0.193566 -0.160798
+v -0.001021 0.193566 -0.035847
+v 0.027653 0.193566 -0.160798
+v 0.027653 0.173083 -0.035847
+v 0.011270 0.121886 -0.035847
+v 0.027653 0.173083 -0.160798
+f 527 528 530
+f 522 523 524
+f 523 522 525
+f 522 524 525
+f 524 521 526
+f 521 525 526
+f 525 524 526
+f 525 521 527
+f 523 525 527
+f 521 524 528
+f 527 521 528
+f 524 523 529
+f 523 528 529
+f 528 524 529
+f 523 527 530
+f 528 523 530
+o convex_41
+v 0.035847 0.119831 -0.011269
+v -0.001024 -0.001024 -0.035847
+v 0.015365 -0.001024 -0.035847
+v 0.035847 -0.001024 0.001024
+v -0.001024 0.119831 0.001024
+v -0.001024 0.119831 -0.035847
+v -0.001024 -0.001024 0.001024
+v 0.033797 0.119831 -0.025604
+v 0.035847 0.119831 0.001024
+v 0.035847 -0.001024 -0.015365
+v 0.025604 0.119831 -0.033797
+v 0.011269 0.119831 -0.035847
+f 536 541 542
+f 532 533 534
+f 532 535 536
+f 533 532 536
+f 535 531 536
+f 532 534 537
+f 534 535 537
+f 535 532 537
+f 536 531 538
+f 534 531 539
+f 531 535 539
+f 535 534 539
+f 534 533 540
+f 531 534 540
+f 533 538 540
+f 538 531 540
+f 538 533 541
+f 536 538 541
+f 533 536 542
+f 541 533 542
+o convex_42
+v 0.093195 -0.001024 -0.029701
+v 0.035860 -0.001024 -0.015365
+v 0.066579 -0.001024 -0.031746
+v 0.103444 0.119831 0.001024
+v 0.103444 -0.001024 0.001024
+v 0.035860 0.119831 0.001024
+v 0.072712 0.054305 -0.033798
+v 0.035860 -0.001024 0.001024
+v 0.035860 0.119831 -0.011267
+v 0.103444 0.119831 -0.011267
+v 0.091144 0.054305 -0.031746
+v 0.103444 -0.001024 -0.015365
+v 0.035860 0.058386 -0.015365
+v 0.084998 -0.001024 -0.033798
+v 0.097291 0.054305 -0.025604
+v 0.066579 0.054305 -0.031746
+v 0.084998 0.054305 -0.033798
+f 553 556 559
+f 543 544 545
+f 544 543 547
+f 547 546 548
+f 544 547 550
+f 548 544 550
+f 547 548 550
+f 544 548 551
+f 548 546 551
+f 549 551 552
+f 546 547 552
+f 551 546 552
+f 547 543 554
+f 552 547 554
+f 545 544 555
+f 544 551 555
+f 543 545 556
+f 545 549 556
+f 553 543 556
+f 543 553 557
+f 553 552 557
+f 552 554 557
+f 554 543 557
+f 549 545 558
+f 551 549 558
+f 545 555 558
+f 555 551 558
+f 549 552 559
+f 552 553 559
+f 556 549 559
+o convex_43
+v 0.218153 0.119831 -0.011265
+v 0.103444 -0.001024 -0.015363
+v 0.103444 -0.001024 0.001024
+v 0.218153 -0.001024 0.001024
+v 0.103444 0.119831 0.001024
+v 0.218153 -0.001024 -0.015363
+v 0.103444 0.119831 -0.011265
+v 0.218153 0.119831 0.001024
+v 0.218153 0.058386 -0.015363
+v 0.103444 0.058386 -0.015363
+f 568 561 569
+f 562 561 563
+f 561 562 564
+f 562 563 564
+f 563 561 565
+f 560 563 565
+f 564 560 566
+f 561 564 566
+f 563 560 567
+f 560 564 567
+f 564 563 567
+f 565 561 568
+f 560 565 568
+f 566 560 568
+f 566 568 569
+f 561 566 569
+o convex_44
+v 0.244779 -0.001024 -0.033798
+v 0.218157 -0.001024 -0.021507
+v 0.218157 -0.001024 0.001024
+v 0.261165 0.119831 0.001024
+v 0.218157 0.119831 -0.011267
+v 0.261165 -0.001024 0.001024
+v 0.250922 0.054305 -0.031746
+v 0.218157 0.119831 0.001024
+v 0.226351 0.054305 -0.031746
+v 0.261165 0.119831 -0.011267
+v 0.259115 -0.001024 -0.021507
+v 0.232498 0.054305 -0.033798
+v 0.226351 -0.001024 -0.031746
+v 0.220208 0.054305 -0.025604
+v 0.257069 0.054305 -0.025604
+v 0.261165 -0.001024 -0.015365
+v 0.250922 -0.001024 -0.031746
+v 0.244779 0.054305 -0.033798
+f 581 579 587
+f 572 571 570
+f 571 572 574
+f 572 570 575
+f 573 572 575
+f 572 573 577
+f 573 574 577
+f 574 572 577
+f 574 573 579
+f 573 575 579
+f 575 570 580
+f 578 574 581
+f 574 579 581
+f 570 571 582
+f 581 570 582
+f 578 581 582
+f 571 574 583
+f 574 578 583
+f 582 571 583
+f 578 582 583
+f 576 579 584
+f 579 580 584
+f 579 575 585
+f 580 579 585
+f 575 580 585
+f 570 576 586
+f 580 570 586
+f 576 584 586
+f 584 580 586
+f 576 570 587
+f 579 576 587
+f 570 581 587
+o convex_45
+v 0.300092 -0.001024 -0.046086
+v 0.318522 0.085008 -0.017411
+v 0.318522 0.085008 -0.046086
+v 0.318522 -0.001024 -0.017411
+v 0.300092 0.058371 -0.017411
+v 0.318522 -0.001024 -0.046086
+v 0.306236 0.085008 -0.046086
+v 0.300092 -0.001024 -0.017411
+v 0.300092 0.058371 -0.046086
+v 0.306236 0.085008 -0.017411
+f 594 592 597
+f 590 589 591
+f 591 589 592
+f 588 590 593
+f 590 591 593
+f 591 588 593
+f 590 588 594
+f 589 590 594
+f 588 591 595
+f 592 588 595
+f 591 592 595
+f 588 592 596
+f 594 588 596
+f 592 594 596
+f 592 589 597
+f 589 594 597
+o convex_46
+v 0.318524 0.148502 -0.017411
+v 0.273464 0.142361 -0.035844
+v 0.273464 0.142361 -0.033795
+v 0.318524 0.085008 -0.046086
+v 0.298036 0.087059 -0.017411
+v 0.318524 0.148502 -0.046086
+v 0.318524 0.085008 -0.017411
+v 0.289850 0.148502 -0.017411
+v 0.306227 0.085008 -0.046086
+v 0.277562 0.148502 -0.044037
+v 0.287803 0.103444 -0.019463
+f 606 602 608
+f 598 601 603
+f 601 598 604
+f 598 602 604
+f 602 598 605
+f 598 603 605
+f 603 601 606
+f 601 604 606
+f 604 602 606
+f 599 600 607
+f 600 605 607
+f 605 603 607
+f 606 599 607
+f 603 606 607
+f 600 599 608
+f 605 600 608
+f 602 605 608
+f 599 606 608
+o convex_47
+v 0.259116 0.054291 -0.300092
+v 0.205863 -0.001024 -0.318524
+v 0.277549 -0.001024 -0.318524
+v 0.205863 0.128024 -0.318524
+v 0.205863 -0.001024 -0.302140
+v 0.277549 0.128024 -0.318524
+v 0.205863 0.128024 -0.306236
+v 0.277549 -0.001024 -0.302140
+v 0.277549 0.128024 -0.306236
+v 0.218164 0.054291 -0.300092
+v 0.259116 -0.001024 -0.300092
+v 0.218164 -0.001024 -0.300092
+v 0.277549 0.058384 -0.302140
+v 0.205863 0.058384 -0.302140
+f 618 615 622
+f 611 610 612
+f 610 611 613
+f 612 610 613
+f 611 612 614
+f 614 612 615
+f 612 613 615
+f 613 611 616
+f 611 614 616
+f 616 614 617
+f 614 615 617
+f 615 609 617
+f 609 615 618
+f 616 609 619
+f 613 616 619
+f 609 618 619
+f 619 618 620
+f 618 613 620
+f 613 619 620
+f 609 616 621
+f 616 617 621
+f 617 609 621
+f 615 613 622
+f 613 618 622
+o convex_48
+v 0.244778 0.054282 -0.283703
+v 0.218157 -0.001024 -0.300089
+v 0.218157 -0.001024 -0.295991
+v 0.259121 -0.001024 -0.300089
+v 0.218157 0.054282 -0.300089
+v 0.259121 0.054282 -0.300089
+v 0.232496 -0.001024 -0.283703
+v 0.226353 0.054282 -0.285753
+v 0.250921 -0.001024 -0.285753
+v 0.257068 0.054282 -0.291896
+v 0.226353 -0.001024 -0.285753
+v 0.220206 0.054282 -0.291896
+v 0.259121 -0.001024 -0.295991
+f 632 631 635
+f 625 624 626
+f 624 625 627
+f 626 624 627
+f 627 623 628
+f 626 627 628
+f 625 626 629
+f 623 627 630
+f 629 623 630
+f 629 626 631
+f 623 629 631
+f 628 623 632
+f 623 631 632
+f 625 629 633
+f 629 630 633
+f 633 630 634
+f 627 625 634
+f 630 627 634
+f 625 633 634
+f 626 628 635
+f 631 626 635
+f 628 632 635
+o convex_49
+v 0.291899 0.128024 -0.300092
+v 0.277564 -0.001024 -0.318524
+v 0.318524 -0.001024 -0.318524
+v 0.277564 0.128024 -0.318524
+v 0.277564 -0.001024 -0.302140
+v 0.318524 0.128024 -0.300092
+v 0.318524 -0.001024 -0.300092
+v 0.318524 0.128024 -0.318524
+v 0.277564 0.128024 -0.306236
+v 0.300083 -0.001024 -0.300092
+v 0.277564 0.058384 -0.302140
+f 644 640 646
+f 638 637 639
+f 637 638 640
+f 639 637 640
+f 639 636 641
+f 640 638 642
+f 638 641 642
+f 641 636 642
+f 638 639 643
+f 641 638 643
+f 639 641 643
+f 636 639 644
+f 639 640 644
+f 636 640 645
+f 642 636 645
+f 640 642 645
+f 640 636 646
+f 636 644 646
+o convex_50
+v 0.259101 0.177189 -0.160810
+v 0.054282 0.177189 -0.281653
+v 0.054282 0.175137 -0.271398
+v 0.054282 0.193569 -0.160810
+v 0.259101 0.193569 -0.281653
+v 0.259101 0.175137 -0.267293
+v 0.054282 0.193569 -0.281653
+v 0.259101 0.193569 -0.160810
+v 0.093221 0.175137 -0.160810
+v 0.054282 0.177189 -0.160810
+v 0.259101 0.177189 -0.281653
+v 0.224273 0.175137 -0.160810
+f 655 652 658
+f 648 649 650
+f 651 647 652
+f 648 650 653
+f 650 651 653
+f 651 648 653
+f 650 647 654
+f 647 651 654
+f 651 650 654
+f 647 650 655
+f 649 652 655
+f 650 649 656
+f 649 655 656
+f 655 650 656
+f 649 648 657
+f 648 651 657
+f 651 652 657
+f 652 649 657
+f 652 647 658
+f 647 655 658
+o convex_51
+v 0.285745 0.128029 -0.281653
+v 0.261175 0.173089 -0.250933
+v 0.259124 0.173089 -0.250933
+v 0.289847 0.173089 -0.281653
+v 0.271415 0.173089 -0.281653
+v 0.259124 0.158745 -0.269359
+v 0.289847 0.128029 -0.279599
+v 0.259124 0.162843 -0.250933
+v 0.271415 0.148512 -0.281653
+v 0.259124 0.173089 -0.269359
+v 0.289847 0.173089 -0.279599
+f 665 662 669
+f 661 660 662
+f 662 659 663
+f 661 662 663
+f 659 662 665
+f 660 661 666
+f 661 664 666
+f 664 659 666
+f 659 665 666
+f 665 660 666
+f 663 659 667
+f 664 663 667
+f 659 664 667
+f 661 663 668
+f 664 661 668
+f 663 664 668
+f 662 660 669
+f 660 665 669
+o convex_52
+v 0.259124 0.173089 -0.269351
+v 0.289847 0.193571 -0.160810
+v 0.259124 0.193571 -0.160810
+v 0.289847 0.193571 -0.281653
+v 0.289847 0.175140 -0.160810
+v 0.259124 0.193571 -0.281653
+v 0.289847 0.173089 -0.281653
+v 0.259124 0.177189 -0.160810
+v 0.259124 0.177189 -0.281653
+v 0.259124 0.173089 -0.250910
+f 674 677 679
+f 672 671 673
+f 671 672 674
+f 673 671 674
+f 670 672 675
+f 672 673 675
+f 675 673 676
+f 673 674 676
+f 672 670 677
+f 674 672 677
+f 670 675 678
+f 675 676 678
+f 676 670 678
+f 670 676 679
+f 676 674 679
+f 677 670 679
+o convex_53
+v 0.060421 0.173089 -0.050189
+v 0.027653 0.128029 -0.037900
+v 0.029706 0.128029 -0.037900
+v 0.060421 0.162843 -0.068621
+v 0.027653 0.173089 -0.037900
+v 0.046085 0.148512 -0.035850
+v 0.058374 0.173089 -0.068621
+v 0.046085 0.173089 -0.035850
+v 0.060421 0.160792 -0.050189
+v 0.027653 0.128029 -0.035850
+f 685 687 689
+f 682 681 683
+f 683 681 686
+f 680 683 686
+f 684 680 686
+f 681 684 686
+f 680 684 687
+f 685 680 687
+f 682 683 688
+f 683 680 688
+f 685 682 688
+f 680 685 688
+f 681 682 689
+f 684 681 689
+f 682 685 689
+f 687 684 689
+o convex_54
+v 0.058374 0.173089 -0.068626
+v 0.060421 0.193571 -0.035847
+v 0.027653 0.193571 -0.035847
+v 0.060421 0.193571 -0.160798
+v 0.027653 0.175140 -0.160798
+v 0.027653 0.173089 -0.035847
+v 0.027653 0.193571 -0.160798
+v 0.060421 0.177189 -0.160798
+v 0.060421 0.177189 -0.035847
+v 0.060421 0.173089 -0.050194
+f 698 695 699
+f 692 691 693
+f 692 694 695
+f 691 692 695
+f 694 690 695
+f 692 693 696
+f 693 694 696
+f 694 692 696
+f 693 691 697
+f 690 694 697
+f 694 693 697
+f 691 695 698
+f 697 691 698
+f 697 698 699
+f 695 690 699
+f 690 697 699
+o convex_55
+v 0.291893 0.095257 -0.300087
+v 0.291893 0.128024 -0.281655
+v 0.285752 0.128024 -0.281655
+v 0.289845 0.128024 -0.300087
+v 0.298037 0.128024 -0.300087
+v 0.298037 0.097302 -0.291894
+v 0.279607 0.125971 -0.287801
+v 0.298037 0.128024 -0.287801
+f 705 704 707
+f 702 701 703
+f 700 703 704
+f 703 701 704
+f 701 702 705
+f 700 704 705
+f 705 702 706
+f 702 703 706
+f 703 700 706
+f 700 705 706
+f 704 701 707
+f 701 705 707
+o convex_56
+v 0.318522 0.128024 -0.281655
+v 0.298042 0.087046 -0.300087
+v 0.300091 -0.001024 -0.300087
+v 0.318522 0.128024 -0.300087
+v 0.318522 -0.001024 -0.281655
+v 0.300091 -0.001024 -0.281655
+v 0.298042 0.128024 -0.289848
+v 0.318522 -0.001024 -0.300087
+v 0.306236 0.128024 -0.281655
+v 0.298042 0.128024 -0.300087
+v 0.300091 0.058384 -0.281655
+f 716 714 718
+f 710 709 711
+f 711 708 712
+f 710 712 713
+f 709 710 713
+f 712 708 713
+f 708 711 714
+f 709 713 714
+f 710 711 715
+f 711 712 715
+f 712 710 715
+f 713 708 716
+f 708 714 716
+f 711 709 717
+f 709 714 717
+f 714 711 717
+f 714 713 718
+f 713 716 718
+o convex_57
+v 0.111631 -0.001024 -0.207910
+v 0.107535 0.056331 -0.166944
+v 0.093206 0.056331 -0.166944
+v 0.119827 0.056331 -0.212008
+v 0.107535 -0.001024 -0.166944
+v 0.087060 -0.001024 -0.191518
+v 0.119827 -0.001024 -0.191518
+v 0.087060 0.037894 -0.191518
+v 0.093206 -0.001024 -0.166944
+v 0.119827 0.056331 -0.191518
+v 0.119827 -0.001024 -0.212008
+v 0.087060 -0.001024 -0.179242
+v 0.099349 0.056331 -0.191518
+v 0.117778 0.056331 -0.212008
+v 0.087060 0.037894 -0.179242
+f 726 730 733
+f 721 720 722
+f 720 721 723
+f 719 723 724
+f 723 719 725
+f 720 723 725
+f 719 724 726
+f 723 721 727
+f 724 723 727
+f 722 720 728
+f 720 725 728
+f 725 722 728
+f 722 725 729
+f 725 719 729
+f 726 724 730
+f 727 721 730
+f 724 727 730
+f 721 722 731
+f 726 721 731
+f 726 731 732
+f 719 726 732
+f 729 719 732
+f 722 729 732
+f 731 722 732
+f 721 726 733
+f 730 721 733
+o convex_58
+v 0.123931 -0.001024 -0.230439
+v 0.121882 0.039944 -0.193576
+v 0.148502 0.039944 -0.207914
+v 0.121882 -0.001024 -0.193576
+v 0.148502 -0.001024 -0.207914
+v 0.134168 0.037894 -0.232492
+v 0.148502 -0.001024 -0.224294
+v 0.119831 0.039944 -0.214051
+v 0.148502 0.039944 -0.224294
+v 0.123931 0.037894 -0.230439
+v 0.119831 -0.001024 -0.193576
+v 0.134168 -0.001024 -0.232492
+v 0.119831 -0.001024 -0.214051
+f 744 741 746
+f 736 735 737
+f 737 734 738
+f 736 737 738
+f 738 734 740
+f 736 738 740
+f 735 736 741
+f 740 739 742
+f 736 740 742
+f 741 736 742
+f 739 734 743
+f 734 741 743
+f 742 739 743
+f 741 742 743
+f 737 735 744
+f 734 737 744
+f 735 741 744
+f 734 739 745
+f 740 734 745
+f 739 740 745
+f 741 734 746
+f 734 744 746
+o convex_59
+v 0.158750 0.111626 -0.224295
+v 0.117782 0.111626 -0.212007
+v 0.117782 0.111626 -0.203819
+v 0.119835 0.039951 -0.193573
+v 0.136225 0.039951 -0.222241
+v 0.158750 0.070680 -0.209953
+v 0.125979 0.156686 -0.203819
+v 0.136217 0.156686 -0.222247
+v 0.119835 0.039951 -0.214049
+v 0.148500 0.039951 -0.224295
+v 0.121885 0.085010 -0.193573
+v 0.158750 0.111626 -0.214049
+v 0.117782 0.156686 -0.212007
+v 0.148500 0.039951 -0.207918
+v 0.140311 0.156686 -0.212007
+v 0.117782 0.156686 -0.203819
+v 0.158750 0.070680 -0.224295
+v 0.125979 0.156686 -0.218148
+v 0.158750 0.085010 -0.209953
+v 0.144406 0.111626 -0.224295
+v 0.121885 0.039951 -0.193573
+f 760 752 767
+f 749 748 750
+f 750 748 755
+f 751 750 755
+f 750 751 756
+f 752 747 758
+f 748 749 759
+f 753 754 759
+f 755 748 759
+f 750 756 760
+f 756 752 760
+f 747 754 761
+f 754 753 761
+f 753 758 761
+f 758 747 761
+f 749 750 762
+f 750 757 762
+f 757 753 762
+f 759 749 762
+f 753 759 762
+f 747 752 763
+f 756 747 763
+f 752 756 763
+f 754 751 764
+f 751 755 764
+f 759 754 764
+f 755 759 764
+f 757 752 765
+f 753 757 765
+f 758 753 765
+f 752 758 765
+f 754 747 766
+f 751 754 766
+f 756 751 766
+f 747 756 766
+f 757 750 767
+f 752 757 767
+f 750 760 767
+o convex_60
+v 0.212008 0.056331 -0.197673
+v 0.164900 -0.001024 -0.212011
+v 0.166952 -0.001024 -0.224295
+v 0.212008 -0.001024 -0.195624
+v 0.189473 0.037894 -0.230440
+v 0.166952 0.056331 -0.207915
+v 0.189473 -0.001024 -0.230440
+v 0.166952 0.056331 -0.224295
+v 0.191520 -0.001024 -0.195624
+v 0.191520 0.056331 -0.195624
+v 0.193568 0.056331 -0.216101
+v 0.179241 -0.001024 -0.230440
+v 0.209951 -0.001024 -0.201770
+v 0.166952 -0.001024 -0.207915
+v 0.179241 0.037894 -0.230440
+v 0.212008 0.056331 -0.195624
+f 777 771 783
+f 769 770 771
+f 771 770 774
+f 770 769 775
+f 769 773 775
+f 773 768 775
+f 769 771 776
+f 773 776 777
+f 768 773 777
+f 776 771 777
+f 768 772 778
+f 775 768 778
+f 772 775 778
+f 774 770 779
+f 772 774 779
+f 770 775 779
+f 768 771 780
+f 772 768 780
+f 771 774 780
+f 774 772 780
+f 773 769 781
+f 769 776 781
+f 776 773 781
+f 775 772 782
+f 772 779 782
+f 779 775 782
+f 771 768 783
+f 768 777 783
+o convex_61
+v 0.230439 0.037894 -0.179237
+v 0.195625 -0.001024 -0.195621
+v 0.214058 -0.001024 -0.195621
+v 0.207916 -0.001024 -0.166949
+v 0.195625 0.039944 -0.195621
+v 0.207916 0.039944 -0.166949
+v 0.232492 -0.001024 -0.189471
+v 0.228389 0.037894 -0.193569
+v 0.222247 -0.001024 -0.168995
+v 0.222247 0.039944 -0.168995
+v 0.195625 0.039944 -0.191520
+v 0.214058 0.039944 -0.195621
+v 0.232492 -0.001024 -0.183327
+v 0.232492 0.037894 -0.189471
+f 796 790 797
+f 785 786 787
+f 786 785 788
+f 787 786 790
+f 790 786 791
+f 789 787 792
+f 787 790 792
+f 788 789 793
+f 792 784 793
+f 789 792 793
+f 785 787 794
+f 788 785 794
+f 787 789 794
+f 789 788 794
+f 786 788 795
+f 791 786 795
+f 788 793 795
+f 784 792 796
+f 792 790 796
+f 790 791 797
+f 793 784 797
+f 791 795 797
+f 795 793 797
+f 784 796 797
+o convex_62
+v 0.212004 0.111632 -0.203810
+v 0.224296 0.111632 -0.160802
+v 0.224296 0.156689 -0.160810
+v 0.195624 0.085008 -0.191520
+v 0.197673 0.175135 -0.201764
+v 0.222244 0.039964 -0.179235
+v 0.209961 0.070690 -0.160802
+v 0.195624 0.039964 -0.195617
+v 0.214051 0.175135 -0.160810
+v 0.218146 0.175135 -0.189470
+v 0.214051 0.039964 -0.195617
+v 0.216100 0.039964 -0.166949
+v 0.224293 0.175135 -0.169004
+v 0.211999 0.175135 -0.201764
+v 0.224296 0.070690 -0.160802
+v 0.199722 0.111632 -0.203810
+v 0.207915 0.039964 -0.166949
+v 0.197673 0.175135 -0.197663
+v 0.195624 0.039964 -0.191520
+v 0.212004 0.156689 -0.203810
+v 0.222244 0.175135 -0.179247
+f 807 810 818
+f 801 802 805
+f 799 800 806
+f 801 804 806
+f 804 799 806
+f 803 798 807
+f 802 806 807
+f 798 803 808
+f 805 798 808
+f 803 805 808
+f 805 803 809
+f 806 800 810
+f 807 806 810
+f 802 807 811
+f 800 799 812
+f 799 804 812
+f 809 803 812
+f 804 809 812
+f 810 800 812
+f 803 810 812
+f 798 805 813
+f 805 802 813
+f 809 804 814
+f 805 809 814
+f 802 801 815
+f 801 806 815
+f 806 802 815
+f 804 801 816
+f 801 805 816
+f 814 804 816
+f 805 814 816
+f 807 798 817
+f 802 811 817
+f 811 807 817
+f 798 813 817
+f 813 802 817
+f 803 807 818
+f 810 803 818
+o convex_63
+v 0.103451 0.111643 -0.203814
+v 0.101403 0.175135 -0.160803
+v 0.123927 0.175135 -0.201760
+v 0.101403 0.111643 -0.160803
+v 0.093205 0.175135 -0.177189
+v 0.123927 0.111643 -0.201760
+v 0.103451 0.175135 -0.203814
+v 0.093205 0.111643 -0.160803
+v 0.093205 0.111643 -0.177189
+v 0.093205 0.175135 -0.160803
+v 0.123927 0.111643 -0.203814
+v 0.123927 0.175135 -0.203814
+f 821 829 830
+f 821 820 822
+f 820 821 823
+f 821 822 824
+f 822 819 824
+f 819 823 825
+f 823 821 825
+f 822 820 826
+f 819 822 826
+f 826 823 827
+f 823 819 827
+f 819 826 827
+f 820 823 828
+f 826 820 828
+f 823 826 828
+f 824 819 829
+f 821 824 829
+f 819 825 829
+f 829 825 830
+f 825 821 830
+o convex_64
+v 0.144419 0.111637 -0.224296
+v 0.199718 0.156697 -0.197672
+v 0.199718 0.156697 -0.212004
+v 0.140320 0.156697 -0.212004
+v 0.199718 0.111637 -0.197672
+v 0.185374 0.111637 -0.220199
+v 0.173083 0.156697 -0.224296
+v 0.140320 0.111637 -0.212004
+v 0.140320 0.156697 -0.222247
+v 0.199718 0.111637 -0.212004
+v 0.173083 0.111637 -0.224296
+v 0.197660 0.111637 -0.197672
+v 0.144419 0.156697 -0.224296
+f 831 839 843
+f 832 833 834
+f 833 832 835
+f 835 831 836
+f 834 833 837
+f 833 836 837
+f 831 835 838
+f 834 837 839
+f 831 838 839
+f 838 834 839
+f 833 835 840
+f 836 833 840
+f 835 836 840
+f 836 831 841
+f 831 837 841
+f 837 836 841
+f 832 834 842
+f 835 832 842
+f 834 838 842
+f 838 835 842
+f 837 831 843
+f 839 837 843
+o convex_65
+v 0.123924 0.068621 -0.121885
+v 0.085008 -0.001024 -0.136215
+v 0.085008 -0.001024 -0.125981
+v 0.123924 -0.001024 -0.119834
+v 0.107536 0.068621 -0.150556
+v 0.107536 -0.001024 -0.150556
+v 0.103449 0.068621 -0.119834
+v 0.093206 0.068621 -0.148502
+v 0.085008 0.037889 -0.125981
+v 0.093206 -0.001024 -0.148502
+v 0.103449 -0.001024 -0.119834
+v 0.085008 0.037889 -0.136215
+v 0.123924 0.068621 -0.119834
+v 0.087061 -0.001024 -0.123933
+v 0.093206 0.068621 -0.140310
+v 0.123924 -0.001024 -0.121885
+f 849 844 859
+f 846 845 847
+f 847 845 849
+f 848 844 849
+f 844 848 850
+f 850 848 851
+f 848 849 851
+f 845 846 852
+f 849 845 853
+f 845 851 853
+f 851 849 853
+f 846 847 854
+f 847 850 854
+f 851 845 855
+f 845 852 855
+f 852 851 855
+f 847 844 856
+f 850 847 856
+f 844 850 856
+f 852 846 857
+f 850 852 857
+f 846 854 857
+f 854 850 857
+f 850 851 858
+f 852 850 858
+f 851 852 858
+f 844 847 859
+f 847 849 859
+o convex_66
+v 0.105497 0.068631 -0.150548
+v 0.119828 0.175135 -0.113690
+v 0.103452 0.175135 -0.113690
+v 0.101403 0.175135 -0.160798
+v 0.123927 0.068631 -0.121884
+v 0.093205 0.068631 -0.140311
+v 0.103449 0.068631 -0.119837
+v 0.093205 0.175135 -0.160798
+v 0.107545 0.070684 -0.160794
+v 0.093205 0.070684 -0.160794
+v 0.093205 0.175135 -0.140306
+v 0.119828 0.175135 -0.117794
+v 0.107545 0.085019 -0.160794
+v 0.119828 0.117796 -0.115737
+v 0.103452 0.154649 -0.113690
+v 0.123927 0.085019 -0.121884
+v 0.123927 0.068631 -0.119837
+f 864 875 876
+f 862 861 863
+f 860 864 865
+f 865 864 866
+f 862 863 867
+f 864 860 868
+f 867 863 868
+f 860 865 869
+f 865 867 869
+f 867 868 869
+f 868 860 869
+f 862 867 870
+f 867 865 870
+f 863 861 871
+f 864 868 872
+f 868 863 872
+f 863 871 872
+f 866 873 874
+f 861 862 874
+f 865 866 874
+f 862 870 874
+f 870 865 874
+f 873 861 874
+f 871 861 875
+f 864 872 875
+f 872 871 875
+f 866 864 876
+f 861 873 876
+f 873 866 876
+f 875 861 876
+o convex_67
+v 0.121879 0.056331 -0.101399
+v 0.105492 -0.001024 -0.119827
+v 0.125982 -0.001024 -0.119827
+v 0.125982 -0.001024 -0.087060
+v 0.150556 0.056331 -0.107535
+v 0.105492 0.056331 -0.119827
+v 0.150556 -0.001024 -0.107535
+v 0.148505 0.056331 -0.093206
+v 0.125982 0.037894 -0.087060
+v 0.148505 -0.001024 -0.093206
+v 0.125982 0.056331 -0.119827
+v 0.107543 0.056331 -0.113681
+v 0.136211 -0.001024 -0.087060
+v 0.107543 -0.001024 -0.113681
+v 0.136211 0.037894 -0.087060
+f 889 884 891
+f 878 879 880
+f 879 878 882
+f 877 881 882
+f 880 879 883
+f 879 881 883
+f 883 881 884
+f 881 877 884
+f 884 877 885
+f 880 883 886
+f 883 884 886
+f 881 879 887
+f 879 882 887
+f 882 881 887
+f 882 878 888
+f 877 882 888
+f 880 885 888
+f 885 877 888
+f 885 880 889
+f 880 886 889
+f 886 884 889
+f 878 880 890
+f 888 878 890
+f 880 888 890
+f 884 885 891
+f 885 889 891
+o convex_68
+v 0.220198 0.056331 -0.132124
+v 0.195624 -0.001024 -0.125980
+v 0.207915 -0.001024 -0.150551
+v 0.230440 -0.001024 -0.128029
+v 0.195624 0.056331 -0.121879
+v 0.220198 0.056331 -0.150551
+v 0.207915 0.056331 -0.150551
+v 0.226343 0.037894 -0.123931
+v 0.222247 -0.001024 -0.148505
+v 0.214053 -0.001024 -0.121879
+v 0.230440 0.037894 -0.136216
+v 0.214053 0.056331 -0.121879
+v 0.195624 -0.001024 -0.121879
+v 0.195624 0.056331 -0.125980
+v 0.230440 -0.001024 -0.136216
+f 900 902 906
+f 893 894 895
+f 896 892 897
+f 896 897 898
+f 894 893 898
+f 897 894 898
+f 895 894 900
+f 894 897 900
+f 893 895 901
+f 895 899 901
+f 897 892 902
+f 899 895 902
+f 892 899 902
+f 900 897 902
+f 892 896 903
+f 899 892 903
+f 896 901 903
+f 901 899 903
+f 896 893 904
+f 893 901 904
+f 901 896 904
+f 893 896 905
+f 896 898 905
+f 898 893 905
+f 895 900 906
+f 902 895 906
+o convex_69
+v 0.220195 0.056342 -0.150551
+v 0.212007 0.154633 -0.115734
+v 0.203823 0.154633 -0.115734
+v 0.214051 0.175135 -0.160798
+v 0.195624 0.056342 -0.125985
+v 0.224296 0.175135 -0.146449
+v 0.216102 0.056342 -0.123934
+v 0.197676 0.175135 -0.119841
+v 0.209961 0.070681 -0.160798
+v 0.224298 0.070681 -0.160798
+v 0.224296 0.175135 -0.160798
+v 0.222244 0.056342 -0.138266
+v 0.220198 0.175135 -0.132130
+v 0.195624 0.056342 -0.121883
+v 0.195624 0.085020 -0.125985
+v 0.212007 0.117780 -0.115734
+v 0.212002 0.175135 -0.117798
+v 0.207915 0.056342 -0.150551
+v 0.224298 0.070681 -0.146453
+v 0.214053 0.056342 -0.121883
+v 0.197676 0.175135 -0.117798
+v 0.203823 0.117780 -0.115734
+f 927 920 928
+f 911 907 913
+f 912 910 914
+f 907 915 916
+f 915 910 916
+f 910 912 917
+f 916 910 917
+f 912 916 917
+f 913 907 918
+f 907 916 918
+f 912 914 919
+f 918 912 919
+f 913 918 919
+f 911 913 920
+f 914 910 921
+f 910 915 921
+f 915 911 921
+f 911 920 921
+f 908 909 922
+f 919 908 922
+f 913 919 922
+f 909 908 923
+f 908 919 923
+f 919 914 923
+f 907 911 924
+f 915 907 924
+f 911 915 924
+f 916 912 925
+f 912 918 925
+f 918 916 925
+f 920 913 926
+f 913 922 926
+f 922 920 926
+f 914 921 927
+f 921 920 927
+f 909 923 927
+f 923 914 927
+f 922 909 928
+f 920 922 928
+f 909 927 928
+o convex_70
+v 0.191518 0.054282 -0.099349
+v 0.166944 -0.001024 -0.107538
+v 0.189476 -0.001024 -0.119826
+v 0.189476 -0.001024 -0.085008
+v 0.212008 0.054282 -0.121875
+v 0.166944 0.054282 -0.107538
+v 0.212008 -0.001024 -0.121875
+v 0.168995 0.054282 -0.093207
+v 0.189476 0.037895 -0.085008
+v 0.193569 0.054282 -0.121875
+v 0.168995 -0.001024 -0.093207
+v 0.212008 0.054282 -0.117776
+v 0.181289 0.037895 -0.085008
+v 0.212008 -0.001024 -0.117776
+v 0.193569 0.037895 -0.089111
+v 0.181289 -0.001024 -0.085008
+f 941 939 944
+f 930 931 932
+f 931 930 934
+f 929 933 934
+f 932 931 935
+f 934 930 936
+f 929 934 936
+f 929 936 937
+f 931 934 938
+f 934 933 938
+f 935 931 938
+f 933 935 938
+f 930 932 939
+f 936 930 939
+f 933 929 940
+f 935 933 940
+f 929 937 940
+f 932 937 941
+f 937 936 941
+f 936 939 941
+f 932 935 942
+f 935 940 942
+f 942 940 943
+f 937 932 943
+f 940 937 943
+f 932 942 943
+f 939 932 944
+f 932 941 944
+o convex_71
+v 0.203815 0.117782 -0.115734
+v 0.244774 0.154646 -0.076822
+v 0.242729 0.154646 -0.074771
+v 0.203815 0.154646 -0.115734
+v 0.220206 0.117782 -0.087060
+v 0.228390 0.117782 -0.099344
+v 0.203815 0.154646 -0.103437
+v 0.212014 0.154646 -0.115734
+v 0.236582 0.154646 -0.070674
+v 0.203815 0.117782 -0.103437
+v 0.244774 0.154646 -0.082966
+v 0.212014 0.117782 -0.115734
+v 0.232488 0.123932 -0.091157
+f 950 955 957
+f 947 946 948
+f 949 945 950
+f 947 948 951
+f 948 945 951
+f 948 946 952
+f 945 948 952
+f 949 947 953
+f 947 951 953
+f 953 951 954
+f 945 949 954
+f 951 945 954
+f 949 953 954
+f 952 946 955
+f 952 955 956
+f 950 945 956
+f 945 952 956
+f 955 950 956
+f 946 947 957
+f 947 949 957
+f 949 950 957
+f 955 946 957
+o convex_72
+v 0.154663 0.117782 -0.093205
+v 0.203814 0.154646 -0.103448
+v 0.203814 0.154646 -0.123927
+v 0.203814 0.117782 -0.103448
+v 0.201760 0.117782 -0.123927
+v 0.154663 0.154646 -0.101400
+v 0.175141 0.154646 -0.093205
+v 0.154663 0.117782 -0.101400
+v 0.175141 0.117782 -0.093205
+v 0.154663 0.154646 -0.093205
+f 964 963 967
+f 960 959 961
+f 961 958 962
+f 960 961 962
+f 960 962 963
+f 959 960 963
+f 961 959 964
+f 959 963 964
+f 962 958 965
+f 963 962 965
+f 958 963 965
+f 958 961 966
+f 964 958 966
+f 961 964 966
+f 963 958 967
+f 958 964 967
+o convex_73
+v 0.261155 0.175137 -0.048139
+v 0.060447 0.177190 -0.160798
+v 0.060447 0.193567 -0.160798
+v 0.060447 0.193567 -0.035847
+v 0.261155 0.193567 -0.160798
+v 0.261155 0.193567 -0.035847
+v 0.060447 0.177190 -0.035847
+v 0.261155 0.177190 -0.160798
+v 0.093220 0.175137 -0.160798
+v 0.060447 0.175137 -0.052236
+v 0.261155 0.177190 -0.035847
+v 0.224295 0.175137 -0.160798
+f 976 975 979
+f 970 969 971
+f 969 970 972
+f 970 971 972
+f 968 972 973
+f 972 971 973
+f 971 969 974
+f 973 971 974
+f 972 968 975
+f 969 972 975
+f 969 975 976
+f 974 969 977
+f 968 974 977
+f 969 976 977
+f 976 968 977
+f 968 973 978
+f 974 968 978
+f 973 974 978
+f 975 968 979
+f 968 976 979
+o convex_74
+v 0.259112 0.175135 -0.048144
+v 0.195634 0.154655 -0.117775
+v 0.195634 0.175135 -0.117775
+v 0.187427 0.154655 -0.099337
+v 0.246821 0.154655 -0.080918
+v 0.212018 0.175135 -0.117775
+v 0.187427 0.175135 -0.099337
+v 0.259112 0.158753 -0.048144
+v 0.261155 0.175135 -0.066583
+v 0.212018 0.154655 -0.117775
+v 0.187427 0.154655 -0.111627
+v 0.261155 0.162849 -0.066583
+v 0.238629 0.154655 -0.068628
+f 984 987 992
+f 983 981 984
+f 982 980 985
+f 981 982 985
+f 980 982 986
+f 983 980 986
+f 980 983 987
+f 985 980 988
+f 980 987 988
+f 984 981 989
+f 981 985 989
+f 985 988 989
+f 982 981 990
+f 981 983 990
+f 986 982 990
+f 983 986 990
+f 987 984 991
+f 988 987 991
+f 984 989 991
+f 989 988 991
+f 983 984 992
+f 987 983 992
+o convex_75
+v 0.107534 0.056331 -0.166948
+v 0.115727 0.056331 -0.212005
+v 0.109583 0.062481 -0.214051
+v 0.119822 0.111637 -0.197672
+v 0.091153 0.111637 -0.220191
+v 0.093202 0.111637 -0.160802
+v 0.093202 0.056331 -0.177194
+v 0.101398 0.111637 -0.226344
+v 0.107534 0.085002 -0.160802
+v 0.119828 0.056331 -0.191526
+v 0.119822 0.111637 -0.212005
+v 0.093202 0.070672 -0.160802
+v 0.101401 0.111637 -0.160802
+v 0.119822 0.056331 -0.212005
+v 0.103447 0.062481 -0.207912
+v 0.091153 0.107539 -0.220191
+v 0.093202 0.056331 -0.166948
+v 0.107534 0.070672 -0.160802
+f 1001 1004 1010
+f 996 997 998
+f 994 993 999
+f 997 996 1000
+f 993 994 1002
+f 996 1001 1002
+f 1000 996 1003
+f 996 1002 1003
+f 998 997 1004
+f 1001 998 1004
+f 996 998 1005
+f 1001 996 1005
+f 998 1001 1005
+f 994 995 1006
+f 995 1000 1006
+f 1002 994 1006
+f 1000 1003 1006
+f 1003 1002 1006
+f 995 994 1007
+f 994 999 1007
+f 1007 999 1008
+f 1000 995 1008
+f 997 1000 1008
+f 1004 997 1008
+f 995 1007 1008
+f 1004 1008 1009
+f 999 993 1009
+f 993 1004 1009
+f 1008 999 1009
+f 993 1002 1010
+f 1002 1001 1010
+f 1004 993 1010
+o convex_76
+v 0.181274 0.175135 -0.095253
+v 0.119831 0.154655 -0.115732
+v 0.119831 0.154655 -0.103446
+v 0.187414 0.154655 -0.109585
+v 0.119831 0.175135 -0.115732
+v 0.175128 0.154655 -0.093202
+v 0.140321 0.175135 -0.093202
+v 0.187414 0.175135 -0.109585
+v 0.140321 0.154655 -0.093202
+v 0.121888 0.175135 -0.101397
+v 0.187414 0.154655 -0.099350
+f 1014 1018 1021
+f 1013 1012 1014
+f 1012 1013 1015
+f 1014 1012 1015
+f 1013 1014 1016
+f 1011 1015 1017
+f 1016 1011 1017
+f 1015 1011 1018
+f 1014 1015 1018
+f 1013 1016 1019
+f 1016 1017 1019
+f 1019 1017 1020
+f 1015 1013 1020
+f 1017 1015 1020
+f 1013 1019 1020
+f 1011 1016 1021
+f 1016 1014 1021
+f 1018 1011 1021
+o convex_77
+v 0.164904 0.056331 -0.212002
+v 0.226347 0.111637 -0.216098
+v 0.226347 0.109585 -0.216098
+v 0.212008 0.111637 -0.195621
+v 0.158750 0.111637 -0.224293
+v 0.212008 0.056331 -0.195621
+v 0.173095 0.056331 -0.224293
+v 0.220193 0.109585 -0.226344
+v 0.158750 0.085002 -0.209960
+v 0.191523 0.056331 -0.195624
+v 0.207912 0.064527 -0.214050
+v 0.199721 0.111637 -0.195621
+v 0.158750 0.070672 -0.224293
+v 0.158750 0.111637 -0.214053
+v 0.214053 0.064527 -0.207918
+v 0.158750 0.070672 -0.209960
+v 0.191523 0.085002 -0.195624
+v 0.199721 0.056331 -0.212002
+f 1032 1036 1039
+f 1024 1023 1025
+f 1025 1023 1026
+f 1024 1025 1027
+f 1027 1022 1028
+f 1023 1024 1029
+f 1026 1023 1029
+f 1022 1027 1031
+f 1028 1029 1032
+f 1025 1026 1033
+f 1027 1025 1033
+f 1031 1027 1033
+f 1028 1022 1034
+f 1029 1028 1034
+f 1026 1029 1034
+f 1030 1026 1034
+f 1026 1030 1035
+f 1033 1026 1035
+f 1030 1033 1035
+f 1024 1027 1036
+f 1029 1024 1036
+f 1032 1029 1036
+f 1022 1031 1037
+f 1031 1030 1037
+f 1034 1022 1037
+f 1030 1034 1037
+f 1030 1031 1038
+f 1033 1030 1038
+f 1031 1033 1038
+f 1027 1028 1039
+f 1028 1032 1039
+f 1036 1027 1039
+o convex_78
+v 0.087065 0.117788 -0.097287
+v 0.119813 0.175135 -0.105485
+v 0.119813 0.154655 -0.105485
+v 0.062491 0.175135 -0.052246
+v 0.103431 0.175135 -0.113683
+v 0.113680 0.117788 -0.103434
+v 0.060427 0.175135 -0.070674
+v 0.103436 0.117788 -0.115734
+v 0.097291 0.117788 -0.087057
+v 0.060427 0.162846 -0.052246
+v 0.119813 0.154655 -0.113683
+v 0.060427 0.162846 -0.070674
+v 0.113680 0.117788 -0.115734
+v 0.103436 0.154644 -0.115734
+v 0.119813 0.175135 -0.113683
+v 0.062491 0.162846 -0.052246
+f 1049 1048 1055
+f 1042 1041 1043
+f 1043 1041 1044
+f 1043 1044 1046
+f 1045 1040 1047
+f 1045 1042 1048
+f 1040 1045 1048
+f 1043 1046 1049
+f 1040 1048 1049
+f 1041 1042 1050
+f 1042 1045 1050
+f 1046 1047 1051
+f 1047 1040 1051
+f 1040 1049 1051
+f 1049 1046 1051
+f 1045 1047 1052
+f 1050 1045 1052
+f 1046 1044 1053
+f 1047 1046 1053
+f 1052 1047 1053
+f 1044 1041 1054
+f 1041 1050 1054
+f 1050 1052 1054
+f 1052 1053 1054
+f 1053 1044 1054
+f 1042 1043 1055
+f 1048 1042 1055
+f 1043 1049 1055
+o convex_79
+v 0.089109 0.111637 -0.220205
+v 0.117777 0.156697 -0.203819
+v 0.117777 0.156697 -0.212014
+v 0.068621 0.156697 -0.238633
+v 0.078870 0.154646 -0.248879
+v 0.117777 0.111637 -0.212014
+v 0.103439 0.111637 -0.203819
+v 0.103439 0.156697 -0.203819
+v 0.099339 0.113693 -0.228391
+v 0.117777 0.111637 -0.203819
+v 0.074770 0.154646 -0.248879
+f 1060 1064 1066
+f 1057 1058 1059
+f 1059 1058 1060
+f 1060 1058 1061
+f 1058 1057 1061
+f 1056 1061 1062
+f 1059 1056 1062
+f 1059 1062 1063
+f 1057 1059 1063
+f 1062 1057 1063
+f 1060 1061 1064
+f 1061 1056 1064
+f 1061 1057 1065
+f 1062 1061 1065
+f 1057 1062 1065
+f 1056 1059 1066
+f 1059 1060 1066
+f 1064 1056 1066
+o convex_80
+v 0.207916 0.054282 -0.113680
+v 0.222246 0.117782 -0.089108
+v 0.218149 0.117782 -0.089108
+v 0.212010 0.117782 -0.121876
+v 0.195624 0.117782 -0.103447
+v 0.195624 0.054282 -0.103447
+v 0.195624 0.054282 -0.121876
+v 0.212010 0.054282 -0.121876
+v 0.228392 0.113680 -0.099351
+v 0.195624 0.117782 -0.117776
+v 0.214053 0.066577 -0.107537
+v 0.199724 0.117782 -0.121876
+v 0.222246 0.113680 -0.089108
+f 1077 1075 1079
+f 1069 1068 1070
+f 1069 1070 1071
+f 1069 1071 1072
+f 1072 1071 1073
+f 1067 1072 1073
+f 1067 1073 1074
+f 1073 1070 1074
+f 1070 1068 1075
+f 1074 1070 1075
+f 1071 1070 1076
+f 1073 1071 1076
+f 1072 1067 1077
+f 1067 1074 1077
+f 1074 1075 1077
+f 1070 1073 1078
+f 1076 1070 1078
+f 1073 1076 1078
+f 1068 1069 1079
+f 1069 1072 1079
+f 1075 1068 1079
+f 1072 1077 1079
+o convex_81
+v 0.212020 0.175135 -0.201766
+v 0.216116 0.111643 -0.226351
+v 0.218161 0.113694 -0.228397
+v 0.197669 0.175135 -0.212018
+v 0.259115 0.175135 -0.267308
+v 0.214065 0.111643 -0.203819
+v 0.259115 0.162847 -0.248870
+v 0.199726 0.111643 -0.203819
+v 0.257058 0.160796 -0.267308
+v 0.228392 0.113694 -0.218164
+v 0.199726 0.111643 -0.212018
+v 0.259115 0.175135 -0.248870
+v 0.197669 0.175135 -0.201766
+v 0.259115 0.160796 -0.267308
+v 0.226347 0.111643 -0.222258
+v 0.197669 0.156700 -0.212018
+f 1087 1092 1095
+f 1083 1080 1084
+f 1080 1085 1086
+f 1081 1085 1087
+f 1085 1080 1087
+f 1083 1084 1088
+f 1086 1085 1089
+f 1082 1081 1090
+f 1081 1087 1090
+f 1088 1082 1090
+f 1084 1080 1091
+f 1080 1086 1091
+f 1086 1084 1091
+f 1080 1083 1092
+f 1087 1080 1092
+f 1084 1086 1093
+f 1082 1088 1093
+f 1088 1084 1093
+f 1086 1089 1093
+f 1093 1089 1094
+f 1081 1082 1094
+f 1085 1081 1094
+f 1089 1085 1094
+f 1082 1093 1094
+f 1083 1088 1095
+f 1090 1087 1095
+f 1088 1090 1095
+f 1092 1083 1095
+o convex_82
+v 0.103442 0.175135 -0.203821
+v 0.054289 0.158753 -0.271405
+v 0.054289 0.175135 -0.271405
+v 0.123927 0.156703 -0.214062
+v 0.066581 0.156703 -0.240682
+v 0.123927 0.175135 -0.214062
+v 0.103442 0.156703 -0.203821
+v 0.054289 0.175135 -0.252967
+v 0.123927 0.156703 -0.203821
+v 0.056334 0.158753 -0.271405
+v 0.123927 0.175135 -0.203821
+v 0.054289 0.162849 -0.252967
+f 1103 1097 1107
+f 1098 1096 1101
+f 1100 1099 1102
+f 1096 1098 1103
+f 1098 1097 1103
+f 1102 1096 1103
+f 1099 1101 1104
+f 1096 1102 1104
+f 1102 1099 1104
+f 1097 1098 1105
+f 1099 1100 1105
+f 1100 1097 1105
+f 1098 1101 1105
+f 1101 1099 1105
+f 1101 1096 1106
+f 1096 1104 1106
+f 1104 1101 1106
+f 1097 1100 1107
+f 1100 1102 1107
+f 1102 1103 1107
diff --git a/data/multibody.bullet b/data/multibody.bullet
index a7d7b44..70f15a2 100644
Binary files a/data/multibody.bullet and b/data/multibody.bullet differ
diff --git a/data/plane.mtl b/data/plane.mtl
index 8ce4b82..cd10152 100644
--- a/data/plane.mtl
+++ b/data/plane.mtl
@@ -1,11 +1,15 @@
-# Blender MTL File: 'None'
-# Material Count: 1
-
newmtl Material
-Ns 96.078431
-Ka 0.000000 0.000000 0.000000
-Kd 0.640000 0.640000 0.640000
-Ks 0.500000 0.500000 0.500000
-Ni 1.000000
-d 1.000000
-illum 2
+ Ns 10.0000
+ Ni 1.5000
+ d 1.0000
+ Tr 0.0000
+ Tf 1.0000 1.0000 1.0000
+ illum 2
+ Ka 0.0000 0.0000 0.0000
+ Kd 0.5880 0.5880 0.5880
+ Ks 0.0000 0.0000 0.0000
+ Ke 0.0000 0.0000 0.0000
+ map_Ka cube.tga
+ map_Kd checker_blue.png
+
+
diff --git a/data/plane.obj b/data/plane.obj
index 6434232..0b77a99 100644
--- a/data/plane.obj
+++ b/data/plane.obj
@@ -2,11 +2,17 @@
# www.blender.org
mtllib plane.mtl
o Plane
-v 1.000000 0.000000 -1.000000
-v 1.000000 0.000000 1.000000
-v -1.000000 0.000000 1.000000
-v -1.000000 0.000000 -1.000000
+v 5.000000 -5.000000 0.000000
+v 5.000000 5.000000 0.000000
+v -5.000000 5.000000 0.000000
+v -5.000000 -5.000000 0.000000
+
+vt 5.000000 0.000000
+vt 5.000000 5.000000
+vt 0.000000 5.000000
+vt 0.000000 0.000000
+
usemtl Material
s off
-f 1 2 3
-f 1 3 4
+f 1/1 2/2 3/3
+f 1/1 3/3 4/4
diff --git a/data/plane.urdf b/data/plane.urdf
new file mode 100644
index 0000000..b2c2d76
--- /dev/null
+++ b/data/plane.urdf
@@ -0,0 +1,26 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="planeLink">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value=".0"/>
+ <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="plane.obj" scale="1 1 1"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="10 10 0.001"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/plane100.obj b/data/plane100.obj
index c60a07b..3a74f59 100644
--- a/data/plane100.obj
+++ b/data/plane100.obj
@@ -2,11 +2,21 @@
# www.blender.org
mtllib plane.mtl
o Plane
-v 100.000000 0.000000 -100.000000
-v 100.000000 0.000000 100.000000
-v -100.000000 0.000000 100.000000
-v -100.000000 0.000000 -100.000000
+v 100.000000 -100.000000 0.000000
+v 100.000000 100.000000 0.000000
+v -100.000000 100.000000 0.000000
+v -100.000000 -100.000000 0.000000
+
+vt 100.000000 0.000000
+vt 100.000000 100.000000
+vt 0.000000 100.000000
+vt 0.000000 0.000000
+
+
+
usemtl Material
s off
-f 3 2 1
-f 4 3 1
+f 1/1 2/2 3/3
+f 1/1 3/3 4/4
+
+
diff --git a/data/plane100.urdf b/data/plane100.urdf
new file mode 100644
index 0000000..1c6a01c
--- /dev/null
+++ b/data/plane100.urdf
@@ -0,0 +1,26 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="baseLink">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value=".0"/>
+ <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="plane100.obj" scale="1 1 1"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="10 10 0.001"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf
new file mode 100644
index 0000000..6ca8dc6
--- /dev/null
+++ b/data/pr2_gripper.urdf
@@ -0,0 +1,142 @@
+<?xml version="1.0"?>
+<robot name="physics">
+ <link name="gripper_pole">
+ <visual>
+ <geometry>
+ <cylinder length="0.2" radius=".01"/>
+ </geometry>
+ <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+ <material name="Gray">
+ <color rgba=".7 .7 .7 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <cylinder length="0.2" radius=".01"/>
+ </geometry>
+ <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.5"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="left_gripper_joint" type="revolute">
+ <axis xyz="0 0 1"/>
+ <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+ <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
+ <parent link="gripper_pole"/>
+ <child link="left_gripper"/>
+ </joint>
+
+ <link name="left_gripper">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <spinning_friction value="1.5"/>
+ </contact>
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="l_finger_collision.stl"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="l_finger_collision.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.1"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="left_tip_joint" type="fixed">
+ <parent link="left_gripper"/>
+ <child link="left_tip"/>
+ </joint>
+
+ <link name="left_tip">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <spinning_friction value="1.5"/>
+ </contact>
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
+ <geometry>
+ <mesh filename="l_finger_tip.stl"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <box size=".03 .03 .02"/>
+ </geometry>
+ <origin rpy="0.0 0 0" xyz="0.105 0.00495 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.1"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="right_gripper_joint" type="revolute">
+ <axis xyz="0 0 -1"/>
+ <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+ <origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
+ <parent link="gripper_pole"/>
+ <child link="right_gripper"/>
+ </joint>
+
+ <link name="right_gripper">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <spinning_friction value="1.5"/>
+ </contact>
+ <visual>
+ <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="l_finger_collision.stl"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="l_finger_collision.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.1"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="right_tip_joint" type="fixed">
+ <parent link="right_gripper"/>
+ <child link="right_tip"/>
+ </joint>
+
+ <link name="right_tip">
+ <contact>
+ <lateral_friction value="1.0"/>
+ <spinning_friction value="1.5"/>
+ </contact>
+ <visual>
+ <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
+ <geometry>
+ <mesh filename="l_finger_tip.stl"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <box size=".03 .03 .02"/>
+ </geometry>
+ <origin rpy="-3.1415 0 0" xyz="0.105 0.00495 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.1"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+</robot>
diff --git a/data/r2d2.urdf b/data/r2d2.urdf
index 460be67..1d28b9a 100644
--- a/data/r2d2.urdf
+++ b/data/r2d2.urdf
@@ -79,7 +79,7 @@
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black">
- <color rgba="0 0 0 1"/>
+ <color rgba="0.5 0.5 0.5 1"/>
</material>
</visual>
<collision>
@@ -234,6 +234,7 @@
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="gripper_extension" type="prismatic">
+ <axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="gripper_pole"/>
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
@@ -387,6 +388,7 @@
<child link="head"/>
<axis xyz="0 0 1"/>
<origin xyz="0 0 0.3"/>
+<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
</joint>
<link name="box">
diff --git a/data/r2d2_multibody.bullet b/data/r2d2_multibody.bullet
index 030da51..23a4133 100644
Binary files a/data/r2d2_multibody.bullet and b/data/r2d2_multibody.bullet differ
diff --git a/data/samurai.urdf b/data/samurai.urdf
new file mode 100644
index 0000000..0a01099
--- /dev/null
+++ b/data/samurai.urdf
@@ -0,0 +1,26 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link concave="yes" name="baseLink">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value=".0"/>
+ <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+ </inertial>
+ <visual>
+ <origin rpy="1.5707963 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="samurai_monastry.obj" scale="1 1 1"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision concave="yes">
+ <origin rpy="1.5707963 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="samurai_monastry.obj" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/samurai_monastry.obj b/data/samurai_monastry.obj
index 75b2a78..435aa18 100644
--- a/data/samurai_monastry.obj
+++ b/data/samurai_monastry.obj
@@ -331416,11 +331416,3 @@ f 30335 30336
f 1329 1330
f 77715 77717
f 20619 20622
-o Cube
-v -0.500000 -0.200000 -1.000000
-v 1.000000 -0.000000 0.000000
-v -0.500000 0.200000 1.000000
-usemtl Material
-s off
-f 86188 86189 86190
-f 86189 86188 86190
diff --git a/data/slope.bullet b/data/slope.bullet
index ef8d134..fb6d74a 100644
Binary files a/data/slope.bullet and b/data/slope.bullet differ
diff --git a/data/sphere2.urdf b/data/sphere2.urdf
index 891e018..f004238 100644
--- a/data/sphere2.urdf
+++ b/data/sphere2.urdf
@@ -1,6 +1,11 @@
<?xml version="0.0" ?>
<robot name="urdf_robot">
<link name="baseLink">
+ <contact>
+ <rolling_friction value="0.03"/>
+ <spinning_friction value="0.03"/>
+ </contact>
+
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="10.0"/>
@@ -9,8 +14,11 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
- <sphere radius="0.5"/>
+ <mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/>
</geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
@@ -19,55 +27,5 @@
</geometry>
</collision>
</link>
- <link name="childA">
- <inertial>
- <origin rpy="0 0 0" xyz="0 0 0"/>
- <mass value="10.0"/>
- <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
- </inertial>
- <visual>
- <origin rpy="0 0 0" xyz="0 0 0"/>
- <geometry>
- <sphere radius="0.5"/>
- </geometry>
- </visual>
- <collision>
- <origin rpy="0 0 0" xyz="0 0 0"/>
- <geometry>
- <sphere radius="0.5"/>
- </geometry>
- </collision>
- </link>
- <joint name="joint_baseLink_childA" type="fixed">
- <parent link="baseLink"/>
- <child link="childA"/>
- <origin xyz="0 0 1.0"/>
- </joint>
- <link name="childB">
- <inertial>
- <origin rpy="0 0 0" xyz="0 0 0.5"/>
- <mass value="10.0"/>
- <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
- </inertial>
- <visual>
- <origin rpy="0 0 0" xyz="0 0 0.5"/>
- <geometry>
- <sphere radius="0.5"/>
- </geometry>
- </visual>
- <collision>
- <origin rpy="0 0 0" xyz="0 0 0.5"/>
- <geometry>
- <sphere radius="0.5"/>
- </geometry>
- </collision>
- </link>
- <joint name="joint_childA_childB" type="continuous">
- <parent link="childA"/>
- <child link="childB"/>
-<axis xyz="0 1 0"/>
- <origin xyz="0 0 0.5"/>
- </joint>
-
</robot>
diff --git a/data/sphere2_rolling_friction.urdf b/data/sphere2_rolling_friction.urdf
new file mode 100644
index 0000000..e4ff663
--- /dev/null
+++ b/data/sphere2_rolling_friction.urdf
@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="urdf_robot">
+ <link name="base_link">
+ <contact>
+ <rolling_friction value="0.3"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="10.0"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.5"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/sphere_small.urdf b/data/sphere_small.urdf
new file mode 100644
index 0000000..c5712bc
--- /dev/null
+++ b/data/sphere_small.urdf
@@ -0,0 +1,30 @@
+<?xml version="0.0" ?>
+<robot name="urdf_robot">
+ <link name="base_link">
+ <contact>
+ <rolling_friction value="0.001"/>
+ <spinning_friction value="0.001"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value=".1"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="textured_sphere_smooth.obj" scale="0.03 0.03 0.03"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.03"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/cube.mtl b/data/table/table.mtl
similarity index 76%
copy from data/cube.mtl
copy to data/table/table.mtl
index 935e48a..97d6457 100644
--- a/data/cube.mtl
+++ b/data/table/table.mtl
@@ -1,4 +1,4 @@
-newmtl cube
+newmtl table
Ns 10.0000
Ni 1.5000
d 1.0000
@@ -9,6 +9,8 @@ newmtl cube
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
- map_Ka cube.png
- map_Kd cube.png
+ map_Ka table.tga
+ map_Kd table.png
+
+
diff --git a/data/table/table.obj b/data/table/table.obj
new file mode 100644
index 0000000..1357e49
--- /dev/null
+++ b/data/table/table.obj
@@ -0,0 +1,48 @@
+# table.obj
+#
+
+o table
+mtllib table.mtl
+
+v -0.500000 -0.500000 0.500000
+v 0.500000 -0.500000 0.500000
+v -0.500000 0.500000 0.500000
+v 0.500000 0.500000 0.500000
+v -0.500000 0.500000 -0.500000
+v 0.500000 0.500000 -0.500000
+v -0.500000 -0.500000 -0.500000
+v 0.500000 -0.500000 -0.500000
+
+vt 0.000000 0.000000
+vt 1.000000 0.000000
+vt 0.000000 1.000000
+vt 1.000000 1.000000
+
+vn 0.000000 0.000000 1.000000
+vn 0.000000 1.000000 0.000000
+vn 0.000000 0.000000 -1.000000
+vn 0.000000 -1.000000 0.000000
+vn 1.000000 0.000000 0.000000
+vn -1.000000 0.000000 0.000000
+
+g table
+usemtl table
+s 1
+f 1/1/1 2/2/1 3/3/1
+f 3/3/1 2/2/1 4/4/1
+s 2
+f 3/1/2 4/2/2 5/3/2
+f 5/3/2 4/2/2 6/4/2
+s 3
+f 5/4/3 6/3/3 7/2/3
+f 7/2/3 6/3/3 8/1/3
+s 4
+f 7/1/4 8/2/4 1/3/4
+f 1/3/4 8/2/4 2/4/4
+s 5
+f 2/1/5 8/2/5 4/3/5
+f 4/3/5 8/2/5 6/4/5
+s 6
+f 7/1/6 1/2/6 5/3/6
+f 5/3/6 1/2/6 3/4/6
+
diff --git a/data/table/table.png b/data/table/table.png
new file mode 100644
index 0000000..cdc3913
Binary files /dev/null and b/data/table/table.png differ
diff --git a/data/table/table.urdf b/data/table/table.urdf
new file mode 100644
index 0000000..5a923a7
--- /dev/null
+++ b/data/table/table.urdf
@@ -0,0 +1,56 @@
+<?xml version="0.0" ?>
+<robot name="table.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="1.0"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value=".0"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0.6"/>
+ <geometry>
+ <mesh filename="table.obj" scale="1.5 1 0.05"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0.6"/>
+ <geometry>
+ <box size="1.5 1 0.05"/>
+ </geometry>
+ </collision>
+ <visual>
+ <origin rpy="0 0 0" xyz="-0.65 -0.4 0.29"/>
+ <geometry>
+ <mesh filename="table.obj" scale="0.1 0.1 0.58"/>
+ </geometry>
+ <material name="framemat0"/>
+ </visual>
+ <visual>
+ <origin rpy="0 0 0" xyz="-0.65 0.4 0.29"/>
+ <geometry>
+ <mesh filename="table.obj" scale="0.1 0.1 0.58"/>
+ </geometry>
+ <material name="framemat0"/>
+ </visual>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.65 -0.4 0.29"/>
+ <geometry>
+ <mesh filename="table.obj" scale="0.1 0.1 0.58"/>
+ </geometry>
+ <material name="framemat0"/>
+ </visual>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.65 0.4 0.29"/>
+ <geometry>
+ <mesh filename="table.obj" scale="0.1 0.1 0.58"/>
+ </geometry>
+ </visual>
+ </link>
+</robot>
+
diff --git a/data/table_square/checker_grid.jpg b/data/table_square/checker_grid.jpg
new file mode 100644
index 0000000..4165504
Binary files /dev/null and b/data/table_square/checker_grid.jpg differ
diff --git a/data/cube.mtl b/data/table_square/table.mtl
similarity index 74%
copy from data/cube.mtl
copy to data/table_square/table.mtl
index 935e48a..a3bc516 100644
--- a/data/cube.mtl
+++ b/data/table_square/table.mtl
@@ -1,4 +1,4 @@
-newmtl cube
+newmtl table
Ns 10.0000
Ni 1.5000
d 1.0000
@@ -9,6 +9,8 @@ newmtl cube
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
- map_Ka cube.png
- map_Kd cube.png
+ map_Ka table.tga
+ map_Kd checker_grid.jpg
+
+
diff --git a/data/table_square/table.obj b/data/table_square/table.obj
new file mode 100644
index 0000000..6ec6d98
--- /dev/null
+++ b/data/table_square/table.obj
@@ -0,0 +1,48 @@
+# table.obj
+#
+
+o table
+mtllib table.mtl
+
+v -0.500000 -0.500000 0.500000
+v 0.500000 -0.500000 0.500000
+v -0.500000 0.500000 0.500000
+v 0.500000 0.500000 0.500000
+v -0.500000 0.500000 -0.500000
+v 0.500000 0.500000 -0.500000
+v -0.500000 -0.500000 -0.500000
+v 0.500000 -0.500000 -0.500000
+
+vt 0.000000 0.000000
+vt 1.000000 0.000000
+vt 0.000000 1.000000
+vt 1.000000 1.000000
+
+vn 0.000000 0.000000 1.000000
+vn 0.000000 1.000000 0.000000
+vn 0.000000 0.000000 -1.000000
+vn 0.000000 -1.000000 0.000000
+vn 1.000000 0.000000 0.000000
+vn -1.000000 0.000000 0.000000
+
+g table
+usemtl table
+s 1
+f 1/1/1 2/2/1 3/3/1
+f 3/3/1 2/2/1 4/4/1
+s 2
+f 3 4 5
+f 5 4 6
+s 3
+f 5 6 7
+f 7 6 8
+s 4
+f 7 8 1
+f 1 8 2
+s 5
+f 2 8 4
+f 4 8 6
+s 6
+f 7 1 5
+f 5 1 3
+
diff --git a/data/table_square/table_square.urdf b/data/table_square/table_square.urdf
new file mode 100644
index 0000000..920d979
--- /dev/null
+++ b/data/table_square/table_square.urdf
@@ -0,0 +1,61 @@
+<?xml version="0.0" ?>
+<robot name="urdf_table">
+ <link name="world"/>
+ <joint name="fixed" type="fixed">
+ <parent link="world"/>
+ <child link="baseLink"/>
+ <origin xyz="0 0 0"/>
+ </joint>
+
+ <link name="baseLink">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0.6"/>
+ <mass value="0"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0.6"/>
+ <geometry>
+ <mesh filename="table.obj" scale="0.6 0.6 0.08"/>
+ </geometry>
+ <material name="framemat0">
+ <color
+ rgba="1 1 1 1" />
+ </material>
+ </visual>
+ <visual>
+ <origin rpy="0 0 0" xyz="-0.25 -0.25 0.28"/>
+ <geometry>
+ <box size="0.05 0.05 0.56"/>
+ </geometry>
+ <material name="framemat0"/>
+ </visual>
+ <visual>
+ <origin rpy="0 0 0" xyz="-0.25 0.25 0.28"/>
+ <geometry>
+ <box size="0.05 0.05 0.56"/>
+ </geometry>
+ <material name="framemat0"/>
+ </visual>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.25 -0.25 0.28"/>
+ <geometry>
+ <box size="0.05 0.05 0.56"/>
+ </geometry>
+ <material name="framemat0"/>
+ </visual>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.25 0.25 0.28"/>
+ <geometry>
+ <box size="0.05 0.05 0.56"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0.6"/>
+ <geometry>
+ <box size="0.6 0.6 0.08"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/teddy_vhacd.urdf b/data/teddy_vhacd.urdf
new file mode 100644
index 0000000..dd6cb2c
--- /dev/null
+++ b/data/teddy_vhacd.urdf
@@ -0,0 +1,32 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+ <link name="baseLink">
+ <contact>
+ <lateral_friction value="0.5"/>
+ <rolling_friction value="0.0"/>
+ <contact_cfm value="0.0"/>
+ <contact_erp value="1.0"/>
+ </contact>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.07 0.05 0.03"/>
+ <mass value=".1"/>
+ <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="teddy2_VHACD_CHs.obj" scale=".1 .1 .1"/>
+ </geometry>
+ <material name="red">
+ <color rgba="1 0.4 0.4 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="teddy2_VHACD_CHs.obj" scale=".1 .1 .1"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
+
diff --git a/data/textured_sphere.mtl b/data/textured_sphere.mtl
new file mode 100644
index 0000000..2eee641
--- /dev/null
+++ b/data/textured_sphere.mtl
@@ -0,0 +1,11 @@
+# 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
+map_Kd checker_grid.jpg
diff --git a/data/textured_sphere_flat.obj b/data/textured_sphere_flat.obj
new file mode 100644
index 0000000..0c9fc75
--- /dev/null
+++ b/data/textured_sphere_flat.obj
@@ -0,0 +1,3755 @@
+# Blender v2.77 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib textured_sphere.mtl
+o Sphere_Sphere.001
+v -0.108119 0.994138 0.000000
+v -0.214970 0.976621 0.000000
+v -0.319302 0.947653 0.000000
+v -0.419889 0.907575 0.000000
+v -0.515554 0.856857 0.000000
+v -0.605174 0.796093 0.000000
+v -0.687699 0.725995 0.000000
+v -0.762162 0.647386 0.000000
+v -0.827689 0.561187 0.000000
+v -0.883512 0.468408 0.000000
+v -0.928977 0.370138 0.000000
+v -0.963550 0.267528 0.000000
+v -0.986827 0.161782 0.000000
+v -0.998533 0.054139 0.000000
+v -0.998533 -0.054139 0.000000
+v -0.986827 -0.161782 0.000000
+v -0.963550 -0.267528 0.000000
+v -0.928977 -0.370138 0.000000
+v -0.883512 -0.468408 0.000000
+v -0.827689 -0.561187 0.000000
+v -0.762162 -0.647386 0.000000
+v -0.687700 -0.725995 0.000000
+v -0.605175 -0.796093 0.000000
+v -0.515554 -0.856857 0.000000
+v -0.419890 -0.907575 0.000000
+v -0.319302 -0.947653 0.000000
+v -0.214971 -0.976620 0.000000
+v -0.108120 -0.994138 0.000000
+v -0.106042 0.994138 -0.021093
+v -0.210840 0.976621 -0.041938
+v -0.313166 0.947653 -0.062292
+v -0.411821 0.907575 -0.081916
+v -0.505648 0.856857 -0.100579
+v -0.593546 0.796093 -0.118063
+v -0.674486 0.725995 -0.134163
+v -0.747517 0.647386 -0.148690
+v -0.811785 0.561187 -0.161474
+v -0.866536 0.468408 -0.172364
+v -0.911127 0.370138 -0.181234
+v -0.945036 0.267528 -0.187979
+v -0.967865 0.161782 -0.192520
+v -0.979347 0.054139 -0.194804
+v -0.979347 -0.054139 -0.194804
+v -0.967865 -0.161782 -0.192520
+v -0.945036 -0.267528 -0.187979
+v -0.911127 -0.370138 -0.181234
+v -0.866536 -0.468408 -0.172365
+v -0.811785 -0.561187 -0.161474
+v -0.747518 -0.647386 -0.148690
+v -0.674486 -0.725995 -0.134163
+v -0.593546 -0.796093 -0.118064
+v -0.505648 -0.856857 -0.100580
+v -0.411822 -0.907575 -0.081916
+v -0.313167 -0.947653 -0.062293
+v -0.210841 -0.976620 -0.041939
+v -0.106042 -0.994138 -0.021093
+v -0.099889 0.994138 -0.041375
+v -0.198607 0.976621 -0.082265
+v -0.294996 0.947653 -0.122191
+v -0.387927 0.907575 -0.160684
+v -0.476310 0.856857 -0.197294
+v -0.559108 0.796093 -0.231590
+v -0.635351 0.725995 -0.263171
+v -0.704146 0.647386 -0.291666
+v -0.764685 0.561187 -0.316743
+v -0.816259 0.468408 -0.338105
+v -0.858263 0.370138 -0.355504
+v -0.890204 0.267528 -0.368734
+v -0.911709 0.161782 -0.377642
+v -0.922525 0.054139 -0.382122
+v -0.922525 -0.054139 -0.382122
+v -0.911709 -0.161782 -0.377642
+v -0.890204 -0.267528 -0.368734
+v -0.858263 -0.370138 -0.355504
+v -0.816259 -0.468408 -0.338105
+v -0.764685 -0.561187 -0.316743
+v -0.704146 -0.647386 -0.291667
+v -0.635352 -0.725995 -0.263171
+v -0.559108 -0.796093 -0.231590
+v -0.476310 -0.856857 -0.197294
+v -0.387928 -0.907575 -0.160684
+v -0.294997 -0.947653 -0.122191
+v -0.198608 -0.976620 -0.082266
+v -0.099890 -0.994138 -0.041375
+v -0.089898 0.994138 -0.060067
+v -0.178742 0.976621 -0.119431
+v -0.265490 0.947653 -0.177394
+v -0.349125 0.907575 -0.233277
+v -0.428668 0.856857 -0.286426
+v -0.503184 0.796093 -0.336216
+v -0.571801 0.725995 -0.382065
+v -0.633715 0.647386 -0.423434
+v -0.688198 0.561187 -0.459839
+v -0.734614 0.468408 -0.490852
+v -0.772416 0.370138 -0.516111
+v -0.801163 0.267528 -0.535319
+v -0.820516 0.161782 -0.548251
+v -0.830250 0.054139 -0.554755
+v -0.830250 -0.054139 -0.554755
+v -0.820516 -0.161782 -0.548251
+v -0.801163 -0.267528 -0.535319
+v -0.772416 -0.370138 -0.516111
+v -0.734614 -0.468408 -0.490853
+v -0.688198 -0.561187 -0.459839
+v -0.633715 -0.647386 -0.423434
+v -0.571801 -0.725995 -0.382065
+v -0.503184 -0.796093 -0.336217
+v -0.428668 -0.856857 -0.286426
+v -0.349126 -0.907575 -0.233278
+v -0.265490 -0.947653 -0.177394
+v -0.178742 -0.976620 -0.119431
+v -0.089898 -0.994138 -0.060068
+v -0.076452 0.994138 -0.076451
+v -0.152007 0.976621 -0.152006
+v -0.225781 0.947653 -0.225780
+v -0.296907 0.907575 -0.296906
+v -0.364552 0.856857 -0.364551
+v -0.427923 0.796093 -0.427922
+v -0.486277 0.725995 -0.486276
+v -0.538930 0.647386 -0.538929
+v -0.585265 0.561187 -0.585264
+v -0.624738 0.468408 -0.624737
+v -0.656886 0.370138 -0.656885
+v -0.681333 0.267528 -0.681332
+v -0.697792 0.161782 -0.697791
+v -0.706070 0.054139 -0.706069
+v -0.706070 -0.054139 -0.706069
+v -0.697792 -0.161782 -0.697791
+v -0.681333 -0.267528 -0.681332
+v -0.656886 -0.370138 -0.656885
+v -0.624738 -0.468408 -0.624737
+v -0.585265 -0.561187 -0.585264
+v -0.538930 -0.647386 -0.538929
+v -0.486277 -0.725995 -0.486276
+v -0.427923 -0.796093 -0.427922
+v -0.364552 -0.856857 -0.364551
+v -0.296907 -0.907575 -0.296906
+v -0.225781 -0.947653 -0.225780
+v -0.152008 -0.976620 -0.152007
+v -0.076452 -0.994138 -0.076452
+v -0.060068 0.994138 -0.089897
+v -0.119432 0.976621 -0.178741
+v -0.177395 0.947653 -0.265489
+v -0.233278 0.907575 -0.349124
+v -0.286427 0.856857 -0.428667
+v -0.336217 0.796093 -0.503183
+v -0.382066 0.725995 -0.571800
+v -0.423435 0.647386 -0.633714
+v -0.459840 0.561187 -0.688197
+v -0.490853 0.468408 -0.734613
+v -0.516112 0.370138 -0.772415
+v -0.535320 0.267528 -0.801162
+v -0.548252 0.161782 -0.820515
+v -0.554756 0.054139 -0.830249
+v -0.554756 -0.054139 -0.830249
+v -0.548252 -0.161782 -0.820515
+v -0.535320 -0.267528 -0.801162
+v -0.516112 -0.370138 -0.772415
+v -0.490853 -0.468408 -0.734613
+v -0.459840 -0.561187 -0.688197
+v -0.423435 -0.647386 -0.633714
+v -0.382066 -0.725995 -0.571801
+v -0.336217 -0.796093 -0.503183
+v -0.286427 -0.856857 -0.428667
+v -0.233279 -0.907575 -0.349125
+v -0.177395 -0.947653 -0.265489
+v -0.119432 -0.976620 -0.178741
+v -0.060069 -0.994138 -0.089898
+v -0.041376 0.994138 -0.099888
+v -0.082266 0.976621 -0.198606
+v -0.122192 0.947653 -0.294995
+v -0.160685 0.907575 -0.387926
+v -0.197294 0.856857 -0.476309
+v -0.231591 0.796093 -0.559107
+v -0.263172 0.725995 -0.635351
+v -0.291667 0.647386 -0.704145
+v -0.316743 0.561187 -0.764684
+v -0.338106 0.468408 -0.816258
+v -0.355504 0.370138 -0.858262
+v -0.368735 0.267528 -0.890203
+v -0.377643 0.161782 -0.911708
+v -0.382123 0.054139 -0.922524
+v -0.382123 -0.054139 -0.922524
+v -0.377643 -0.161782 -0.911708
+v -0.368735 -0.267528 -0.890203
+v -0.355504 -0.370138 -0.858262
+v -0.338106 -0.468408 -0.816258
+v -0.316743 -0.561187 -0.764684
+v -0.291667 -0.647386 -0.704145
+v -0.263172 -0.725995 -0.635351
+v -0.231591 -0.796093 -0.559107
+v -0.197295 -0.856857 -0.476309
+v -0.160685 -0.907575 -0.387927
+v -0.122192 -0.947653 -0.294996
+v -0.082266 -0.976620 -0.198607
+v -0.041376 -0.994138 -0.099889
+v -0.021094 0.994138 -0.106041
+v -0.041939 0.976621 -0.210839
+v -0.062293 0.947653 -0.313165
+v -0.081917 0.907575 -0.411820
+v -0.100580 0.856857 -0.505647
+v -0.118064 0.796093 -0.593545
+v -0.134164 0.725995 -0.674484
+v -0.148691 0.647386 -0.747516
+v -0.161475 0.561187 -0.811784
+v -0.172365 0.468408 -0.866535
+v -0.181235 0.370138 -0.911126
+v -0.187980 0.267528 -0.945035
+v -0.192521 0.161782 -0.967864
+v -0.194805 0.054139 -0.979346
+v -0.194805 -0.054139 -0.979346
+v -0.192521 -0.161782 -0.967864
+v -0.187980 -0.267528 -0.945035
+v -0.181235 -0.370138 -0.911126
+v -0.172365 -0.468408 -0.866535
+v -0.161475 -0.561187 -0.811784
+v -0.148691 -0.647386 -0.747517
+v -0.134164 -0.725995 -0.674485
+v -0.118064 -0.796093 -0.593545
+v -0.100580 -0.856857 -0.505647
+v -0.081917 -0.907575 -0.411821
+v -0.062293 -0.947653 -0.313166
+v -0.041939 -0.976620 -0.210840
+v -0.021094 -0.994138 -0.106041
+v -0.000001 0.994138 -0.108118
+v -0.000001 0.976621 -0.214970
+v -0.000001 0.947653 -0.319301
+v -0.000001 0.907575 -0.419888
+v -0.000001 0.856857 -0.515553
+v -0.000001 0.796093 -0.605173
+v -0.000001 0.725995 -0.687698
+v -0.000001 0.647386 -0.762161
+v -0.000001 0.561187 -0.827688
+v -0.000001 0.468408 -0.883511
+v -0.000001 0.370138 -0.928976
+v -0.000001 0.267528 -0.963549
+v -0.000001 0.161782 -0.986825
+v -0.000001 0.054139 -0.998532
+v -0.000001 -0.054139 -0.998532
+v -0.000001 -0.161782 -0.986825
+v -0.000001 -0.267528 -0.963549
+v -0.000001 -0.370138 -0.928976
+v -0.000001 -0.468408 -0.883511
+v -0.000001 -0.561187 -0.827688
+v -0.000001 -0.647386 -0.762161
+v -0.000001 -0.725995 -0.687699
+v -0.000001 -0.796093 -0.605174
+v -0.000001 -0.856857 -0.515553
+v -0.000001 -0.907575 -0.419889
+v -0.000001 -0.947653 -0.319301
+v -0.000001 -0.976620 -0.214970
+v -0.000001 -0.994138 -0.108119
+v 0.021092 0.994138 -0.106041
+v 0.041938 0.976621 -0.210839
+v 0.062292 0.947653 -0.313165
+v 0.081915 0.907575 -0.411820
+v 0.100579 0.856857 -0.505647
+v 0.118063 0.796093 -0.593545
+v 0.134162 0.725995 -0.674484
+v 0.148689 0.647386 -0.747516
+v 0.161473 0.561187 -0.811784
+v 0.172364 0.468408 -0.866535
+v 0.181233 0.370138 -0.911126
+v 0.187978 0.267528 -0.945035
+v 0.192519 0.161782 -0.967864
+v 0.194803 0.054139 -0.979346
+v 0.194803 -0.054139 -0.979346
+v 0.192519 -0.161782 -0.967864
+v 0.187978 -0.267528 -0.945035
+v 0.181233 -0.370138 -0.911126
+v 0.172364 -0.468408 -0.866535
+v 0.161473 -0.561187 -0.811784
+v 0.148689 -0.647386 -0.747516
+v 0.134162 -0.725995 -0.674485
+v 0.118063 -0.796093 -0.593545
+v 0.100579 -0.856857 -0.505647
+v 0.081915 -0.907575 -0.411821
+v 0.062292 -0.947653 -0.313166
+v 0.041938 -0.976620 -0.210840
+v 0.021092 -0.994138 -0.106041
+v 0.041374 0.994138 -0.099888
+v 0.082264 0.976621 -0.198606
+v 0.122190 0.947653 -0.294995
+v 0.160683 0.907575 -0.387926
+v 0.197293 0.856857 -0.476309
+v 0.231589 0.796093 -0.559107
+v 0.263170 0.725995 -0.635350
+v 0.291665 0.647386 -0.704145
+v 0.316742 0.561187 -0.764684
+v 0.338104 0.468408 -0.816258
+v 0.355503 0.370138 -0.858261
+v 0.368733 0.267528 -0.890203
+v 0.377641 0.161782 -0.911708
+v 0.382121 0.054139 -0.922523
+v 0.382121 -0.054139 -0.922523
+v 0.377641 -0.161782 -0.911708
+v 0.368733 -0.267528 -0.890203
+v 0.355503 -0.370138 -0.858261
+v 0.338104 -0.468408 -0.816258
+v 0.316742 -0.561187 -0.764684
+v 0.291666 -0.647386 -0.704145
+v 0.263170 -0.725995 -0.635351
+v 0.231589 -0.796093 -0.559107
+v 0.197293 -0.856857 -0.476309
+v 0.160684 -0.907575 -0.387927
+v 0.122190 -0.947653 -0.294996
+v 0.082265 -0.976620 -0.198607
+v 0.041374 -0.994138 -0.099889
+v 0.060066 0.994138 -0.089897
+v 0.119430 0.976621 -0.178741
+v 0.177393 0.947653 -0.265489
+v 0.233276 0.907575 -0.349124
+v 0.286425 0.856857 -0.428667
+v 0.336215 0.796093 -0.503183
+v 0.382064 0.725995 -0.571800
+v 0.423433 0.647386 -0.633714
+v 0.459838 0.561187 -0.688197
+v 0.490851 0.468408 -0.734612
+v 0.516110 0.370138 -0.772415
+v 0.535318 0.267528 -0.801161
+v 0.548250 0.161782 -0.820515
+v 0.554754 0.054139 -0.830249
+v 0.554754 -0.054139 -0.830249
+v 0.548250 -0.161782 -0.820515
+v 0.535318 -0.267528 -0.801161
+v 0.516110 -0.370138 -0.772415
+v 0.490851 -0.468408 -0.734612
+v 0.459838 -0.561187 -0.688197
+v 0.423433 -0.647386 -0.633714
+v 0.382064 -0.725995 -0.571800
+v 0.336216 -0.796093 -0.503183
+v 0.286425 -0.856857 -0.428667
+v 0.233277 -0.907575 -0.349125
+v 0.177393 -0.947653 -0.265489
+v 0.119430 -0.976620 -0.178741
+v 0.060067 -0.994138 -0.089898
+v -0.000001 1.000000 0.000001
+v 0.076450 0.994138 -0.076451
+v 0.152006 0.976621 -0.152006
+v 0.225779 0.947653 -0.225780
+v 0.296905 0.907575 -0.296906
+v 0.364550 0.856857 -0.364551
+v 0.427921 0.796093 -0.427922
+v 0.486275 0.725995 -0.486276
+v 0.538928 0.647386 -0.538929
+v 0.585263 0.561187 -0.585264
+v 0.624736 0.468408 -0.624736
+v 0.656884 0.370138 -0.656885
+v 0.681331 0.267528 -0.681332
+v 0.697790 0.161782 -0.697791
+v 0.706068 0.054139 -0.706069
+v 0.706068 -0.054139 -0.706069
+v 0.697790 -0.161782 -0.697791
+v 0.681331 -0.267528 -0.681332
+v 0.656884 -0.370138 -0.656885
+v 0.624736 -0.468408 -0.624736
+v 0.585263 -0.561187 -0.585264
+v 0.538929 -0.647386 -0.538929
+v 0.486275 -0.725995 -0.486276
+v 0.427921 -0.796093 -0.427922
+v 0.364550 -0.856857 -0.364551
+v 0.296905 -0.907575 -0.296906
+v 0.225779 -0.947653 -0.225780
+v 0.152006 -0.976620 -0.152007
+v 0.076451 -0.994138 -0.076452
+v -0.000001 -1.000000 0.000000
+v 0.089896 0.994138 -0.060067
+v 0.178740 0.976621 -0.119431
+v 0.265488 0.947653 -0.177394
+v 0.349123 0.907575 -0.233277
+v 0.428666 0.856857 -0.286426
+v 0.503182 0.796093 -0.336216
+v 0.571799 0.725995 -0.382065
+v 0.633713 0.647386 -0.423434
+v 0.688196 0.561187 -0.459839
+v 0.734612 0.468408 -0.490852
+v 0.772414 0.370138 -0.516111
+v 0.801161 0.267528 -0.535319
+v 0.820514 0.161782 -0.548251
+v 0.830248 0.054139 -0.554755
+v 0.830248 -0.054139 -0.554755
+v 0.820514 -0.161782 -0.548251
+v 0.801161 -0.267528 -0.535319
+v 0.772414 -0.370138 -0.516111
+v 0.734612 -0.468408 -0.490852
+v 0.688197 -0.561187 -0.459839
+v 0.633713 -0.647386 -0.423434
+v 0.571800 -0.725995 -0.382065
+v 0.503182 -0.796093 -0.336216
+v 0.428666 -0.856857 -0.286426
+v 0.349124 -0.907575 -0.233278
+v 0.265488 -0.947653 -0.177394
+v 0.178740 -0.976620 -0.119431
+v 0.089897 -0.994138 -0.060068
+v 0.099887 0.994138 -0.041375
+v 0.198605 0.976621 -0.082265
+v 0.294994 0.947653 -0.122191
+v 0.387925 0.907575 -0.160684
+v 0.476308 0.856857 -0.197294
+v 0.559106 0.796093 -0.231590
+v 0.635349 0.725995 -0.263171
+v 0.704144 0.647386 -0.291666
+v 0.764683 0.561187 -0.316742
+v 0.816257 0.468408 -0.338105
+v 0.858261 0.370138 -0.355503
+v 0.890202 0.267528 -0.368734
+v 0.911707 0.161782 -0.377642
+v 0.922523 0.054139 -0.382122
+v 0.922523 -0.054139 -0.382122
+v 0.911707 -0.161782 -0.377642
+v 0.890202 -0.267528 -0.368734
+v 0.858261 -0.370138 -0.355503
+v 0.816257 -0.468408 -0.338105
+v 0.764683 -0.561187 -0.316742
+v 0.704144 -0.647386 -0.291666
+v 0.635350 -0.725995 -0.263171
+v 0.559106 -0.796093 -0.231590
+v 0.476308 -0.856857 -0.197294
+v 0.387926 -0.907575 -0.160684
+v 0.294995 -0.947653 -0.122191
+v 0.198606 -0.976620 -0.082266
+v 0.099888 -0.994138 -0.041375
+v 0.106040 0.994138 -0.021093
+v 0.210838 0.976621 -0.041938
+v 0.313164 0.947653 -0.062292
+v 0.411819 0.907575 -0.081916
+v 0.505646 0.856857 -0.100579
+v 0.593544 0.796093 -0.118063
+v 0.674483 0.725995 -0.134163
+v 0.747515 0.647386 -0.148690
+v 0.811783 0.561187 -0.161474
+v 0.866534 0.468408 -0.172364
+v 0.911125 0.370138 -0.181234
+v 0.945034 0.267528 -0.187979
+v 0.967863 0.161782 -0.192520
+v 0.979345 0.054139 -0.194804
+v 0.979345 -0.054139 -0.194804
+v 0.967863 -0.161782 -0.192520
+v 0.945034 -0.267528 -0.187979
+v 0.911125 -0.370138 -0.181234
+v 0.866534 -0.468408 -0.172364
+v 0.811783 -0.561187 -0.161474
+v 0.747516 -0.647386 -0.148690
+v 0.674484 -0.725995 -0.134163
+v 0.593544 -0.796093 -0.118063
+v 0.505646 -0.856857 -0.100579
+v 0.411820 -0.907575 -0.081916
+v 0.313165 -0.947653 -0.062293
+v 0.210839 -0.976620 -0.041939
+v 0.106041 -0.994138 -0.021093
+v 0.108117 0.994138 0.000000
+v 0.214969 0.976621 0.000000
+v 0.319300 0.947653 0.000000
+v 0.419887 0.907575 0.000000
+v 0.515552 0.856857 -0.000000
+v 0.605172 0.796093 0.000000
+v 0.687697 0.725995 -0.000000
+v 0.762160 0.647386 0.000000
+v 0.827687 0.561187 0.000000
+v 0.883510 0.468408 0.000000
+v 0.928975 0.370138 0.000000
+v 0.963548 0.267528 0.000000
+v 0.986824 0.161782 0.000000
+v 0.998531 0.054139 0.000000
+v 0.998531 -0.054139 0.000000
+v 0.986824 -0.161782 0.000000
+v 0.963548 -0.267528 0.000000
+v 0.928975 -0.370138 0.000000
+v 0.883510 -0.468408 0.000000
+v 0.827687 -0.561187 0.000000
+v 0.762160 -0.647386 0.000000
+v 0.687698 -0.725995 0.000000
+v 0.605173 -0.796093 0.000000
+v 0.515552 -0.856857 0.000000
+v 0.419888 -0.907575 0.000000
+v 0.319300 -0.947653 -0.000000
+v 0.214969 -0.976620 0.000000
+v 0.108118 -0.994138 0.000000
+v 0.106040 0.994138 0.021093
+v 0.210838 0.976621 0.041939
+v 0.313164 0.947653 0.062292
+v 0.411819 0.907575 0.081916
+v 0.505646 0.856857 0.100579
+v 0.593544 0.796093 0.118063
+v 0.674483 0.725995 0.134163
+v 0.747515 0.647386 0.148690
+v 0.811783 0.561187 0.161474
+v 0.866534 0.468408 0.172364
+v 0.911125 0.370138 0.181234
+v 0.945033 0.267528 0.187979
+v 0.967863 0.161782 0.192520
+v 0.979345 0.054139 0.194804
+v 0.979345 -0.054139 0.194804
+v 0.967863 -0.161782 0.192520
+v 0.945033 -0.267528 0.187979
+v 0.911125 -0.370138 0.181234
+v 0.866534 -0.468408 0.172364
+v 0.811783 -0.561187 0.161474
+v 0.747515 -0.647386 0.148690
+v 0.674484 -0.725995 0.134163
+v 0.593544 -0.796093 0.118064
+v 0.505646 -0.856857 0.100579
+v 0.411820 -0.907575 0.081916
+v 0.313165 -0.947653 0.062293
+v 0.210839 -0.976620 0.041939
+v 0.106041 -0.994138 0.021093
+v 0.099887 0.994138 0.041375
+v 0.198605 0.976621 0.082265
+v 0.294994 0.947653 0.122191
+v 0.387925 0.907575 0.160684
+v 0.476308 0.856857 0.197293
+v 0.559106 0.796093 0.231590
+v 0.635349 0.725995 0.263171
+v 0.704144 0.647386 0.291666
+v 0.764683 0.561187 0.316742
+v 0.816257 0.468408 0.338105
+v 0.858260 0.370138 0.355504
+v 0.890202 0.267528 0.368734
+v 0.911707 0.161782 0.377642
+v 0.922522 0.054139 0.382122
+v 0.922522 -0.054139 0.382122
+v 0.911707 -0.161782 0.377642
+v 0.890202 -0.267528 0.368734
+v 0.858260 -0.370138 0.355504
+v 0.816257 -0.468408 0.338105
+v 0.764683 -0.561187 0.316743
+v 0.704144 -0.647386 0.291667
+v 0.635350 -0.725995 0.263171
+v 0.559106 -0.796093 0.231590
+v 0.476308 -0.856857 0.197294
+v 0.387926 -0.907575 0.160684
+v 0.294995 -0.947653 0.122191
+v 0.198606 -0.976620 0.082266
+v 0.099888 -0.994138 0.041375
+v 0.089896 0.994138 0.060067
+v 0.178740 0.976621 0.119431
+v 0.265488 0.947653 0.177394
+v 0.349123 0.907575 0.233277
+v 0.428666 0.856857 0.286426
+v 0.503182 0.796093 0.336216
+v 0.571799 0.725995 0.382065
+v 0.633713 0.647386 0.423434
+v 0.688196 0.561187 0.459839
+v 0.734612 0.468408 0.490852
+v 0.772414 0.370138 0.516111
+v 0.801161 0.267528 0.535319
+v 0.820514 0.161782 0.548251
+v 0.830248 0.054139 0.554755
+v 0.830248 -0.054139 0.554755
+v 0.820514 -0.161782 0.548251
+v 0.801161 -0.267528 0.535319
+v 0.772414 -0.370138 0.516111
+v 0.734612 -0.468408 0.490852
+v 0.688196 -0.561187 0.459839
+v 0.633713 -0.647386 0.423434
+v 0.571800 -0.725995 0.382065
+v 0.503182 -0.796093 0.336216
+v 0.428666 -0.856857 0.286426
+v 0.349124 -0.907575 0.233278
+v 0.265488 -0.947653 0.177394
+v 0.178740 -0.976620 0.119431
+v 0.089897 -0.994138 0.060068
+v 0.076450 0.994138 0.076451
+v 0.152006 0.976621 0.152006
+v 0.225779 0.947653 0.225779
+v 0.296905 0.907575 0.296906
+v 0.364550 0.856857 0.364551
+v 0.427921 0.796093 0.427922
+v 0.486275 0.725995 0.486276
+v 0.538928 0.647386 0.538929
+v 0.585263 0.561187 0.585264
+v 0.624736 0.468408 0.624736
+v 0.656884 0.370138 0.656885
+v 0.681331 0.267528 0.681332
+v 0.697790 0.161782 0.697791
+v 0.706068 0.054139 0.706069
+v 0.706068 -0.054139 0.706069
+v 0.697790 -0.161782 0.697791
+v 0.681331 -0.267528 0.681332
+v 0.656884 -0.370138 0.656885
+v 0.624736 -0.468408 0.624736
+v 0.585263 -0.561187 0.585264
+v 0.538928 -0.647386 0.538929
+v 0.486275 -0.725995 0.486276
+v 0.427921 -0.796093 0.427922
+v 0.364550 -0.856857 0.364551
+v 0.296905 -0.907575 0.296906
+v 0.225779 -0.947653 0.225780
+v 0.152006 -0.976620 0.152007
+v 0.076451 -0.994138 0.076452
+v 0.060066 0.994138 0.089897
+v 0.119430 0.976621 0.178741
+v 0.177393 0.947653 0.265489
+v 0.233276 0.907575 0.349124
+v 0.286425 0.856857 0.428666
+v 0.336215 0.796093 0.503183
+v 0.382064 0.725995 0.571800
+v 0.423433 0.647386 0.633714
+v 0.459838 0.561187 0.688197
+v 0.490851 0.468408 0.734612
+v 0.516110 0.370138 0.772415
+v 0.535318 0.267528 0.801162
+v 0.548250 0.161782 0.820515
+v 0.554754 0.054139 0.830249
+v 0.554754 -0.054139 0.830249
+v 0.548250 -0.161782 0.820515
+v 0.535318 -0.267528 0.801162
+v 0.516110 -0.370138 0.772415
+v 0.490851 -0.468408 0.734612
+v 0.459838 -0.561187 0.688197
+v 0.423433 -0.647386 0.633714
+v 0.382064 -0.725995 0.571800
+v 0.336215 -0.796093 0.503183
+v 0.286425 -0.856857 0.428667
+v 0.233277 -0.907575 0.349125
+v 0.177393 -0.947653 0.265489
+v 0.119430 -0.976620 0.178741
+v 0.060067 -0.994138 0.089898
+v 0.041374 0.994138 0.099888
+v 0.082264 0.976621 0.198606
+v 0.122190 0.947653 0.294995
+v 0.160683 0.907575 0.387926
+v 0.197293 0.856857 0.476309
+v 0.231589 0.796093 0.559107
+v 0.263170 0.725995 0.635350
+v 0.291665 0.647386 0.704145
+v 0.316741 0.561187 0.764684
+v 0.338104 0.468408 0.816258
+v 0.355502 0.370138 0.858261
+v 0.368733 0.267528 0.890203
+v 0.377641 0.161782 0.911707
+v 0.382120 0.054139 0.922523
+v 0.382120 -0.054139 0.922523
+v 0.377641 -0.161782 0.911707
+v 0.368733 -0.267528 0.890203
+v 0.355502 -0.370138 0.858261
+v 0.338104 -0.468408 0.816258
+v 0.316741 -0.561187 0.764684
+v 0.291665 -0.647386 0.704145
+v 0.263170 -0.725995 0.635351
+v 0.231589 -0.796093 0.559107
+v 0.197293 -0.856857 0.476309
+v 0.160683 -0.907575 0.387926
+v 0.122190 -0.947653 0.294996
+v 0.082265 -0.976620 0.198607
+v 0.041374 -0.994138 0.099889
+v 0.021092 0.994138 0.106041
+v 0.041938 0.976621 0.210839
+v 0.062291 0.947653 0.313165
+v 0.081915 0.907575 0.411820
+v 0.100578 0.856857 0.505646
+v 0.118063 0.796093 0.593545
+v 0.134162 0.725995 0.674484
+v 0.148689 0.647386 0.747516
+v 0.161473 0.561187 0.811784
+v 0.172363 0.468408 0.866534
+v 0.181233 0.370138 0.911125
+v 0.187978 0.267528 0.945034
+v 0.192519 0.161782 0.967863
+v 0.194803 0.054139 0.979346
+v 0.194803 -0.054139 0.979346
+v 0.192519 -0.161782 0.967863
+v 0.187978 -0.267528 0.945034
+v 0.181233 -0.370138 0.911125
+v 0.172363 -0.468408 0.866534
+v 0.161473 -0.561187 0.811784
+v 0.148689 -0.647386 0.747516
+v 0.134162 -0.725995 0.674485
+v 0.118062 -0.796093 0.593545
+v 0.100578 -0.856857 0.505647
+v 0.081915 -0.907575 0.411820
+v 0.062292 -0.947653 0.313166
+v 0.041938 -0.976620 0.210840
+v 0.021092 -0.994138 0.106041
+v -0.000001 0.994138 0.108118
+v -0.000001 0.976621 0.214969
+v -0.000001 0.947653 0.319300
+v -0.000001 0.907575 0.419888
+v -0.000001 0.856857 0.515553
+v -0.000001 0.796093 0.605173
+v -0.000001 0.725995 0.687698
+v -0.000001 0.647386 0.762161
+v -0.000001 0.561187 0.827688
+v -0.000001 0.468408 0.883511
+v -0.000001 0.370138 0.928975
+v -0.000001 0.267528 0.963549
+v -0.000001 0.161782 0.986825
+v -0.000001 0.054139 0.998532
+v -0.000001 -0.054139 0.998532
+v -0.000001 -0.161782 0.986825
+v -0.000001 -0.267528 0.963549
+v -0.000001 -0.370138 0.928975
+v -0.000001 -0.468408 0.883511
+v -0.000001 -0.561187 0.827688
+v -0.000001 -0.647386 0.762161
+v -0.000001 -0.725995 0.687698
+v -0.000001 -0.796093 0.605173
+v -0.000001 -0.856857 0.515553
+v -0.000001 -0.907575 0.419888
+v -0.000001 -0.947653 0.319301
+v -0.000001 -0.976620 0.214970
+v -0.000001 -0.994138 0.108119
+v -0.021094 0.994138 0.106041
+v -0.041939 0.976621 0.210839
+v -0.062293 0.947653 0.313165
+v -0.081917 0.907575 0.411820
+v -0.100580 0.856857 0.505646
+v -0.118064 0.796093 0.593545
+v -0.134164 0.725995 0.674484
+v -0.148691 0.647386 0.747516
+v -0.161475 0.561187 0.811784
+v -0.172365 0.468408 0.866534
+v -0.181235 0.370138 0.911125
+v -0.187980 0.267528 0.945034
+v -0.192521 0.161782 0.967863
+v -0.194805 0.054139 0.979345
+v -0.194805 -0.054139 0.979345
+v -0.192521 -0.161782 0.967863
+v -0.187980 -0.267528 0.945034
+v -0.181235 -0.370138 0.911125
+v -0.172365 -0.468408 0.866534
+v -0.161475 -0.561187 0.811784
+v -0.148691 -0.647386 0.747516
+v -0.134164 -0.725995 0.674484
+v -0.118065 -0.796093 0.593545
+v -0.100580 -0.856857 0.505647
+v -0.081917 -0.907575 0.411820
+v -0.062293 -0.947653 0.313166
+v -0.041940 -0.976620 0.210840
+v -0.021094 -0.994138 0.106041
+v -0.041376 0.994138 0.099888
+v -0.082266 0.976621 0.198606
+v -0.122192 0.947653 0.294995
+v -0.160685 0.907575 0.387926
+v -0.197294 0.856857 0.476308
+v -0.231591 0.796093 0.559107
+v -0.263172 0.725995 0.635350
+v -0.291667 0.647386 0.704145
+v -0.316743 0.561187 0.764684
+v -0.338106 0.468408 0.816257
+v -0.355505 0.370138 0.858261
+v -0.368735 0.267528 0.890203
+v -0.377642 0.161782 0.911707
+v -0.382123 0.054139 0.922523
+v -0.382123 -0.054139 0.922523
+v -0.377642 -0.161782 0.911707
+v -0.368735 -0.267528 0.890203
+v -0.355505 -0.370138 0.858261
+v -0.338106 -0.468408 0.816257
+v -0.316743 -0.561187 0.764684
+v -0.291667 -0.647386 0.704145
+v -0.263172 -0.725995 0.635350
+v -0.231591 -0.796093 0.559107
+v -0.197295 -0.856857 0.476309
+v -0.160685 -0.907575 0.387926
+v -0.122192 -0.947653 0.294996
+v -0.082266 -0.976620 0.198607
+v -0.041376 -0.994138 0.099889
+v -0.060068 0.994138 0.089897
+v -0.119432 0.976621 0.178741
+v -0.177395 0.947653 0.265489
+v -0.233278 0.907575 0.349124
+v -0.286427 0.856857 0.428666
+v -0.336217 0.796093 0.503183
+v -0.382066 0.725995 0.571800
+v -0.423435 0.647386 0.633713
+v -0.459840 0.561187 0.688197
+v -0.490853 0.468408 0.734612
+v -0.516112 0.370138 0.772415
+v -0.535320 0.267528 0.801161
+v -0.548251 0.161782 0.820515
+v -0.554756 0.054139 0.830249
+v -0.554756 -0.054139 0.830249
+v -0.548251 -0.161782 0.820515
+v -0.535320 -0.267528 0.801161
+v -0.516112 -0.370138 0.772415
+v -0.490853 -0.468408 0.734612
+v -0.459840 -0.561187 0.688197
+v -0.423435 -0.647386 0.633713
+v -0.382066 -0.725995 0.571800
+v -0.336217 -0.796093 0.503183
+v -0.286427 -0.856857 0.428667
+v -0.233278 -0.907575 0.349124
+v -0.177395 -0.947653 0.265489
+v -0.119432 -0.976620 0.178741
+v -0.060069 -0.994138 0.089898
+v -0.076452 0.994138 0.076451
+v -0.152007 0.976621 0.152006
+v -0.225780 0.947653 0.225779
+v -0.296907 0.907575 0.296905
+v -0.364552 0.856857 0.364551
+v -0.427923 0.796093 0.427922
+v -0.486277 0.725995 0.486276
+v -0.538930 0.647386 0.538929
+v -0.585265 0.561187 0.585263
+v -0.624737 0.468408 0.624736
+v -0.656886 0.370138 0.656885
+v -0.681333 0.267528 0.681332
+v -0.697791 0.161782 0.697791
+v -0.706070 0.054139 0.706068
+v -0.706070 -0.054139 0.706068
+v -0.697791 -0.161782 0.697791
+v -0.681333 -0.267528 0.681332
+v -0.656886 -0.370138 0.656885
+v -0.624737 -0.468408 0.624736
+v -0.585265 -0.561187 0.585264
+v -0.538930 -0.647386 0.538929
+v -0.486277 -0.725995 0.486276
+v -0.427923 -0.796093 0.427922
+v -0.364552 -0.856857 0.364551
+v -0.296907 -0.907575 0.296906
+v -0.225781 -0.947653 0.225780
+v -0.152008 -0.976620 0.152007
+v -0.076452 -0.994138 0.076452
+v -0.089898 0.994138 0.060067
+v -0.178741 0.976621 0.119431
+v -0.265489 0.947653 0.177394
+v -0.349125 0.907575 0.233277
+v -0.428667 0.856857 0.286426
+v -0.503184 0.796093 0.336216
+v -0.571801 0.725995 0.382064
+v -0.633714 0.647386 0.423434
+v -0.688198 0.561187 0.459839
+v -0.734613 0.468408 0.490852
+v -0.772416 0.370138 0.516111
+v -0.801162 0.267528 0.535319
+v -0.820516 0.161782 0.548250
+v -0.830250 0.054139 0.554754
+v -0.830250 -0.054139 0.554754
+v -0.820516 -0.161782 0.548250
+v -0.801162 -0.267528 0.535319
+v -0.772416 -0.370138 0.516111
+v -0.734613 -0.468408 0.490852
+v -0.688198 -0.561187 0.459839
+v -0.633715 -0.647386 0.423434
+v -0.571801 -0.725995 0.382065
+v -0.503184 -0.796093 0.336216
+v -0.428668 -0.856857 0.286426
+v -0.349125 -0.907575 0.233277
+v -0.265490 -0.947653 0.177394
+v -0.178742 -0.976620 0.119431
+v -0.089898 -0.994138 0.060068
+v -0.099889 0.994138 0.041375
+v -0.198607 0.976621 0.082265
+v -0.294996 0.947653 0.122191
+v -0.387927 0.907575 0.160684
+v -0.476309 0.856857 0.197293
+v -0.559108 0.796093 0.231590
+v -0.635351 0.725995 0.263171
+v -0.704146 0.647386 0.291666
+v -0.764685 0.561187 0.316742
+v -0.816258 0.468408 0.338105
+v -0.858262 0.370138 0.355503
+v -0.890204 0.267528 0.368734
+v -0.911708 0.161782 0.377641
+v -0.922524 0.054139 0.382121
+v -0.922524 -0.054139 0.382121
+v -0.911708 -0.161782 0.377641
+v -0.890204 -0.267528 0.368734
+v -0.858262 -0.370138 0.355503
+v -0.816258 -0.468408 0.338105
+v -0.764685 -0.561187 0.316742
+v -0.704146 -0.647386 0.291666
+v -0.635351 -0.725995 0.263171
+v -0.559108 -0.796093 0.231590
+v -0.476310 -0.856857 0.197293
+v -0.387927 -0.907575 0.160684
+v -0.294997 -0.947653 0.122191
+v -0.198607 -0.976620 0.082265
+v -0.099890 -0.994138 0.041375
+v -0.106041 0.994138 0.021093
+v -0.210840 0.976621 0.041938
+v -0.313166 0.947653 0.062292
+v -0.411821 0.907575 0.081916
+v -0.505647 0.856857 0.100579
+v -0.593546 0.796093 0.118063
+v -0.674485 0.725995 0.134163
+v -0.747517 0.647386 0.148690
+v -0.811785 0.561187 0.161474
+v -0.866535 0.468408 0.172364
+v -0.911126 0.370138 0.181234
+v -0.945035 0.267528 0.187979
+v -0.967864 0.161782 0.192520
+v -0.979346 0.054139 0.194804
+v -0.979346 -0.054139 0.194804
+v -0.967864 -0.161782 0.192520
+v -0.945035 -0.267528 0.187979
+v -0.911126 -0.370138 0.181234
+v -0.866535 -0.468408 0.172364
+v -0.811785 -0.561187 0.161474
+v -0.747517 -0.647386 0.148690
+v -0.674485 -0.725995 0.134163
+v -0.593546 -0.796093 0.118063
+v -0.505648 -0.856857 0.100579
+v -0.411821 -0.907575 0.081916
+v -0.313167 -0.947653 0.062293
+v -0.210840 -0.976620 0.041939
+v -0.106042 -0.994138 0.021093
+vt 1.0000 0.4483
+vt 1.0000 0.4828
+vt 0.9688 0.4828
+vt 0.9688 0.4483
+vt 1.0000 0.8966
+vt 1.0000 0.9310
+vt 0.9688 0.9310
+vt 0.9688 0.8966
+vt 1.0000 0.4138
+vt 0.9688 0.4138
+vt 1.0000 0.8621
+vt 0.9688 0.8621
+vt 1.0000 0.3793
+vt 0.9688 0.3793
+vt 1.0000 0.8276
+vt 0.9688 0.8276
+vt 1.0000 0.3448
+vt 0.9688 0.3448
+vt 1.0000 0.7931
+vt 0.9688 0.7931
+vt 1.0000 0.3103
+vt 0.9688 0.3103
+vt 1.0000 0.7586
+vt 0.9688 0.7586
+vt 1.0000 0.2759
+vt 0.9688 0.2759
+vt 1.0000 0.7241
+vt 0.9688 0.7241
+vt 1.0000 0.2414
+vt 0.9688 0.2414
+vt 1.0000 0.6897
+vt 0.9688 0.6897
+vt 1.0000 0.2069
+vt 0.9688 0.2069
+vt 1.0000 0.6552
+vt 0.9688 0.6552
+vt 1.0000 0.1724
+vt 0.9688 0.1724
+vt 1.0000 0.6207
+vt 0.9688 0.6207
+vt 1.0000 0.1379
+vt 0.9688 0.1379
+vt 1.0000 0.5862
+vt 0.9688 0.5862
+vt 1.0000 0.1034
+vt 0.9688 0.1034
+vt 1.0000 0.5517
+vt 0.9688 0.5517
+vt 1.0000 0.0690
+vt 0.9688 0.0690
+vt 1.0000 0.5172
+vt 0.9688 0.5172
+vt 0.4272 0.9623
+vt 0.4239 0.9603
+vt 0.4290 0.9623
+vt 1.0000 0.0345
+vt 0.9688 0.0345
+vt 1.0000 0.9655
+vt 0.9688 0.9655
+vt 0.4955 0.0510
+vt 0.4942 0.0538
+vt 0.4916 0.0538
+vt 0.9375 0.5517
+vt 0.9375 0.5172
+vt 0.4308 0.9623
+vt 0.9375 0.0690
+vt 0.9375 0.0345
+vt 0.9375 0.4828
+vt 0.9375 0.9655
+vt 0.9375 0.9310
+vt 0.4890 0.0538
+vt 0.9375 0.4483
+vt 0.9375 0.8966
+vt 0.9375 0.4138
+vt 0.9375 0.8621
+vt 0.9375 0.3793
+vt 0.9375 0.8276
+vt 0.9375 0.3448
+vt 0.9375 0.7931
+vt 0.9375 0.3103
+vt 0.9375 0.7586
+vt 0.9375 0.2759
+vt 0.9375 0.7241
+vt 0.9375 0.2414
+vt 0.9375 0.6897
+vt 0.9375 0.2069
+vt 0.9375 0.6552
+vt 0.9375 0.1724
+vt 0.9375 0.6207
+vt 0.9375 0.1379
+vt 0.9375 0.5862
+vt 0.9375 0.1034
+vt 0.9063 0.2759
+vt 0.9063 0.2414
+vt 0.9063 0.7241
+vt 0.9063 0.6897
+vt 0.9063 0.2069
+vt 0.9063 0.6552
+vt 0.9063 0.1724
+vt 0.9062 0.6207
+vt 0.9063 0.1379
+vt 0.9063 0.5862
+vt 0.9063 0.1034
+vt 0.9063 0.5517
+vt 0.9063 0.0690
+vt 0.9063 0.5172
+vt 0.4326 0.9623
+vt 0.9063 0.0345
+vt 0.9063 0.4828
+vt 0.9063 0.9655
+vt 0.9063 0.9310
+vt 0.4864 0.0538
+vt 0.9063 0.4483
+vt 0.9063 0.8966
+vt 0.9063 0.4138
+vt 0.9063 0.8621
+vt 0.9063 0.3793
+vt 0.9063 0.8276
+vt 0.9063 0.3448
+vt 0.9063 0.7931
+vt 0.9063 0.3103
+vt 0.9063 0.7586
+vt 0.8750 0.9310
+vt 0.8750 0.8966
+vt 0.8750 0.4483
+vt 0.8750 0.4138
+vt 0.8750 0.8621
+vt 0.8750 0.3793
+vt 0.8750 0.8276
+vt 0.8750 0.3448
+vt 0.8750 0.7931
+vt 0.8750 0.3103
+vt 0.8750 0.7586
+vt 0.8750 0.2759
+vt 0.8750 0.7241
+vt 0.8750 0.2414
+vt 0.8750 0.6897
+vt 0.8750 0.2069
+vt 0.8750 0.6552
+vt 0.8750 0.1724
+vt 0.8750 0.6207
+vt 0.8750 0.1379
+vt 0.8750 0.5862
+vt 0.8750 0.1034
+vt 0.8750 0.5517
+vt 0.8750 0.0690
+vt 0.8750 0.5172
+vt 0.4344 0.9623
+vt 0.8750 0.0345
+vt 0.8750 0.4828
+vt 0.8750 0.9655
+vt 0.4837 0.0538
+vt 0.8438 0.6552
+vt 0.8438 0.6207
+vt 0.8438 0.1724
+vt 0.8438 0.1379
+vt 0.8438 0.5862
+vt 0.8438 0.1034
+vt 0.8438 0.5517
+vt 0.8438 0.0690
+vt 0.8438 0.5172
+vt 0.4362 0.9623
+vt 0.8438 0.0345
+vt 0.8438 0.4828
+vt 0.8438 0.9655
+vt 0.8438 0.9310
+vt 0.4811 0.0538
+vt 0.8438 0.4483
+vt 0.8438 0.8966
+vt 0.8438 0.4138
+vt 0.8438 0.8621
+vt 0.8438 0.3793
+vt 0.8438 0.8276
+vt 0.8438 0.3448
+vt 0.8438 0.7931
+vt 0.8438 0.3103
+vt 0.8438 0.7586
+vt 0.8438 0.2759
+vt 0.8438 0.7241
+vt 0.8438 0.2414
+vt 0.8438 0.6897
+vt 0.8438 0.2069
+vt 0.8125 0.3793
+vt 0.8125 0.3448
+vt 0.8125 0.8276
+vt 0.8125 0.7931
+vt 0.8125 0.3103
+vt 0.8125 0.7586
+vt 0.8125 0.2759
+vt 0.8125 0.7241
+vt 0.8125 0.2414
+vt 0.8125 0.6897
+vt 0.8125 0.2069
+vt 0.8125 0.6552
+vt 0.8125 0.1724
+vt 0.8125 0.6207
+vt 0.8125 0.1379
+vt 0.8125 0.5862
+vt 0.8125 0.1034
+vt 0.8125 0.5517
+vt 0.8125 0.0690
+vt 0.8125 0.5172
+vt 0.4381 0.9623
+vt 0.8125 0.0345
+vt 0.8125 0.4828
+vt 0.8125 0.9655
+vt 0.8125 0.9310
+vt 0.4785 0.0538
+vt 0.8125 0.4483
+vt 0.8125 0.8966
+vt 0.8125 0.4138
+vt 0.8125 0.8621
+vt 0.7813 0.1034
+vt 0.7813 0.0690
+vt 0.7813 0.5517
+vt 0.7813 0.5172
+vt 0.4399 0.9623
+vt 0.7813 0.0345
+vt 0.7813 0.4828
+vt 0.7813 0.9655
+vt 0.7813 0.9310
+vt 0.4759 0.0538
+vt 0.7813 0.4483
+vt 0.7813 0.8966
+vt 0.7813 0.4138
+vt 0.7813 0.8621
+vt 0.7813 0.3793
+vt 0.7813 0.8276
+vt 0.7813 0.3448
+vt 0.7813 0.7931
+vt 0.7813 0.3103
+vt 0.7813 0.7586
+vt 0.7813 0.2759
+vt 0.7813 0.7241
+vt 0.7813 0.2414
+vt 0.7813 0.6897
+vt 0.7813 0.2069
+vt 0.7813 0.6552
+vt 0.7813 0.1724
+vt 0.7813 0.6207
+vt 0.7813 0.1379
+vt 0.7813 0.5862
+vt 0.7500 0.2759
+vt 0.7500 0.2414
+vt 0.7500 0.7241
+vt 0.7500 0.6897
+vt 0.7500 0.2069
+vt 0.7500 0.6552
+vt 0.7500 0.1724
+vt 0.7500 0.6207
+vt 0.7500 0.1379
+vt 0.7500 0.5862
+vt 0.7500 0.1034
+vt 0.7500 0.5517
+vt 0.7500 0.0690
+vt 0.7500 0.5172
+vt 0.4417 0.9623
+vt 0.7500 0.0345
+vt 0.7500 0.4828
+vt 0.7500 0.9655
+vt 0.7500 0.9310
+vt 0.4733 0.0538
+vt 0.7500 0.4483
+vt 0.7500 0.8966
+vt 0.7500 0.4138
+vt 0.7500 0.8621
+vt 0.7500 0.3793
+vt 0.7500 0.8276
+vt 0.7500 0.3448
+vt 0.7500 0.7931
+vt 0.7500 0.3103
+vt 0.7500 0.7586
+vt 0.7188 0.9310
+vt 0.7188 0.8966
+vt 0.7188 0.4483
+vt 0.7188 0.4138
+vt 0.7188 0.8621
+vt 0.7188 0.3793
+vt 0.7188 0.8276
+vt 0.7188 0.3448
+vt 0.7188 0.7931
+vt 0.7188 0.3103
+vt 0.7188 0.7586
+vt 0.7188 0.2759
+vt 0.7188 0.7241
+vt 0.7188 0.2414
+vt 0.7188 0.6897
+vt 0.7188 0.2069
+vt 0.7188 0.6552
+vt 0.7188 0.1724
+vt 0.7188 0.6207
+vt 0.7188 0.1379
+vt 0.7188 0.5862
+vt 0.7188 0.1034
+vt 0.7188 0.5517
+vt 0.7188 0.0690
+vt 0.7188 0.5172
+vt 0.4435 0.9623
+vt 0.7188 0.0345
+vt 0.7188 0.4828
+vt 0.7188 0.9655
+vt 0.4706 0.0538
+vt 0.6875 0.6552
+vt 0.6875 0.6207
+vt 0.6875 0.1724
+vt 0.6875 0.1379
+vt 0.6875 0.5862
+vt 0.6875 0.1034
+vt 0.6875 0.5517
+vt 0.6875 0.0690
+vt 0.6875 0.5172
+vt 0.4453 0.9623
+vt 0.6875 0.0345
+vt 0.6875 0.4828
+vt 0.6875 0.9655
+vt 0.6875 0.9310
+vt 0.4680 0.0538
+vt 0.6875 0.4483
+vt 0.6875 0.8966
+vt 0.6875 0.4138
+vt 0.6875 0.8621
+vt 0.6875 0.3793
+vt 0.6875 0.8276
+vt 0.6875 0.3448
+vt 0.6875 0.7931
+vt 0.6875 0.3103
+vt 0.6875 0.7586
+vt 0.6875 0.2759
+vt 0.6875 0.7241
+vt 0.6875 0.2414
+vt 0.6875 0.6897
+vt 0.6875 0.2069
+vt 0.6563 0.3793
+vt 0.6563 0.3448
+vt 0.6563 0.8276
+vt 0.6563 0.7931
+vt 0.6563 0.3103
+vt 0.6563 0.7586
+vt 0.6563 0.2759
+vt 0.6563 0.7241
+vt 0.6563 0.2414
+vt 0.6563 0.6897
+vt 0.6563 0.2069
+vt 0.6563 0.6552
+vt 0.6563 0.1724
+vt 0.6563 0.6207
+vt 0.6563 0.1379
+vt 0.6563 0.5862
+vt 0.6563 0.1034
+vt 0.6563 0.5517
+vt 0.6563 0.0690
+vt 0.6563 0.5172
+vt 0.4471 0.9623
+vt 0.6563 0.0345
+vt 0.6563 0.4828
+vt 0.6563 0.9655
+vt 0.6563 0.9310
+vt 0.4654 0.0538
+vt 0.6563 0.4483
+vt 0.6563 0.8966
+vt 0.6563 0.4138
+vt 0.6563 0.8621
+vt 0.6250 0.1034
+vt 0.6250 0.0690
+vt 0.6250 0.5517
+vt 0.6250 0.5172
+vt 0.4490 0.9623
+vt 0.6250 0.0345
+vt 0.6250 0.4828
+vt 0.6250 0.9655
+vt 0.6250 0.9310
+vt 0.4628 0.0538
+vt 0.6250 0.4483
+vt 0.6250 0.8966
+vt 0.6250 0.4138
+vt 0.6250 0.8621
+vt 0.6250 0.3793
+vt 0.6250 0.8276
+vt 0.6250 0.3448
+vt 0.6250 0.7931
+vt 0.6250 0.3103
+vt 0.6250 0.7586
+vt 0.6250 0.2759
+vt 0.6250 0.7241
+vt 0.6250 0.2414
+vt 0.6250 0.6897
+vt 0.6250 0.2069
+vt 0.6250 0.6552
+vt 0.6250 0.1724
+vt 0.6250 0.6207
+vt 0.6250 0.1379
+vt 0.6250 0.5862
+vt 0.5938 0.7586
+vt 0.5938 0.7241
+vt 0.5938 0.2759
+vt 0.5938 0.2414
+vt 0.5938 0.6897
+vt 0.5938 0.2069
+vt 0.5938 0.6552
+vt 0.5938 0.1724
+vt 0.5938 0.6207
+vt 0.5938 0.1379
+vt 0.5938 0.5862
+vt 0.5938 0.1034
+vt 0.5938 0.5517
+vt 0.5938 0.0690
+vt 0.5938 0.5172
+vt 0.4508 0.9623
+vt 0.5938 0.0345
+vt 0.5938 0.4828
+vt 0.5938 0.9655
+vt 0.5938 0.9310
+vt 0.4602 0.0538
+vt 0.5938 0.4483
+vt 0.5938 0.8966
+vt 0.5938 0.4138
+vt 0.5938 0.8621
+vt 0.5938 0.3793
+vt 0.5938 0.8276
+vt 0.5938 0.3448
+vt 0.5938 0.7931
+vt 0.5938 0.3103
+vt 0.5625 0.4828
+vt 0.5625 0.4483
+vt 0.5625 0.9310
+vt 0.5625 0.8966
+vt 0.5625 0.4138
+vt 0.5625 0.8621
+vt 0.5625 0.3793
+vt 0.5625 0.8276
+vt 0.5625 0.3448
+vt 0.5625 0.7931
+vt 0.5625 0.3103
+vt 0.5625 0.7586
+vt 0.5625 0.2759
+vt 0.5625 0.7241
+vt 0.5625 0.2414
+vt 0.5625 0.6897
+vt 0.5625 0.2069
+vt 0.5625 0.6552
+vt 0.5625 0.1724
+vt 0.5625 0.6207
+vt 0.5625 0.1379
+vt 0.5625 0.5862
+vt 0.5625 0.1034
+vt 0.5625 0.5517
+vt 0.5625 0.0690
+vt 0.5625 0.5172
+vt 0.4526 0.9623
+vt 0.5625 0.0345
+vt 0.5625 0.9655
+vt 0.4575 0.0538
+vt 0.5312 0.2069
+vt 0.5312 0.1724
+vt 0.5312 0.6552
+vt 0.5312 0.6207
+vt 0.5312 0.1379
+vt 0.5312 0.5862
+vt 0.5313 0.1034
+vt 0.5312 0.5517
+vt 0.5313 0.0690
+vt 0.5312 0.5172
+vt 0.4544 0.9623
+vt 0.5313 0.0345
+vt 0.5312 0.4828
+vt 0.5313 0.9655
+vt 0.5313 0.9310
+vt 0.4549 0.0538
+vt 0.5312 0.4483
+vt 0.5313 0.8966
+vt 0.5312 0.4138
+vt 0.5313 0.8621
+vt 0.5312 0.3793
+vt 0.5313 0.8276
+vt 0.5312 0.3448
+vt 0.5313 0.7931
+vt 0.5312 0.3103
+vt 0.5313 0.7586
+vt 0.5312 0.2759
+vt 0.5312 0.7241
+vt 0.5312 0.2414
+vt 0.5313 0.6897
+vt 0.5000 0.3793
+vt 0.5000 0.3448
+vt 0.5000 0.8276
+vt 0.5000 0.7931
+vt 0.5000 0.3103
+vt 0.5000 0.7586
+vt 0.5000 0.2759
+vt 0.5000 0.7241
+vt 0.5000 0.2414
+vt 0.5000 0.6897
+vt 0.5000 0.2069
+vt 0.5000 0.6552
+vt 0.5000 0.1724
+vt 0.5000 0.6207
+vt 0.5000 0.1379
+vt 0.5000 0.5862
+vt 0.5000 0.1034
+vt 0.5000 0.5517
+vt 0.5000 0.0690
+vt 0.5000 0.5172
+vt 0.4820 0.9603
+vt 0.4562 0.9623
+vt 0.5000 0.0345
+vt 0.5000 0.4828
+vt 0.5000 0.9655
+vt 0.5000 0.9310
+vt 0.4523 0.0538
+vt 0.5000 0.4483
+vt 0.5000 0.8966
+vt 0.5000 0.4138
+vt 0.5000 0.8621
+vt 0.4687 0.1034
+vt 0.4687 0.0690
+vt 0.4687 0.5517
+vt 0.4687 0.5172
+vt 0.4580 0.9623
+vt 0.4687 0.0345
+vt 0.4687 0.4828
+vt 0.4687 0.9655
+vt 0.4687 0.9310
+vt 0.4117 0.0510
+vt 0.4497 0.0538
+vt 0.4687 0.4483
+vt 0.4687 0.8966
+vt 0.4687 0.4138
+vt 0.4687 0.8621
+vt 0.4687 0.3793
+vt 0.4687 0.8276
+vt 0.4687 0.3448
+vt 0.4687 0.7931
+vt 0.4687 0.3103
+vt 0.4688 0.7586
+vt 0.4687 0.2759
+vt 0.4687 0.7241
+vt 0.4687 0.2414
+vt 0.4687 0.6897
+vt 0.4687 0.2069
+vt 0.4687 0.6552
+vt 0.4687 0.1724
+vt 0.4687 0.6207
+vt 0.4687 0.1379
+vt 0.4687 0.5862
+vt 0.4375 0.7586
+vt 0.4375 0.7241
+vt 0.4375 0.2759
+vt 0.4375 0.2414
+vt 0.4375 0.6897
+vt 0.4375 0.2069
+vt 0.4375 0.6552
+vt 0.4375 0.1724
+vt 0.4375 0.6207
+vt 0.4375 0.1379
+vt 0.4375 0.5862
+vt 0.4375 0.1034
+vt 0.4375 0.5517
+vt 0.4375 0.0690
+vt 0.4375 0.5172
+vt 0.4599 0.9623
+vt 0.4375 0.0345
+vt 0.4375 0.4828
+vt 0.4375 0.9655
+vt 0.4375 0.9310
+vt 0.4471 0.0538
+vt 0.4375 0.4483
+vt 0.4375 0.8966
+vt 0.4375 0.4138
+vt 0.4375 0.8621
+vt 0.4375 0.3793
+vt 0.4375 0.8276
+vt 0.4375 0.3448
+vt 0.4375 0.7931
+vt 0.4375 0.3103
+vt 0.4062 0.4828
+vt 0.4062 0.4483
+vt 0.4062 0.9310
+vt 0.4062 0.8966
+vt 0.4062 0.4138
+vt 0.4062 0.8621
+vt 0.4062 0.3793
+vt 0.4062 0.8276
+vt 0.4062 0.3448
+vt 0.4062 0.7931
+vt 0.4062 0.3103
+vt 0.4062 0.7586
+vt 0.4062 0.2759
+vt 0.4062 0.7241
+vt 0.4062 0.2414
+vt 0.4062 0.6897
+vt 0.4062 0.2069
+vt 0.4062 0.6552
+vt 0.4062 0.1724
+vt 0.4062 0.6207
+vt 0.4062 0.1379
+vt 0.4062 0.5862
+vt 0.4062 0.1034
+vt 0.4062 0.5517
+vt 0.4062 0.0690
+vt 0.4062 0.5172
+vt 0.4617 0.9623
+vt 0.4062 0.0345
+vt 0.4062 0.9655
+vt 0.4444 0.0538
+vt 0.3750 0.2069
+vt 0.3750 0.1724
+vt 0.3750 0.6552
+vt 0.3750 0.6207
+vt 0.3750 0.1379
+vt 0.3750 0.5862
+vt 0.3750 0.1034
+vt 0.3750 0.5517
+vt 0.3750 0.0690
+vt 0.3750 0.5172
+vt 0.4635 0.9623
+vt 0.3750 0.0345
+vt 0.3750 0.4828
+vt 0.3750 0.9655
+vt 0.3750 0.9310
+vt 0.4418 0.0538
+vt 0.3750 0.4483
+vt 0.3750 0.8966
+vt 0.3750 0.4138
+vt 0.3750 0.8621
+vt 0.3750 0.3793
+vt 0.3750 0.8276
+vt 0.3750 0.3448
+vt 0.3750 0.7931
+vt 0.3750 0.3103
+vt 0.3750 0.7586
+vt 0.3750 0.2759
+vt 0.3750 0.7241
+vt 0.3750 0.2414
+vt 0.3750 0.6897
+vt 0.3437 0.8621
+vt 0.3437 0.8276
+vt 0.3437 0.3793
+vt 0.3437 0.3448
+vt 0.3437 0.7931
+vt 0.3437 0.3103
+vt 0.3437 0.7586
+vt 0.3437 0.2759
+vt 0.3437 0.7241
+vt 0.3437 0.2414
+vt 0.3437 0.6897
+vt 0.3437 0.2069
+vt 0.3437 0.6552
+vt 0.3437 0.1724
+vt 0.3437 0.6207
+vt 0.3437 0.1379
+vt 0.3437 0.5862
+vt 0.3437 0.1034
+vt 0.3437 0.5517
+vt 0.3437 0.0690
+vt 0.3437 0.5172
+vt 0.4653 0.9623
+vt 0.3437 0.0345
+vt 0.3437 0.4828
+vt 0.3437 0.9655
+vt 0.3437 0.9310
+vt 0.4392 0.0538
+vt 0.3437 0.4483
+vt 0.3437 0.8966
+vt 0.3437 0.4138
+vt 0.3125 0.5862
+vt 0.3125 0.5517
+vt 0.3125 0.1034
+vt 0.3125 0.0690
+vt 0.3125 0.5172
+vt 0.4671 0.9623
+vt 0.3125 0.0345
+vt 0.3125 0.4828
+vt 0.3125 0.9655
+vt 0.3125 0.9310
+vt 0.4366 0.0538
+vt 0.3125 0.4483
+vt 0.3125 0.8966
+vt 0.3125 0.4138
+vt 0.3125 0.8621
+vt 0.3125 0.3793
+vt 0.3125 0.8276
+vt 0.3125 0.3448
+vt 0.3125 0.7931
+vt 0.3125 0.3103
+vt 0.3125 0.7586
+vt 0.3125 0.2759
+vt 0.3125 0.7241
+vt 0.3125 0.2414
+vt 0.3125 0.6897
+vt 0.3125 0.2069
+vt 0.3125 0.6552
+vt 0.3125 0.1724
+vt 0.3125 0.6207
+vt 0.3125 0.1379
+vt 0.2812 0.3103
+vt 0.2812 0.2759
+vt 0.2812 0.7586
+vt 0.2812 0.7241
+vt 0.2812 0.2414
+vt 0.2812 0.6897
+vt 0.2812 0.2069
+vt 0.2812 0.6552
+vt 0.2812 0.1724
+vt 0.2812 0.6207
+vt 0.2812 0.1379
+vt 0.2812 0.5862
+vt 0.2812 0.1034
+vt 0.2812 0.5517
+vt 0.2812 0.0690
+vt 0.2812 0.5172
+vt 0.4689 0.9623
+vt 0.2812 0.0345
+vt 0.2812 0.4828
+vt 0.2812 0.9655
+vt 0.2812 0.9310
+vt 0.4340 0.0538
+vt 0.2812 0.4483
+vt 0.2812 0.8966
+vt 0.2812 0.4138
+vt 0.2812 0.8621
+vt 0.2812 0.3793
+vt 0.2812 0.8276
+vt 0.2812 0.3448
+vt 0.2812 0.7931
+vt 0.4313 0.0538
+vt 0.2500 0.4828
+vt 0.2500 0.4483
+vt 0.2500 0.9310
+vt 0.2500 0.8966
+vt 0.2500 0.4138
+vt 0.2500 0.8621
+vt 0.2500 0.3793
+vt 0.2500 0.8276
+vt 0.2500 0.3448
+vt 0.2500 0.7931
+vt 0.2500 0.3103
+vt 0.2500 0.7586
+vt 0.2500 0.2759
+vt 0.2500 0.7241
+vt 0.2500 0.2414
+vt 0.2500 0.6897
+vt 0.2500 0.2069
+vt 0.2500 0.6552
+vt 0.2500 0.1724
+vt 0.2500 0.6207
+vt 0.2500 0.1379
+vt 0.2500 0.5862
+vt 0.2500 0.1034
+vt 0.2500 0.5517
+vt 0.2500 0.0690
+vt 0.2500 0.5172
+vt 0.4708 0.9623
+vt 0.2500 0.0345
+vt 0.2500 0.9655
+vt 0.2187 0.2069
+vt 0.2187 0.1724
+vt 0.2187 0.6552
+vt 0.2187 0.6207
+vt 0.2187 0.1379
+vt 0.2187 0.5862
+vt 0.2187 0.1034
+vt 0.2187 0.5517
+vt 0.2187 0.0690
+vt 0.2187 0.5172
+vt 0.4726 0.9623
+vt 0.2187 0.0345
+vt 0.2187 0.4828
+vt 0.2187 0.9655
+vt 0.2187 0.9310
+vt 0.4287 0.0538
+vt 0.2187 0.4483
+vt 0.2187 0.8966
+vt 0.2187 0.4138
+vt 0.2187 0.8621
+vt 0.2187 0.3793
+vt 0.2187 0.8276
+vt 0.2187 0.3448
+vt 0.2187 0.7931
+vt 0.2187 0.3103
+vt 0.2187 0.7586
+vt 0.2187 0.2759
+vt 0.2187 0.7241
+vt 0.2187 0.2414
+vt 0.2187 0.6897
+vt 0.1875 0.8621
+vt 0.1875 0.8276
+vt 0.1875 0.3793
+vt 0.1875 0.3448
+vt 0.1875 0.7931
+vt 0.1875 0.3103
+vt 0.1875 0.7586
+vt 0.1875 0.2759
+vt 0.1875 0.7241
+vt 0.1875 0.2414
+vt 0.1875 0.6897
+vt 0.1875 0.2069
+vt 0.1875 0.6552
+vt 0.1875 0.1724
+vt 0.1875 0.6207
+vt 0.1875 0.1379
+vt 0.1875 0.5862
+vt 0.1875 0.1034
+vt 0.1875 0.5517
+vt 0.1875 0.0690
+vt 0.1875 0.5172
+vt 0.4744 0.9623
+vt 0.1875 0.0345
+vt 0.1875 0.4828
+vt 0.1875 0.9655
+vt 0.1875 0.9310
+vt 0.4261 0.0538
+vt 0.1875 0.4483
+vt 0.1875 0.8966
+vt 0.1875 0.4138
+vt 0.1562 0.5862
+vt 0.1562 0.5517
+vt 0.1562 0.1034
+vt 0.1562 0.0690
+vt 0.1562 0.5172
+vt 0.4762 0.9623
+vt 0.1562 0.0345
+vt 0.1562 0.4828
+vt 0.1562 0.9655
+vt 0.1562 0.9310
+vt 0.4235 0.0538
+vt 0.1562 0.4483
+vt 0.1562 0.8966
+vt 0.1562 0.4138
+vt 0.1562 0.8621
+vt 0.1562 0.3793
+vt 0.1562 0.8276
+vt 0.1562 0.3448
+vt 0.1562 0.7931
+vt 0.1562 0.3103
+vt 0.1562 0.7586
+vt 0.1562 0.2759
+vt 0.1562 0.7241
+vt 0.1562 0.2414
+vt 0.1562 0.6897
+vt 0.1562 0.2069
+vt 0.1562 0.6552
+vt 0.1562 0.1724
+vt 0.1562 0.6207
+vt 0.1562 0.1379
+vt 0.1250 0.3103
+vt 0.1250 0.2759
+vt 0.1250 0.7586
+vt 0.1250 0.7241
+vt 0.1250 0.2414
+vt 0.1250 0.6897
+vt 0.1250 0.2069
+vt 0.1250 0.6552
+vt 0.1250 0.1724
+vt 0.1250 0.6207
+vt 0.1250 0.1379
+vt 0.1250 0.5862
+vt 0.1250 0.1034
+vt 0.1250 0.5517
+vt 0.1250 0.0690
+vt 0.1250 0.5172
+vt 0.4780 0.9623
+vt 0.1250 0.0345
+vt 0.1250 0.4828
+vt 0.1250 0.9655
+vt 0.1250 0.9310
+vt 0.4209 0.0538
+vt 0.1250 0.4483
+vt 0.1250 0.8966
+vt 0.1250 0.4138
+vt 0.1250 0.8621
+vt 0.1250 0.3793
+vt 0.1250 0.8276
+vt 0.1250 0.3448
+vt 0.1250 0.7931
+vt 0.0937 0.9655
+vt 0.0937 0.9310
+vt 0.0937 0.4828
+vt 0.0937 0.4483
+vt 0.0937 0.0690
+vt 0.0937 0.0345
+vt 0.0937 0.8966
+vt 0.4182 0.0538
+vt 0.0937 0.8621
+vt 0.0937 0.8276
+vt 0.0937 0.4138
+vt 0.0937 0.7931
+vt 0.0937 0.3793
+vt 0.0937 0.7586
+vt 0.0937 0.3448
+vt 0.0937 0.7241
+vt 0.0937 0.3103
+vt 0.0937 0.6897
+vt 0.0937 0.2759
+vt 0.0937 0.6552
+vt 0.0937 0.2414
+vt 0.0937 0.6207
+vt 0.0937 0.2069
+vt 0.0937 0.5862
+vt 0.0937 0.1724
+vt 0.0937 0.5517
+vt 0.0937 0.1379
+vt 0.0937 0.5172
+vt 0.0937 0.1034
+vt 0.4798 0.9623
+vt 0.0625 0.2759
+vt 0.0625 0.2414
+vt 0.0625 0.7241
+vt 0.0625 0.6897
+vt 0.0625 0.2069
+vt 0.0625 0.6552
+vt 0.0625 0.1724
+vt 0.0625 0.6207
+vt 0.0625 0.1379
+vt 0.0625 0.5862
+vt 0.0625 0.1034
+vt 0.0625 0.5517
+vt 0.0625 0.0690
+vt 0.0625 0.5172
+vt 0.4817 0.9623
+vt 0.0625 0.0345
+vt 0.0625 0.4828
+vt 0.0625 0.9655
+vt 0.0625 0.9310
+vt 0.4156 0.0538
+vt 0.0625 0.4483
+vt 0.0625 0.8966
+vt 0.0625 0.4138
+vt 0.0625 0.8621
+vt 0.0625 0.3793
+vt 0.0625 0.8276
+vt 0.0625 0.3448
+vt 0.0625 0.7931
+vt 0.0625 0.3103
+vt 0.0625 0.7586
+vt 0.0312 0.4483
+vt 0.0312 0.4138
+vt 0.0312 0.8966
+vt 0.0312 0.8621
+vt 0.0312 0.3793
+vt 0.0312 0.8276
+vt 0.0312 0.3448
+vt 0.0312 0.7931
+vt 0.0312 0.3103
+vt 0.0312 0.7586
+vt 0.0312 0.2759
+vt 0.0312 0.7241
+vt 0.0312 0.2414
+vt 0.0312 0.6897
+vt 0.0312 0.2069
+vt 0.0312 0.6552
+vt 0.0312 0.1724
+vt 0.0312 0.6207
+vt 0.0312 0.1379
+vt 0.0312 0.5862
+vt 0.0312 0.1034
+vt 0.0312 0.5517
+vt 0.0312 0.0690
+vt 0.0312 0.5172
+vt 0.4835 0.9623
+vt 0.0312 0.0345
+vt 0.0312 0.4828
+vt 0.0312 0.9655
+vt 0.0312 0.9310
+vt 0.4130 0.0538
+vt 0.0000 0.1724
+vt 0.0000 0.1379
+vt 0.0000 0.6207
+vt 0.0000 0.5862
+vt 0.0000 0.1034
+vt 0.0000 0.5517
+vt 0.0000 0.0690
+vt 0.0000 0.5172
+vt 0.4253 0.9623
+vt 0.0000 0.0345
+vt 0.0000 0.4828
+vt 1.0312 0.9310
+vt 1.0312 0.9655
+vt 0.4104 0.0538
+vt 0.0000 0.4483
+vt 1.0312 0.8966
+vt 0.0000 0.4138
+vt 0.0000 0.8966
+vt 0.0000 0.8621
+vt 0.0000 0.3793
+vt 0.0000 0.8276
+vt 0.0000 0.3448
+vt 0.0000 0.7931
+vt 0.0000 0.3103
+vt 0.0000 0.7586
+vt 0.0000 0.2759
+vt 0.0000 0.7241
+vt 0.0000 0.2414
+vt 0.0000 0.6897
+vt 0.0000 0.2069
+vt 0.0000 0.6552
+vn -0.9894 -0.1076 -0.0974
+vn -0.2674 0.9632 -0.0263
+vn -0.9721 -0.2140 -0.0957
+vn -0.3699 0.9284 -0.0364
+vn -0.9436 -0.3179 -0.0929
+vn -0.4679 0.8826 -0.0461
+vn -0.9040 -0.4182 -0.0890
+vn -0.5603 0.8264 -0.0552
+vn -0.8538 -0.5137 -0.0841
+vn -0.6461 0.7606 -0.0636
+vn -0.7937 -0.6033 -0.0782
+vn -0.7241 0.6859 -0.0713
+vn -0.7241 -0.6859 -0.0713
+vn -0.7937 0.6033 -0.0782
+vn -0.6461 -0.7606 -0.0636
+vn -0.8538 0.5137 -0.0841
+vn -0.5603 -0.8264 -0.0552
+vn -0.9040 0.4182 -0.0890
+vn -0.4679 -0.8826 -0.0461
+vn -0.9436 0.3179 -0.0929
+vn -0.3699 -0.9284 -0.0364
+vn -0.9721 0.2140 -0.0957
+vn -0.2674 -0.9632 -0.0263
+vn -0.9894 0.1076 -0.0974
+vn -0.0541 0.9985 -0.0053
+vn -0.1618 -0.9867 -0.0159
+vn -0.9952 0.0000 -0.0980
+vn -0.1618 0.9867 -0.0159
+vn -0.0541 -0.9985 -0.0053
+vn -0.9514 0.1076 -0.2886
+vn -0.0521 0.9985 -0.0158
+vn -0.1555 -0.9867 -0.0472
+vn -0.9569 0.0000 -0.2903
+vn -0.1555 0.9867 -0.0472
+vn -0.0521 -0.9985 -0.0158
+vn -0.9514 -0.1076 -0.2886
+vn -0.2572 0.9632 -0.0780
+vn -0.9348 -0.2140 -0.2836
+vn -0.3557 0.9284 -0.1079
+vn -0.9073 -0.3179 -0.2752
+vn -0.4499 0.8826 -0.1365
+vn -0.8692 -0.4182 -0.2637
+vn -0.5388 0.8264 -0.1634
+vn -0.8210 -0.5137 -0.2491
+vn -0.6212 0.7606 -0.1885
+vn -0.7632 -0.6033 -0.2315
+vn -0.6963 0.6859 -0.2112
+vn -0.6963 -0.6859 -0.2112
+vn -0.7632 0.6033 -0.2315
+vn -0.6212 -0.7606 -0.1885
+vn -0.8210 0.5137 -0.2491
+vn -0.5388 -0.8264 -0.1634
+vn -0.8692 0.4182 -0.2637
+vn -0.4499 -0.8826 -0.1365
+vn -0.9073 0.3179 -0.2752
+vn -0.3557 -0.9284 -0.1079
+vn -0.9348 0.2140 -0.2836
+vn -0.2572 -0.9632 -0.0780
+vn -0.6417 -0.6859 -0.3430
+vn -0.7033 0.6033 -0.3759
+vn -0.5725 -0.7606 -0.3060
+vn -0.7566 0.5137 -0.4044
+vn -0.4966 -0.8264 -0.2654
+vn -0.8011 0.4182 -0.4282
+vn -0.4147 -0.8826 -0.2216
+vn -0.8362 0.3179 -0.4469
+vn -0.3278 -0.9284 -0.1752
+vn -0.8615 0.2140 -0.4605
+vn -0.2370 -0.9632 -0.1267
+vn -0.8768 0.1076 -0.4687
+vn -0.0480 0.9985 -0.0256
+vn -0.1434 -0.9867 -0.0766
+vn -0.8819 0.0000 -0.4714
+vn -0.1434 0.9867 -0.0766
+vn -0.0480 -0.9985 -0.0256
+vn -0.8768 -0.1076 -0.4687
+vn -0.2370 0.9632 -0.1267
+vn -0.8615 -0.2140 -0.4605
+vn -0.3278 0.9284 -0.1752
+vn -0.8362 -0.3179 -0.4469
+vn -0.4147 0.8826 -0.2216
+vn -0.8011 -0.4182 -0.4282
+vn -0.4966 0.8264 -0.2654
+vn -0.7566 -0.5137 -0.4044
+vn -0.5725 0.7606 -0.3060
+vn -0.7033 -0.6033 -0.3759
+vn -0.6417 0.6859 -0.3430
+vn -0.2077 0.9632 -0.1705
+vn -0.7551 -0.2140 -0.6197
+vn -0.2873 0.9284 -0.2358
+vn -0.7329 -0.3179 -0.6015
+vn -0.3635 0.8826 -0.2983
+vn -0.7022 -0.4182 -0.5762
+vn -0.4352 0.8264 -0.3572
+vn -0.6632 -0.5137 -0.5443
+vn -0.5018 0.7606 -0.4118
+vn -0.6165 -0.6033 -0.5059
+vn -0.5625 0.6859 -0.4616
+vn -0.5625 -0.6859 -0.4616
+vn -0.6165 0.6033 -0.5059
+vn -0.5018 -0.7606 -0.4118
+vn -0.6632 0.5137 -0.5443
+vn -0.4352 -0.8264 -0.3572
+vn -0.7022 0.4182 -0.5762
+vn -0.3635 -0.8826 -0.2983
+vn -0.7329 0.3179 -0.6015
+vn -0.2873 -0.9284 -0.2358
+vn -0.7551 0.2140 -0.6197
+vn -0.2077 -0.9632 -0.1705
+vn -0.7685 0.1076 -0.6307
+vn -0.0421 0.9985 -0.0345
+vn -0.1256 -0.9867 -0.1031
+vn -0.7730 0.0000 -0.6344
+vn -0.1256 0.9867 -0.1031
+vn -0.0421 -0.9985 -0.0345
+vn -0.7685 -0.1076 -0.6307
+vn -0.5762 0.4182 -0.7022
+vn -0.2983 -0.8826 -0.3635
+vn -0.6015 0.3179 -0.7329
+vn -0.2358 -0.9284 -0.2873
+vn -0.6197 0.2140 -0.7551
+vn -0.1705 -0.9632 -0.2077
+vn -0.6307 0.1076 -0.7685
+vn -0.0345 0.9985 -0.0421
+vn -0.1031 -0.9867 -0.1256
+vn -0.6344 0.0000 -0.7730
+vn -0.1031 0.9867 -0.1256
+vn -0.0345 -0.9985 -0.0421
+vn -0.6307 -0.1076 -0.7685
+vn -0.1705 0.9632 -0.2077
+vn -0.6197 -0.2140 -0.7551
+vn -0.2358 0.9284 -0.2873
+vn -0.6015 -0.3179 -0.7329
+vn -0.2983 0.8826 -0.3635
+vn -0.5762 -0.4182 -0.7022
+vn -0.3572 0.8264 -0.4352
+vn -0.5443 -0.5137 -0.6632
+vn -0.4118 0.7606 -0.5018
+vn -0.5059 -0.6033 -0.6165
+vn -0.4616 0.6859 -0.5625
+vn -0.4616 -0.6859 -0.5625
+vn -0.5059 0.6033 -0.6165
+vn -0.4118 -0.7606 -0.5018
+vn -0.5443 0.5137 -0.6632
+vn -0.3572 -0.8264 -0.4352
+vn -0.4282 -0.4182 -0.8011
+vn -0.2654 0.8264 -0.4966
+vn -0.4044 -0.5137 -0.7566
+vn -0.3060 0.7606 -0.5725
+vn -0.3759 -0.6033 -0.7033
+vn -0.3430 0.6859 -0.6417
+vn -0.3430 -0.6859 -0.6417
+vn -0.3759 0.6033 -0.7033
+vn -0.3060 -0.7606 -0.5725
+vn -0.4044 0.5137 -0.7566
+vn -0.2654 -0.8264 -0.4966
+vn -0.4282 0.4182 -0.8011
+vn -0.2216 -0.8826 -0.4147
+vn -0.4469 0.3179 -0.8362
+vn -0.1752 -0.9284 -0.3278
+vn -0.4605 0.2140 -0.8615
+vn -0.1267 -0.9632 -0.2370
+vn -0.4687 0.1076 -0.8768
+vn -0.0256 0.9985 -0.0480
+vn -0.0766 -0.9867 -0.1434
+vn -0.4714 0.0000 -0.8819
+vn -0.0766 0.9867 -0.1433
+vn -0.0256 -0.9985 -0.0480
+vn -0.4687 -0.1076 -0.8768
+vn -0.1267 0.9632 -0.2370
+vn -0.4605 -0.2140 -0.8615
+vn -0.1752 0.9284 -0.3278
+vn -0.4469 -0.3179 -0.8362
+vn -0.2216 0.8826 -0.4147
+vn -0.0780 -0.9632 -0.2572
+vn -0.2886 0.1076 -0.9514
+vn -0.0158 0.9985 -0.0521
+vn -0.0472 -0.9867 -0.1555
+vn -0.2903 0.0000 -0.9569
+vn -0.0472 0.9867 -0.1555
+vn -0.0158 -0.9985 -0.0521
+vn -0.2886 -0.1076 -0.9514
+vn -0.0780 0.9632 -0.2572
+vn -0.2836 -0.2140 -0.9348
+vn -0.1079 0.9284 -0.3557
+vn -0.2752 -0.3179 -0.9073
+vn -0.1365 0.8826 -0.4499
+vn -0.2637 -0.4182 -0.8692
+vn -0.1634 0.8264 -0.5388
+vn -0.2491 -0.5137 -0.8210
+vn -0.1885 0.7606 -0.6212
+vn -0.2315 -0.6033 -0.7632
+vn -0.2112 0.6859 -0.6963
+vn -0.2112 -0.6859 -0.6963
+vn -0.2315 0.6033 -0.7632
+vn -0.1885 -0.7606 -0.6212
+vn -0.2491 0.5137 -0.8210
+vn -0.1634 -0.8264 -0.5388
+vn -0.2637 0.4182 -0.8692
+vn -0.1365 -0.8826 -0.4499
+vn -0.2752 0.3179 -0.9073
+vn -0.1079 -0.9284 -0.3557
+vn -0.2836 0.2140 -0.9348
+vn -0.0713 -0.6859 -0.7241
+vn -0.0782 0.6033 -0.7937
+vn -0.0636 -0.7606 -0.6461
+vn -0.0841 0.5137 -0.8538
+vn -0.0552 -0.8264 -0.5603
+vn -0.0890 0.4182 -0.9040
+vn -0.0461 -0.8826 -0.4679
+vn -0.0929 0.3179 -0.9436
+vn -0.0364 -0.9284 -0.3699
+vn -0.0957 0.2140 -0.9721
+vn -0.0263 -0.9632 -0.2674
+vn -0.0974 0.1076 -0.9894
+vn -0.0053 0.9985 -0.0541
+vn -0.0159 -0.9867 -0.1618
+vn -0.0980 0.0000 -0.9952
+vn -0.0159 0.9867 -0.1618
+vn -0.0053 -0.9985 -0.0541
+vn -0.0974 -0.1076 -0.9894
+vn -0.0263 0.9632 -0.2674
+vn -0.0957 -0.2140 -0.9721
+vn -0.0364 0.9284 -0.3699
+vn -0.0929 -0.3179 -0.9436
+vn -0.0461 0.8826 -0.4679
+vn -0.0890 -0.4182 -0.9040
+vn -0.0552 0.8264 -0.5603
+vn -0.0841 -0.5137 -0.8538
+vn -0.0636 0.7606 -0.6461
+vn -0.0782 -0.6033 -0.7937
+vn -0.0713 0.6859 -0.7241
+vn 0.0263 0.9632 -0.2674
+vn 0.0957 -0.2140 -0.9721
+vn 0.0364 0.9284 -0.3699
+vn 0.0929 -0.3179 -0.9436
+vn 0.0461 0.8826 -0.4679
+vn 0.0890 -0.4182 -0.9040
+vn 0.0552 0.8264 -0.5603
+vn 0.0841 -0.5137 -0.8538
+vn 0.0636 0.7606 -0.6461
+vn 0.0782 -0.6033 -0.7937
+vn 0.0713 0.6859 -0.7241
+vn 0.0713 -0.6859 -0.7241
+vn 0.0782 0.6033 -0.7937
+vn 0.0636 -0.7606 -0.6461
+vn 0.0841 0.5137 -0.8538
+vn 0.0552 -0.8264 -0.5603
+vn 0.0890 0.4182 -0.9040
+vn 0.0461 -0.8826 -0.4679
+vn 0.0929 0.3179 -0.9436
+vn 0.0364 -0.9284 -0.3699
+vn 0.0957 0.2140 -0.9721
+vn 0.0263 -0.9632 -0.2674
+vn 0.0974 0.1076 -0.9894
+vn 0.0053 0.9985 -0.0541
+vn 0.0159 -0.9867 -0.1618
+vn 0.0980 0.0000 -0.9952
+vn 0.0159 0.9867 -0.1618
+vn 0.0053 -0.9985 -0.0541
+vn 0.0974 -0.1076 -0.9894
+vn 0.2637 0.4182 -0.8692
+vn 0.1365 -0.8826 -0.4499
+vn 0.2752 0.3179 -0.9073
+vn 0.1079 -0.9284 -0.3557
+vn 0.2836 0.2140 -0.9348
+vn 0.0780 -0.9632 -0.2572
+vn 0.2886 0.1076 -0.9514
+vn 0.0158 0.9985 -0.0521
+vn 0.0472 -0.9867 -0.1555
+vn 0.2903 0.0000 -0.9569
+vn 0.0472 0.9867 -0.1555
+vn 0.0158 -0.9985 -0.0521
+vn 0.2886 -0.1076 -0.9514
+vn 0.0780 0.9632 -0.2572
+vn 0.2836 -0.2140 -0.9348
+vn 0.1079 0.9284 -0.3557
+vn 0.2752 -0.3179 -0.9073
+vn 0.1365 0.8826 -0.4499
+vn 0.2637 -0.4182 -0.8692
+vn 0.1634 0.8264 -0.5388
+vn 0.2491 -0.5137 -0.8210
+vn 0.1885 0.7606 -0.6212
+vn 0.2315 -0.6033 -0.7632
+vn 0.2112 0.6859 -0.6963
+vn 0.2112 -0.6859 -0.6963
+vn 0.2315 0.6033 -0.7632
+vn 0.1885 -0.7606 -0.6212
+vn 0.2491 0.5137 -0.8210
+vn 0.1634 -0.8264 -0.5388
+vn 0.4282 -0.4182 -0.8011
+vn 0.2654 0.8264 -0.4966
+vn 0.4044 -0.5137 -0.7566
+vn 0.3060 0.7606 -0.5725
+vn 0.3759 -0.6033 -0.7033
+vn 0.3430 0.6859 -0.6417
+vn 0.3430 -0.6859 -0.6417
+vn 0.3759 0.6033 -0.7033
+vn 0.3060 -0.7606 -0.5725
+vn 0.4044 0.5137 -0.7566
+vn 0.2654 -0.8264 -0.4966
+vn 0.4282 0.4182 -0.8011
+vn 0.2216 -0.8826 -0.4147
+vn 0.4469 0.3179 -0.8362
+vn 0.1752 -0.9284 -0.3278
+vn 0.4605 0.2140 -0.8615
+vn 0.1267 -0.9632 -0.2370
+vn 0.4687 0.1076 -0.8768
+vn 0.0256 0.9985 -0.0480
+vn 0.0766 -0.9867 -0.1434
+vn 0.4714 0.0000 -0.8819
+vn 0.0766 0.9867 -0.1434
+vn 0.0256 -0.9985 -0.0480
+vn 0.4687 -0.1076 -0.8768
+vn 0.1267 0.9632 -0.2370
+vn 0.4605 -0.2140 -0.8615
+vn 0.1752 0.9284 -0.3278
+vn 0.4469 -0.3179 -0.8362
+vn 0.2216 0.8826 -0.4147
+vn 0.1705 -0.9632 -0.2077
+vn 0.6307 0.1076 -0.7685
+vn 0.0345 0.9985 -0.0420
+vn 0.1031 -0.9867 -0.1256
+vn 0.6344 0.0000 -0.7730
+vn 0.1031 0.9867 -0.1256
+vn 0.0345 -0.9985 -0.0420
+vn 0.6307 -0.1076 -0.7685
+vn 0.1705 0.9632 -0.2077
+vn 0.6197 -0.2140 -0.7551
+vn 0.2358 0.9284 -0.2873
+vn 0.6015 -0.3179 -0.7329
+vn 0.2983 0.8826 -0.3635
+vn 0.5762 -0.4182 -0.7022
+vn 0.3572 0.8264 -0.4352
+vn 0.5443 -0.5137 -0.6632
+vn 0.4118 0.7606 -0.5018
+vn 0.5059 -0.6033 -0.6165
+vn 0.4616 0.6859 -0.5625
+vn 0.4616 -0.6859 -0.5625
+vn 0.5059 0.6033 -0.6165
+vn 0.4118 -0.7606 -0.5018
+vn 0.5443 0.5137 -0.6632
+vn 0.3572 -0.8264 -0.4352
+vn 0.5762 0.4182 -0.7022
+vn 0.2983 -0.8826 -0.3635
+vn 0.6015 0.3179 -0.7329
+vn 0.2358 -0.9284 -0.2873
+vn 0.6197 0.2140 -0.7551
+vn 0.5625 0.6859 -0.4616
+vn 0.5625 -0.6859 -0.4616
+vn 0.6165 0.6033 -0.5059
+vn 0.5018 -0.7606 -0.4118
+vn 0.6632 0.5137 -0.5443
+vn 0.4352 -0.8264 -0.3572
+vn 0.7022 0.4182 -0.5762
+vn 0.3635 -0.8826 -0.2983
+vn 0.7329 0.3179 -0.6015
+vn 0.2873 -0.9284 -0.2358
+vn 0.7551 0.2140 -0.6197
+vn 0.2077 -0.9632 -0.1705
+vn 0.7685 0.1076 -0.6307
+vn 0.0421 0.9985 -0.0345
+vn 0.1256 -0.9867 -0.1031
+vn 0.7730 0.0000 -0.6344
+vn 0.1256 0.9867 -0.1031
+vn 0.0421 -0.9985 -0.0345
+vn 0.7685 -0.1076 -0.6307
+vn 0.2077 0.9632 -0.1705
+vn 0.7551 -0.2140 -0.6197
+vn 0.2873 0.9284 -0.2358
+vn 0.7329 -0.3179 -0.6015
+vn 0.3635 0.8826 -0.2983
+vn 0.7022 -0.4182 -0.5762
+vn 0.4352 0.8264 -0.3572
+vn 0.6632 -0.5137 -0.5443
+vn 0.5018 0.7606 -0.4118
+vn 0.6165 -0.6033 -0.5059
+vn 0.8768 -0.1076 -0.4687
+vn 0.2370 0.9632 -0.1267
+vn 0.8615 -0.2140 -0.4605
+vn 0.3278 0.9284 -0.1752
+vn 0.8362 -0.3179 -0.4469
+vn 0.4147 0.8826 -0.2216
+vn 0.8011 -0.4182 -0.4282
+vn 0.4966 0.8264 -0.2654
+vn 0.7566 -0.5137 -0.4044
+vn 0.5725 0.7606 -0.3060
+vn 0.7033 -0.6033 -0.3759
+vn 0.6417 0.6859 -0.3430
+vn 0.6417 -0.6859 -0.3430
+vn 0.7033 0.6033 -0.3759
+vn 0.5725 -0.7606 -0.3060
+vn 0.7566 0.5137 -0.4044
+vn 0.4966 -0.8264 -0.2654
+vn 0.8011 0.4182 -0.4282
+vn 0.4147 -0.8826 -0.2216
+vn 0.8362 0.3179 -0.4469
+vn 0.3278 -0.9284 -0.1752
+vn 0.8615 0.2140 -0.4605
+vn 0.2370 -0.9632 -0.1267
+vn 0.8768 0.1076 -0.4687
+vn 0.0480 0.9985 -0.0256
+vn 0.1434 -0.9867 -0.0766
+vn 0.8819 0.0000 -0.4714
+vn 0.1434 0.9867 -0.0766
+vn 0.0480 -0.9985 -0.0256
+vn 0.5388 -0.8264 -0.1634
+vn 0.8692 0.4182 -0.2637
+vn 0.4499 -0.8826 -0.1365
+vn 0.9073 0.3179 -0.2752
+vn 0.3557 -0.9284 -0.1079
+vn 0.9348 0.2140 -0.2836
+vn 0.2572 -0.9632 -0.0780
+vn 0.9514 0.1076 -0.2886
+vn 0.0521 0.9985 -0.0158
+vn 0.1555 -0.9867 -0.0472
+vn 0.9569 0.0000 -0.2903
+vn 0.1555 0.9867 -0.0472
+vn 0.0521 -0.9985 -0.0158
+vn 0.9514 -0.1076 -0.2886
+vn 0.2572 0.9632 -0.0780
+vn 0.9348 -0.2140 -0.2836
+vn 0.3557 0.9284 -0.1079
+vn 0.9073 -0.3179 -0.2752
+vn 0.4499 0.8826 -0.1365
+vn 0.8692 -0.4182 -0.2637
+vn 0.5388 0.8264 -0.1634
+vn 0.8210 -0.5137 -0.2491
+vn 0.6212 0.7606 -0.1885
+vn 0.7632 -0.6033 -0.2315
+vn 0.6963 0.6859 -0.2112
+vn 0.6963 -0.6859 -0.2112
+vn 0.7632 0.6033 -0.2315
+vn 0.6212 -0.7606 -0.1885
+vn 0.8210 0.5137 -0.2491
+vn 0.9040 -0.4182 -0.0890
+vn 0.5603 0.8264 -0.0552
+vn 0.8538 -0.5137 -0.0841
+vn 0.6461 0.7606 -0.0636
+vn 0.7937 -0.6033 -0.0782
+vn 0.7241 0.6859 -0.0713
+vn 0.7241 -0.6859 -0.0713
+vn 0.7937 0.6033 -0.0782
+vn 0.6461 -0.7606 -0.0636
+vn 0.8538 0.5137 -0.0841
+vn 0.5603 -0.8264 -0.0552
+vn 0.9040 0.4182 -0.0890
+vn 0.4679 -0.8826 -0.0461
+vn 0.9436 0.3179 -0.0929
+vn 0.3699 -0.9284 -0.0364
+vn 0.9721 0.2140 -0.0957
+vn 0.2674 -0.9632 -0.0263
+vn 0.9894 0.1076 -0.0974
+vn 0.0541 0.9985 -0.0053
+vn 0.1618 -0.9867 -0.0159
+vn 0.9952 0.0000 -0.0980
+vn 0.1618 0.9867 -0.0159
+vn 0.0541 -0.9985 -0.0053
+vn 0.9894 -0.1076 -0.0974
+vn 0.2674 0.9632 -0.0263
+vn 0.9721 -0.2140 -0.0957
+vn 0.3699 0.9284 -0.0364
+vn 0.9436 -0.3179 -0.0929
+vn 0.4679 0.8826 -0.0461
+vn 0.2674 -0.9632 0.0263
+vn 0.9894 0.1076 0.0974
+vn 0.0541 0.9985 0.0053
+vn 0.1618 -0.9867 0.0159
+vn 0.9952 0.0000 0.0980
+vn 0.1618 0.9867 0.0159
+vn 0.0541 -0.9985 0.0053
+vn 0.9894 -0.1076 0.0974
+vn 0.2674 0.9632 0.0263
+vn 0.9721 -0.2140 0.0957
+vn 0.3699 0.9284 0.0364
+vn 0.9436 -0.3179 0.0929
+vn 0.4679 0.8826 0.0461
+vn 0.9040 -0.4182 0.0890
+vn 0.5603 0.8264 0.0552
+vn 0.8538 -0.5137 0.0841
+vn 0.6461 0.7606 0.0636
+vn 0.7937 -0.6033 0.0782
+vn 0.7241 0.6859 0.0713
+vn 0.7241 -0.6859 0.0713
+vn 0.7937 0.6033 0.0782
+vn 0.6461 -0.7606 0.0636
+vn 0.8538 0.5137 0.0841
+vn 0.5603 -0.8264 0.0552
+vn 0.9040 0.4182 0.0890
+vn 0.4679 -0.8826 0.0461
+vn 0.9436 0.3179 0.0929
+vn 0.3699 -0.9284 0.0364
+vn 0.9721 0.2140 0.0957
+vn 0.6963 0.6859 0.2112
+vn 0.6963 -0.6859 0.2112
+vn 0.7632 0.6033 0.2315
+vn 0.6212 -0.7606 0.1885
+vn 0.8210 0.5137 0.2491
+vn 0.5388 -0.8264 0.1634
+vn 0.8692 0.4182 0.2637
+vn 0.4499 -0.8826 0.1365
+vn 0.9073 0.3179 0.2752
+vn 0.3557 -0.9284 0.1079
+vn 0.9348 0.2140 0.2836
+vn 0.2572 -0.9632 0.0780
+vn 0.9514 0.1076 0.2886
+vn 0.0521 0.9985 0.0158
+vn 0.1555 -0.9867 0.0472
+vn 0.9569 0.0000 0.2903
+vn 0.1555 0.9867 0.0472
+vn 0.0521 -0.9985 0.0158
+vn 0.9514 -0.1076 0.2886
+vn 0.2572 0.9632 0.0780
+vn 0.9348 -0.2140 0.2836
+vn 0.3557 0.9284 0.1079
+vn 0.9073 -0.3179 0.2752
+vn 0.4499 0.8826 0.1365
+vn 0.8692 -0.4182 0.2637
+vn 0.5388 0.8264 0.1634
+vn 0.8210 -0.5137 0.2491
+vn 0.6212 0.7606 0.1885
+vn 0.7632 -0.6033 0.2315
+vn 0.8768 -0.1076 0.4687
+vn 0.2370 0.9632 0.1267
+vn 0.8615 -0.2140 0.4605
+vn 0.3278 0.9284 0.1752
+vn 0.8362 -0.3179 0.4469
+vn 0.4147 0.8826 0.2216
+vn 0.8011 -0.4182 0.4282
+vn 0.4966 0.8264 0.2654
+vn 0.7566 -0.5137 0.4044
+vn 0.5725 0.7606 0.3060
+vn 0.7033 -0.6033 0.3759
+vn 0.6417 0.6859 0.3430
+vn 0.6417 -0.6859 0.3430
+vn 0.7033 0.6033 0.3759
+vn 0.5725 -0.7606 0.3060
+vn 0.7566 0.5137 0.4044
+vn 0.4966 -0.8264 0.2654
+vn 0.8011 0.4182 0.4282
+vn 0.4147 -0.8826 0.2216
+vn 0.8362 0.3179 0.4469
+vn 0.3278 -0.9284 0.1752
+vn 0.8615 0.2140 0.4605
+vn 0.2370 -0.9632 0.1267
+vn 0.8768 0.1076 0.4687
+vn 0.0480 0.9985 0.0256
+vn 0.1434 -0.9867 0.0766
+vn 0.8819 0.0000 0.4714
+vn 0.1434 0.9867 0.0766
+vn 0.0480 -0.9985 0.0256
+vn 0.4352 -0.8264 0.3572
+vn 0.7022 0.4182 0.5762
+vn 0.3635 -0.8826 0.2983
+vn 0.7329 0.3179 0.6015
+vn 0.2873 -0.9284 0.2358
+vn 0.7551 0.2140 0.6197
+vn 0.2077 -0.9632 0.1705
+vn 0.7685 0.1076 0.6307
+vn 0.0421 0.9985 0.0345
+vn 0.1256 -0.9867 0.1031
+vn 0.7730 0.0000 0.6344
+vn 0.1256 0.9867 0.1031
+vn 0.0421 -0.9985 0.0345
+vn 0.7685 -0.1076 0.6307
+vn 0.2077 0.9632 0.1705
+vn 0.7551 -0.2140 0.6197
+vn 0.2873 0.9284 0.2358
+vn 0.7329 -0.3179 0.6015
+vn 0.3634 0.8826 0.2983
+vn 0.7022 -0.4182 0.5762
+vn 0.4352 0.8264 0.3572
+vn 0.6632 -0.5137 0.5443
+vn 0.5018 0.7606 0.4118
+vn 0.6165 -0.6033 0.5059
+vn 0.5625 0.6859 0.4616
+vn 0.5625 -0.6859 0.4616
+vn 0.6165 0.6033 0.5059
+vn 0.5018 -0.7606 0.4118
+vn 0.6632 0.5137 0.5443
+vn 0.2983 0.8826 0.3635
+vn 0.5762 -0.4182 0.7022
+vn 0.3572 0.8264 0.4352
+vn 0.5443 -0.5137 0.6632
+vn 0.4118 0.7606 0.5018
+vn 0.5059 -0.6033 0.6165
+vn 0.4616 0.6859 0.5625
+vn 0.4616 -0.6859 0.5625
+vn 0.5059 0.6033 0.6165
+vn 0.4118 -0.7606 0.5018
+vn 0.5443 0.5137 0.6632
+vn 0.3572 -0.8264 0.4352
+vn 0.5762 0.4182 0.7022
+vn 0.2983 -0.8826 0.3635
+vn 0.6015 0.3179 0.7329
+vn 0.2358 -0.9284 0.2873
+vn 0.6197 0.2140 0.7551
+vn 0.1705 -0.9632 0.2077
+vn 0.6307 0.1076 0.7685
+vn 0.0345 0.9985 0.0421
+vn 0.1031 -0.9867 0.1256
+vn 0.6344 0.0000 0.7730
+vn 0.1031 0.9867 0.1256
+vn 0.0345 -0.9985 0.0420
+vn 0.6307 -0.1076 0.7685
+vn 0.1705 0.9632 0.2077
+vn 0.6197 -0.2140 0.7551
+vn 0.2358 0.9284 0.2873
+vn 0.6015 -0.3179 0.7329
+vn 0.4605 0.2140 0.8615
+vn 0.1267 -0.9632 0.2370
+vn 0.4687 0.1076 0.8768
+vn 0.0256 0.9985 0.0480
+vn 0.0766 -0.9867 0.1434
+vn 0.4714 0.0000 0.8819
+vn 0.0766 0.9867 0.1434
+vn 0.0256 -0.9985 0.0480
+vn 0.4687 -0.1076 0.8768
+vn 0.1267 0.9632 0.2370
+vn 0.4605 -0.2140 0.8615
+vn 0.1752 0.9284 0.3278
+vn 0.4469 -0.3179 0.8362
+vn 0.2216 0.8826 0.4147
+vn 0.4282 -0.4182 0.8011
+vn 0.2654 0.8264 0.4966
+vn 0.4044 -0.5137 0.7566
+vn 0.3060 0.7606 0.5725
+vn 0.3759 -0.6033 0.7033
+vn 0.3430 0.6859 0.6417
+vn 0.3430 -0.6859 0.6417
+vn 0.3759 0.6033 0.7033
+vn 0.3060 -0.7606 0.5725
+vn 0.4044 0.5137 0.7566
+vn 0.2654 -0.8264 0.4966
+vn 0.4282 0.4182 0.8011
+vn 0.2216 -0.8826 0.4147
+vn 0.4469 0.3179 0.8362
+vn 0.1752 -0.9284 0.3278
+vn 0.2315 -0.6033 0.7632
+vn 0.2112 0.6859 0.6963
+vn 0.2112 -0.6859 0.6963
+vn 0.2315 0.6033 0.7632
+vn 0.1885 -0.7606 0.6212
+vn 0.2491 0.5137 0.8210
+vn 0.1634 -0.8264 0.5388
+vn 0.2637 0.4182 0.8692
+vn 0.1365 -0.8826 0.4499
+vn 0.2752 0.3179 0.9073
+vn 0.1079 -0.9284 0.3557
+vn 0.2836 0.2140 0.9348
+vn 0.0780 -0.9632 0.2572
+vn 0.2886 0.1076 0.9514
+vn 0.0158 0.9985 0.0521
+vn 0.0472 -0.9867 0.1555
+vn 0.2903 0.0000 0.9569
+vn 0.0472 0.9867 0.1555
+vn 0.0158 -0.9985 0.0521
+vn 0.2886 -0.1076 0.9514
+vn 0.0780 0.9632 0.2572
+vn 0.2836 -0.2140 0.9348
+vn 0.1079 0.9284 0.3557
+vn 0.2752 -0.3179 0.9073
+vn 0.1365 0.8826 0.4499
+vn 0.2637 -0.4182 0.8692
+vn 0.1634 0.8264 0.5388
+vn 0.2491 -0.5137 0.8210
+vn 0.1885 0.7606 0.6212
+vn 0.0053 -0.9985 0.0541
+vn 0.0974 -0.1076 0.9894
+vn 0.0263 0.9632 0.2674
+vn 0.0957 -0.2140 0.9721
+vn 0.0364 0.9284 0.3699
+vn 0.0929 -0.3179 0.9436
+vn 0.0461 0.8826 0.4679
+vn 0.0890 -0.4182 0.9040
+vn 0.0552 0.8264 0.5603
+vn 0.0841 -0.5137 0.8538
+vn 0.0636 0.7606 0.6461
+vn 0.0782 -0.6033 0.7937
+vn 0.0713 0.6859 0.7241
+vn 0.0713 -0.6859 0.7241
+vn 0.0782 0.6033 0.7937
+vn 0.0636 -0.7606 0.6461
+vn 0.0841 0.5137 0.8538
+vn 0.0552 -0.8264 0.5603
+vn 0.0890 0.4182 0.9040
+vn 0.0461 -0.8826 0.4679
+vn 0.0929 0.3179 0.9436
+vn 0.0364 -0.9284 0.3699
+vn 0.0957 0.2140 0.9721
+vn 0.0263 -0.9632 0.2674
+vn 0.0974 0.1076 0.9894
+vn 0.0053 0.9985 0.0541
+vn 0.0159 -0.9867 0.1618
+vn 0.0980 0.0000 0.9952
+vn 0.0159 0.9867 0.1618
+vn -0.0552 -0.8264 0.5603
+vn -0.0890 0.4182 0.9040
+vn -0.0461 -0.8826 0.4679
+vn -0.0929 0.3179 0.9436
+vn -0.0364 -0.9284 0.3699
+vn -0.0957 0.2140 0.9721
+vn -0.0263 -0.9632 0.2674
+vn -0.0974 0.1076 0.9894
+vn -0.0053 0.9985 0.0541
+vn -0.0159 -0.9867 0.1618
+vn -0.0980 0.0000 0.9952
+vn -0.0159 0.9867 0.1618
+vn -0.0053 -0.9985 0.0541
+vn -0.0974 -0.1076 0.9894
+vn -0.0263 0.9632 0.2674
+vn -0.0957 -0.2140 0.9721
+vn -0.0364 0.9284 0.3699
+vn -0.0929 -0.3179 0.9436
+vn -0.0461 0.8826 0.4679
+vn -0.0890 -0.4182 0.9040
+vn -0.0552 0.8264 0.5603
+vn -0.0841 -0.5137 0.8538
+vn -0.0636 0.7606 0.6461
+vn -0.0782 -0.6033 0.7937
+vn -0.0713 0.6859 0.7241
+vn -0.0713 -0.6859 0.7241
+vn -0.0782 0.6033 0.7937
+vn -0.0636 -0.7606 0.6461
+vn -0.0841 0.5137 0.8538
+vn -0.1365 0.8826 0.4499
+vn -0.2637 -0.4182 0.8692
+vn -0.1634 0.8264 0.5388
+vn -0.2491 -0.5137 0.8210
+vn -0.1885 0.7606 0.6212
+vn -0.2315 -0.6033 0.7632
+vn -0.2112 0.6859 0.6963
+vn -0.2112 -0.6859 0.6963
+vn -0.2315 0.6033 0.7632
+vn -0.1885 -0.7606 0.6212
+vn -0.2491 0.5137 0.8210
+vn -0.1634 -0.8264 0.5388
+vn -0.2637 0.4182 0.8692
+vn -0.1365 -0.8826 0.4499
+vn -0.2752 0.3179 0.9073
+vn -0.1079 -0.9284 0.3557
+vn -0.2836 0.2140 0.9348
+vn -0.0780 -0.9632 0.2572
+vn -0.2886 0.1076 0.9514
+vn -0.0158 0.9985 0.0521
+vn -0.0472 -0.9867 0.1555
+vn -0.2903 0.0000 0.9569
+vn -0.0472 0.9867 0.1555
+vn -0.0158 -0.9985 0.0521
+vn -0.2886 -0.1076 0.9514
+vn -0.0780 0.9632 0.2572
+vn -0.2836 -0.2140 0.9348
+vn -0.1079 0.9284 0.3557
+vn -0.2752 -0.3179 0.9073
+vn -0.4605 0.2140 0.8615
+vn -0.1267 -0.9632 0.2370
+vn -0.4687 0.1076 0.8768
+vn -0.0256 0.9985 0.0480
+vn -0.0766 -0.9867 0.1434
+vn -0.4714 0.0000 0.8819
+vn -0.0766 0.9867 0.1434
+vn -0.0256 -0.9985 0.0480
+vn -0.4687 -0.1076 0.8768
+vn -0.1267 0.9632 0.2370
+vn -0.4605 -0.2140 0.8615
+vn -0.1752 0.9284 0.3278
+vn -0.4469 -0.3179 0.8362
+vn -0.2216 0.8826 0.4147
+vn -0.4282 -0.4182 0.8011
+vn -0.2654 0.8264 0.4966
+vn -0.4044 -0.5137 0.7566
+vn -0.3060 0.7606 0.5725
+vn -0.3759 -0.6033 0.7033
+vn -0.3430 0.6859 0.6417
+vn -0.3430 -0.6859 0.6417
+vn -0.3759 0.6033 0.7033
+vn -0.3060 -0.7606 0.5725
+vn -0.4044 0.5137 0.7566
+vn -0.2654 -0.8264 0.4966
+vn -0.4282 0.4182 0.8011
+vn -0.2216 -0.8826 0.4147
+vn -0.4469 0.3179 0.8362
+vn -0.1752 -0.9284 0.3278
+vn -0.5059 -0.6033 0.6165
+vn -0.4616 0.6859 0.5625
+vn -0.4616 -0.6859 0.5625
+vn -0.5059 0.6033 0.6165
+vn -0.4118 -0.7606 0.5018
+vn -0.5443 0.5137 0.6632
+vn -0.3572 -0.8264 0.4352
+vn -0.5762 0.4182 0.7022
+vn -0.2983 -0.8826 0.3635
+vn -0.6015 0.3179 0.7329
+vn -0.2358 -0.9284 0.2873
+vn -0.6197 0.2140 0.7551
+vn -0.1705 -0.9632 0.2077
+vn -0.6307 0.1076 0.7685
+vn -0.0345 0.9985 0.0421
+vn -0.1031 -0.9867 0.1256
+vn -0.6344 0.0000 0.7730
+vn -0.1031 0.9867 0.1256
+vn -0.0345 -0.9985 0.0421
+vn -0.6307 -0.1076 0.7685
+vn -0.1705 0.9632 0.2077
+vn -0.6197 -0.2140 0.7551
+vn -0.2358 0.9284 0.2873
+vn -0.6015 -0.3179 0.7329
+vn -0.2983 0.8826 0.3634
+vn -0.5762 -0.4182 0.7022
+vn -0.3572 0.8264 0.4352
+vn -0.5443 -0.5137 0.6632
+vn -0.4118 0.7606 0.5018
+vn -0.1256 0.9867 0.1031
+vn -0.7685 -0.1076 0.6307
+vn -0.1256 -0.9867 0.1031
+vn -0.2077 0.9632 0.1705
+vn -0.0421 -0.9985 0.0345
+vn -0.2873 0.9284 0.2358
+vn -0.3634 0.8826 0.2983
+vn -0.7551 -0.2140 0.6197
+vn -0.4352 0.8264 0.3572
+vn -0.7329 -0.3179 0.6015
+vn -0.5018 0.7606 0.4118
+vn -0.7022 -0.4182 0.5762
+vn -0.5625 0.6859 0.4616
+vn -0.6632 -0.5137 0.5443
+vn -0.6165 0.6033 0.5059
+vn -0.6165 -0.6033 0.5059
+vn -0.6632 0.5137 0.5443
+vn -0.5625 -0.6859 0.4616
+vn -0.7022 0.4182 0.5762
+vn -0.5018 -0.7606 0.4118
+vn -0.7329 0.3179 0.6015
+vn -0.4352 -0.8264 0.3572
+vn -0.7551 0.2140 0.6197
+vn -0.3635 -0.8826 0.2983
+vn -0.7685 0.1076 0.6307
+vn -0.2873 -0.9284 0.2358
+vn -0.0421 0.9985 0.0345
+vn -0.7730 0.0000 0.6344
+vn -0.2077 -0.9632 0.1705
+vn -0.6417 -0.6859 0.3430
+vn -0.7033 0.6033 0.3759
+vn -0.5725 -0.7606 0.3060
+vn -0.7566 0.5137 0.4044
+vn -0.4966 -0.8264 0.2654
+vn -0.8011 0.4182 0.4282
+vn -0.4147 -0.8826 0.2216
+vn -0.8362 0.3179 0.4469
+vn -0.3278 -0.9284 0.1752
+vn -0.8615 0.2140 0.4605
+vn -0.2370 -0.9632 0.1267
+vn -0.8768 0.1076 0.4687
+vn -0.0480 0.9985 0.0256
+vn -0.1434 -0.9867 0.0766
+vn -0.8819 0.0000 0.4714
+vn -0.1434 0.9867 0.0766
+vn -0.0480 -0.9985 0.0256
+vn -0.8768 -0.1076 0.4687
+vn -0.2370 0.9632 0.1267
+vn -0.8615 -0.2140 0.4605
+vn -0.3278 0.9284 0.1752
+vn -0.8362 -0.3179 0.4469
+vn -0.4147 0.8826 0.2216
+vn -0.8011 -0.4182 0.4282
+vn -0.4966 0.8264 0.2654
+vn -0.7566 -0.5137 0.4044
+vn -0.5725 0.7606 0.3060
+vn -0.7033 -0.6033 0.3759
+vn -0.6417 0.6859 0.3430
+vn -0.9348 -0.2140 0.2836
+vn -0.3557 0.9284 0.1079
+vn -0.9073 -0.3179 0.2752
+vn -0.4499 0.8826 0.1365
+vn -0.8692 -0.4182 0.2637
+vn -0.5388 0.8264 0.1634
+vn -0.8210 -0.5137 0.2491
+vn -0.6212 0.7606 0.1885
+vn -0.7632 -0.6033 0.2315
+vn -0.6963 0.6859 0.2112
+vn -0.6963 -0.6859 0.2112
+vn -0.7632 0.6033 0.2315
+vn -0.6212 -0.7606 0.1885
+vn -0.8210 0.5137 0.2491
+vn -0.5388 -0.8264 0.1634
+vn -0.8692 0.4182 0.2637
+vn -0.4499 -0.8826 0.1365
+vn -0.9073 0.3179 0.2752
+vn -0.3557 -0.9284 0.1079
+vn -0.9348 0.2140 0.2836
+vn -0.2572 -0.9632 0.0780
+vn -0.9514 0.1076 0.2886
+vn -0.0521 0.9985 0.0158
+vn -0.1555 -0.9867 0.0472
+vn -0.9569 0.0000 0.2903
+vn -0.1555 0.9867 0.0472
+vn -0.0521 -0.9985 0.0158
+vn -0.9514 -0.1076 0.2886
+vn -0.2572 0.9632 0.0780
+vn -0.4679 -0.8826 0.0461
+vn -0.9436 0.3179 0.0929
+vn -0.3699 -0.9284 0.0364
+vn -0.9721 0.2140 0.0958
+vn -0.2674 -0.9632 0.0263
+vn -0.9894 0.1076 0.0975
+vn -0.0541 0.9985 0.0053
+vn -0.1618 -0.9867 0.0159
+vn -0.9952 0.0000 0.0980
+vn -0.1618 0.9867 0.0159
+vn -0.0541 -0.9985 0.0053
+vn -0.9894 -0.1076 0.0975
+vn -0.2674 0.9632 0.0263
+vn -0.9721 -0.2140 0.0958
+vn -0.3699 0.9284 0.0364
+vn -0.9436 -0.3179 0.0929
+vn -0.4679 0.8826 0.0461
+vn -0.9040 -0.4182 0.0890
+vn -0.5603 0.8264 0.0552
+vn -0.8538 -0.5137 0.0841
+vn -0.6461 0.7606 0.0636
+vn -0.7937 -0.6033 0.0782
+vn -0.7241 0.6859 0.0713
+vn -0.7241 -0.6859 0.0713
+vn -0.7937 0.6033 0.0782
+vn -0.6461 -0.7606 0.0636
+vn -0.8538 0.5137 0.0841
+vn -0.5603 -0.8264 0.0552
+vn -0.9040 0.4182 0.0890
+usemtl None
+s off
+f 16/1/1 15/2/1 43/3/1 44/4/1
+f 3/5/2 2/6/2 30/7/2 31/8/2
+f 17/9/3 16/1/3 44/4/3 45/10/3
+f 4/11/4 3/5/4 31/8/4 32/12/4
+f 18/13/5 17/9/5 45/10/5 46/14/5
+f 5/15/6 4/11/6 32/12/6 33/16/6
+f 19/17/7 18/13/7 46/14/7 47/18/7
+f 6/19/8 5/15/8 33/16/8 34/20/8
+f 20/21/9 19/17/9 47/18/9 48/22/9
+f 7/23/10 6/19/10 34/20/10 35/24/10
+f 21/25/11 20/21/11 48/22/11 49/26/11
+f 8/27/12 7/23/12 35/24/12 36/28/12
+f 22/29/13 21/25/13 49/26/13 50/30/13
+f 9/31/14 8/27/14 36/28/14 37/32/14
+f 23/33/15 22/29/15 50/30/15 51/34/15
+f 10/35/16 9/31/16 37/32/16 38/36/16
+f 24/37/17 23/33/17 51/34/17 52/38/17
+f 11/39/18 10/35/18 38/36/18 39/40/18
+f 25/41/19 24/37/19 52/38/19 53/42/19
+f 12/43/20 11/39/20 39/40/20 40/44/20
+f 26/45/21 25/41/21 53/42/21 54/46/21
+f 13/47/22 12/43/22 40/44/22 41/48/22
+f 27/49/23 26/45/23 54/46/23 55/50/23
+f 14/51/24 13/47/24 41/48/24 42/52/24
+f 1/53/25 337/54/25 29/55/25
+f 28/56/26 27/49/26 55/50/26 56/57/26
+f 15/2/27 14/51/27 42/52/27 43/3/27
+f 2/6/28 1/58/28 29/59/28 30/7/28
+f 366/60/29 28/61/29 56/62/29
+f 42/52/30 41/48/30 69/63/30 70/64/30
+f 29/55/31 337/54/31 57/65/31
+f 56/57/32 55/50/32 83/66/32 84/67/32
+f 43/3/33 42/52/33 70/64/33 71/68/33
+f 30/7/34 29/59/34 57/69/34 58/70/34
+f 366/60/35 56/62/35 84/71/35
+f 44/4/36 43/3/36 71/68/36 72/72/36
+f 31/8/37 30/7/37 58/70/37 59/73/37
+f 45/10/38 44/4/38 72/72/38 73/74/38
+f 32/12/39 31/8/39 59/73/39 60/75/39
+f 46/14/40 45/10/40 73/74/40 74/76/40
+f 33/16/41 32/12/41 60/75/41 61/77/41
+f 47/18/42 46/14/42 74/76/42 75/78/42
+f 34/20/43 33/16/43 61/77/43 62/79/43
+f 48/22/44 47/18/44 75/78/44 76/80/44
+f 35/24/45 34/20/45 62/79/45 63/81/45
+f 49/26/46 48/22/46 76/80/46 77/82/46
+f 36/28/47 35/24/47 63/81/47 64/83/47
+f 50/30/48 49/26/48 77/82/48 78/84/48
+f 37/32/49 36/28/49 64/83/49 65/85/49
+f 51/34/50 50/30/50 78/84/50 79/86/50
+f 38/36/51 37/32/51 65/85/51 66/87/51
+f 52/38/52 51/34/52 79/86/52 80/88/52
+f 39/40/53 38/36/53 66/87/53 67/89/53
+f 53/42/54 52/38/54 80/88/54 81/90/54
+f 40/44/55 39/40/55 67/89/55 68/91/55
+f 54/46/56 53/42/56 81/90/56 82/92/56
+f 41/48/57 40/44/57 68/91/57 69/63/57
+f 55/50/58 54/46/58 82/92/58 83/66/58
+f 78/84/59 77/82/59 105/93/59 106/94/59
+f 65/85/60 64/83/60 92/95/60 93/96/60
+f 79/86/61 78/84/61 106/94/61 107/97/61
+f 66/87/62 65/85/62 93/96/62 94/98/62
+f 80/88/63 79/86/63 107/97/63 108/99/63
+f 67/89/64 66/87/64 94/98/64 95/100/64
+f 81/90/65 80/88/65 108/99/65 109/101/65
+f 68/91/66 67/89/66 95/100/66 96/102/66
+f 82/92/67 81/90/67 109/101/67 110/103/67
+f 69/63/68 68/91/68 96/102/68 97/104/68
+f 83/66/69 82/92/69 110/103/69 111/105/69
+f 70/64/70 69/63/70 97/104/70 98/106/70
+f 57/65/71 337/54/71 85/107/71
+f 84/67/72 83/66/72 111/105/72 112/108/72
+f 71/68/73 70/64/73 98/106/73 99/109/73
+f 58/70/74 57/69/74 85/110/74 86/111/74
+f 366/60/75 84/71/75 112/112/75
+f 72/72/76 71/68/76 99/109/76 100/113/76
+f 59/73/77 58/70/77 86/111/77 87/114/77
+f 73/74/78 72/72/78 100/113/78 101/115/78
+f 60/75/79 59/73/79 87/114/79 88/116/79
+f 74/76/80 73/74/80 101/115/80 102/117/80
+f 61/77/81 60/75/81 88/116/81 89/118/81
+f 75/78/82 74/76/82 102/117/82 103/119/82
+f 62/79/83 61/77/83 89/118/83 90/120/83
+f 76/80/84 75/78/84 103/119/84 104/121/84
+f 63/81/85 62/79/85 90/120/85 91/122/85
+f 77/82/86 76/80/86 104/121/86 105/93/86
+f 64/83/87 63/81/87 91/122/87 92/95/87
+f 87/114/88 86/111/88 114/123/88 115/124/88
+f 101/115/89 100/113/89 128/125/89 129/126/89
+f 88/116/90 87/114/90 115/124/90 116/127/90
+f 102/117/91 101/115/91 129/126/91 130/128/91
+f 89/118/92 88/116/92 116/127/92 117/129/92
+f 103/119/93 102/117/93 130/128/93 131/130/93
+f 90/120/94 89/118/94 117/129/94 118/131/94
+f 104/121/95 103/119/95 131/130/95 132/132/95
+f 91/122/96 90/120/96 118/131/96 119/133/96
+f 105/93/97 104/121/97 132/132/97 133/134/97
+f 92/95/98 91/122/98 119/133/98 120/135/98
+f 106/94/99 105/93/99 133/134/99 134/136/99
+f 93/96/100 92/95/100 120/135/100 121/137/100
+f 107/97/101 106/94/101 134/136/101 135/138/101
+f 94/98/102 93/96/102 121/137/102 122/139/102
+f 108/99/103 107/97/103 135/138/103 136/140/103
+f 95/100/104 94/98/104 122/139/104 123/141/104
+f 109/101/105 108/99/105 136/140/105 137/142/105
+f 96/102/106 95/100/106 123/141/106 124/143/106
+f 110/103/107 109/101/107 137/142/107 138/144/107
+f 97/104/108 96/102/108 124/143/108 125/145/108
+f 111/105/109 110/103/109 138/144/109 139/146/109
+f 98/106/110 97/104/110 125/145/110 126/147/110
+f 85/107/111 337/54/111 113/148/111
+f 112/108/112 111/105/112 139/146/112 140/149/112
+f 99/109/113 98/106/113 126/147/113 127/150/113
+f 86/111/114 85/110/114 113/151/114 114/123/114
+f 366/60/115 112/112/115 140/152/115
+f 100/113/116 99/109/116 127/150/116 128/125/116
+f 123/141/117 122/139/117 150/153/117 151/154/117
+f 137/142/118 136/140/118 164/155/118 165/156/118
+f 124/143/119 123/141/119 151/154/119 152/157/119
+f 138/144/120 137/142/120 165/156/120 166/158/120
+f 125/145/121 124/143/121 152/157/121 153/159/121
+f 139/146/122 138/144/122 166/158/122 167/160/122
+f 126/147/123 125/145/123 153/159/123 154/161/123
+f 113/148/124 337/54/124 141/162/124
+f 140/149/125 139/146/125 167/160/125 168/163/125
+f 127/150/126 126/147/126 154/161/126 155/164/126
+f 114/123/127 113/151/127 141/165/127 142/166/127
+f 366/60/128 140/152/128 168/167/128
+f 128/125/129 127/150/129 155/164/129 156/168/129
+f 115/124/130 114/123/130 142/166/130 143/169/130
+f 129/126/131 128/125/131 156/168/131 157/170/131
+f 116/127/132 115/124/132 143/169/132 144/171/132
+f 130/128/133 129/126/133 157/170/133 158/172/133
+f 117/129/134 116/127/134 144/171/134 145/173/134
+f 131/130/135 130/128/135 158/172/135 159/174/135
+f 118/131/136 117/129/136 145/173/136 146/175/136
+f 132/132/137 131/130/137 159/174/137 160/176/137
+f 119/133/138 118/131/138 146/175/138 147/177/138
+f 133/134/139 132/132/139 160/176/139 161/178/139
+f 120/135/140 119/133/140 147/177/140 148/179/140
+f 134/136/141 133/134/141 161/178/141 162/180/141
+f 121/137/142 120/135/142 148/179/142 149/181/142
+f 135/138/143 134/136/143 162/180/143 163/182/143
+f 122/139/144 121/137/144 149/181/144 150/153/144
+f 136/140/145 135/138/145 163/182/145 164/155/145
+f 159/174/146 158/172/146 186/183/146 187/184/146
+f 146/175/147 145/173/147 173/185/147 174/186/147
+f 160/176/148 159/174/148 187/184/148 188/187/148
+f 147/177/149 146/175/149 174/186/149 175/188/149
+f 161/178/150 160/176/150 188/187/150 189/189/150
+f 148/179/151 147/177/151 175/188/151 176/190/151
+f 162/180/152 161/178/152 189/189/152 190/191/152
+f 149/181/153 148/179/153 176/190/153 177/192/153
+f 163/182/154 162/180/154 190/191/154 191/193/154
+f 150/153/155 149/181/155 177/192/155 178/194/155
+f 164/155/156 163/182/156 191/193/156 192/195/156
+f 151/154/157 150/153/157 178/194/157 179/196/157
+f 165/156/158 164/155/158 192/195/158 193/197/158
+f 152/157/159 151/154/159 179/196/159 180/198/159
+f 166/158/160 165/156/160 193/197/160 194/199/160
+f 153/159/161 152/157/161 180/198/161 181/200/161
+f 167/160/162 166/158/162 194/199/162 195/201/162
+f 154/161/163 153/159/163 181/200/163 182/202/163
+f 141/162/164 337/54/164 169/203/164
+f 168/163/165 167/160/165 195/201/165 196/204/165
+f 155/164/166 154/161/166 182/202/166 183/205/166
+f 142/166/167 141/165/167 169/206/167 170/207/167
+f 366/60/168 168/167/168 196/208/168
+f 156/168/169 155/164/169 183/205/169 184/209/169
+f 143/169/170 142/166/170 170/207/170 171/210/170
+f 157/170/171 156/168/171 184/209/171 185/211/171
+f 144/171/172 143/169/172 171/210/172 172/212/172
+f 158/172/173 157/170/173 185/211/173 186/183/173
+f 145/173/174 144/171/174 172/212/174 173/185/174
+f 195/201/175 194/199/175 222/213/175 223/214/175
+f 182/202/176 181/200/176 209/215/176 210/216/176
+f 169/203/177 337/54/177 197/217/177
+f 196/204/178 195/201/178 223/214/178 224/218/178
+f 183/205/179 182/202/179 210/216/179 211/219/179
+f 170/207/180 169/206/180 197/220/180 198/221/180
+f 366/60/181 196/208/181 224/222/181
+f 184/209/182 183/205/182 211/219/182 212/223/182
+f 171/210/183 170/207/183 198/221/183 199/224/183
+f 185/211/184 184/209/184 212/223/184 213/225/184
+f 172/212/185 171/210/185 199/224/185 200/226/185
+f 186/183/186 185/211/186 213/225/186 214/227/186
+f 173/185/187 172/212/187 200/226/187 201/228/187
+f 187/184/188 186/183/188 214/227/188 215/229/188
+f 174/186/189 173/185/189 201/228/189 202/230/189
+f 188/187/190 187/184/190 215/229/190 216/231/190
+f 175/188/191 174/186/191 202/230/191 203/232/191
+f 189/189/192 188/187/192 216/231/192 217/233/192
+f 176/190/193 175/188/193 203/232/193 204/234/193
+f 190/191/194 189/189/194 217/233/194 218/235/194
+f 177/192/195 176/190/195 204/234/195 205/236/195
+f 191/193/196 190/191/196 218/235/196 219/237/196
+f 178/194/197 177/192/197 205/236/197 206/238/197
+f 192/195/198 191/193/198 219/237/198 220/239/198
+f 179/196/199 178/194/199 206/238/199 207/240/199
+f 193/197/200 192/195/200 220/239/200 221/241/200
+f 180/198/201 179/196/201 207/240/201 208/242/201
+f 194/199/202 193/197/202 221/241/202 222/213/202
+f 181/200/203 180/198/203 208/242/203 209/215/203
+f 218/235/204 217/233/204 245/243/204 246/244/204
+f 205/236/205 204/234/205 232/245/205 233/246/205
+f 219/237/206 218/235/206 246/244/206 247/247/206
+f 206/238/207 205/236/207 233/246/207 234/248/207
+f 220/239/208 219/237/208 247/247/208 248/249/208
+f 207/240/209 206/238/209 234/248/209 235/250/209
+f 221/241/210 220/239/210 248/249/210 249/251/210
+f 208/242/211 207/240/211 235/250/211 236/252/211
+f 222/213/212 221/241/212 249/251/212 250/253/212
+f 209/215/213 208/242/213 236/252/213 237/254/213
+f 223/214/214 222/213/214 250/253/214 251/255/214
+f 210/216/215 209/215/215 237/254/215 238/256/215
+f 197/217/216 337/54/216 225/257/216
+f 224/218/217 223/214/217 251/255/217 252/258/217
+f 211/219/218 210/216/218 238/256/218 239/259/218
+f 198/221/219 197/220/219 225/260/219 226/261/219
+f 366/60/220 224/222/220 252/262/220
+f 212/223/221 211/219/221 239/259/221 240/263/221
+f 199/224/222 198/221/222 226/261/222 227/264/222
+f 213/225/223 212/223/223 240/263/223 241/265/223
+f 200/226/224 199/224/224 227/264/224 228/266/224
+f 214/227/225 213/225/225 241/265/225 242/267/225
+f 201/228/226 200/226/226 228/266/226 229/268/226
+f 215/229/227 214/227/227 242/267/227 243/269/227
+f 202/230/228 201/228/228 229/268/228 230/270/228
+f 216/231/229 215/229/229 243/269/229 244/271/229
+f 203/232/230 202/230/230 230/270/230 231/272/230
+f 217/233/231 216/231/231 244/271/231 245/243/231
+f 204/234/232 203/232/232 231/272/232 232/245/232
+f 227/264/233 226/261/233 254/273/233 255/274/233
+f 241/265/234 240/263/234 268/275/234 269/276/234
+f 228/266/235 227/264/235 255/274/235 256/277/235
+f 242/267/236 241/265/236 269/276/236 270/278/236
+f 229/268/237 228/266/237 256/277/237 257/279/237
+f 243/269/238 242/267/238 270/278/238 271/280/238
+f 230/270/239 229/268/239 257/279/239 258/281/239
+f 244/271/240 243/269/240 271/280/240 272/282/240
+f 231/272/241 230/270/241 258/281/241 259/283/241
+f 245/243/242 244/271/242 272/282/242 273/284/242
+f 232/245/243 231/272/243 259/283/243 260/285/243
+f 246/244/244 245/243/244 273/284/244 274/286/244
+f 233/246/245 232/245/245 260/285/245 261/287/245
+f 247/247/246 246/244/246 274/286/246 275/288/246
+f 234/248/247 233/246/247 261/287/247 262/289/247
+f 248/249/248 247/247/248 275/288/248 276/290/248
+f 235/250/249 234/248/249 262/289/249 263/291/249
+f 249/251/250 248/249/250 276/290/250 277/292/250
+f 236/252/251 235/250/251 263/291/251 264/293/251
+f 250/253/252 249/251/252 277/292/252 278/294/252
+f 237/254/253 236/252/253 264/293/253 265/295/253
+f 251/255/254 250/253/254 278/294/254 279/296/254
+f 238/256/255 237/254/255 265/295/255 266/297/255
+f 225/257/256 337/54/256 253/298/256
+f 252/258/257 251/255/257 279/296/257 280/299/257
+f 239/259/258 238/256/258 266/297/258 267/300/258
+f 226/261/259 225/260/259 253/301/259 254/273/259
+f 366/60/260 252/262/260 280/302/260
+f 240/263/261 239/259/261 267/300/261 268/275/261
+f 263/291/262 262/289/262 290/303/262 291/304/262
+f 277/292/263 276/290/263 304/305/263 305/306/263
+f 264/293/264 263/291/264 291/304/264 292/307/264
+f 278/294/265 277/292/265 305/306/265 306/308/265
+f 265/295/266 264/293/266 292/307/266 293/309/266
+f 279/296/267 278/294/267 306/308/267 307/310/267
+f 266/297/268 265/295/268 293/309/268 294/311/268
+f 253/298/269 337/54/269 281/312/269
+f 280/299/270 279/296/270 307/310/270 308/313/270
+f 267/300/271 266/297/271 294/311/271 295/314/271
+f 254/273/272 253/301/272 281/315/272 282/316/272
+f 366/60/273 280/302/273 308/317/273
+f 268/275/274 267/300/274 295/314/274 296/318/274
+f 255/274/275 254/273/275 282/316/275 283/319/275
+f 269/276/276 268/275/276 296/318/276 297/320/276
+f 256/277/277 255/274/277 283/319/277 284/321/277
+f 270/278/278 269/276/278 297/320/278 298/322/278
+f 257/279/279 256/277/279 284/321/279 285/323/279
+f 271/280/280 270/278/280 298/322/280 299/324/280
+f 258/281/281 257/279/281 285/323/281 286/325/281
+f 272/282/282 271/280/282 299/324/282 300/326/282
+f 259/283/283 258/281/283 286/325/283 287/327/283
+f 273/284/284 272/282/284 300/326/284 301/328/284
+f 260/285/285 259/283/285 287/327/285 288/329/285
+f 274/286/286 273/284/286 301/328/286 302/330/286
+f 261/287/287 260/285/287 288/329/287 289/331/287
+f 275/288/288 274/286/288 302/330/288 303/332/288
+f 262/289/289 261/287/289 289/331/289 290/303/289
+f 276/290/290 275/288/290 303/332/290 304/305/290
+f 299/324/291 298/322/291 326/333/291 327/334/291
+f 286/325/292 285/323/292 313/335/292 314/336/292
+f 300/326/293 299/324/293 327/334/293 328/337/293
+f 287/327/294 286/325/294 314/336/294 315/338/294
+f 301/328/295 300/326/295 328/337/295 329/339/295
+f 288/329/296 287/327/296 315/338/296 316/340/296
+f 302/330/297 301/328/297 329/339/297 330/341/297
+f 289/331/298 288/329/298 316/340/298 317/342/298
+f 303/332/299 302/330/299 330/341/299 331/343/299
+f 290/303/300 289/331/300 317/342/300 318/344/300
+f 304/305/301 303/332/301 331/343/301 332/345/301
+f 291/304/302 290/303/302 318/344/302 319/346/302
+f 305/306/303 304/305/303 332/345/303 333/347/303
+f 292/307/304 291/304/304 319/346/304 320/348/304
+f 306/308/305 305/306/305 333/347/305 334/349/305
+f 293/309/306 292/307/306 320/348/306 321/350/306
+f 307/310/307 306/308/307 334/349/307 335/351/307
+f 294/311/308 293/309/308 321/350/308 322/352/308
+f 281/312/309 337/54/309 309/353/309
+f 308/313/310 307/310/310 335/351/310 336/354/310
+f 295/314/311 294/311/311 322/352/311 323/355/311
+f 282/316/312 281/315/312 309/356/312 310/357/312
+f 366/60/313 308/317/313 336/358/313
+f 296/318/314 295/314/314 323/355/314 324/359/314
+f 283/319/315 282/316/315 310/357/315 311/360/315
+f 297/320/316 296/318/316 324/359/316 325/361/316
+f 284/321/317 283/319/317 311/360/317 312/362/317
+f 298/322/318 297/320/318 325/361/318 326/333/318
+f 285/323/319 284/321/319 312/362/319 313/335/319
+f 335/351/320 334/349/320 363/363/320 364/364/320
+f 322/352/321 321/350/321 350/365/321 351/366/321
+f 309/353/322 337/54/322 338/367/322
+f 336/354/323 335/351/323 364/364/323 365/368/323
+f 323/355/324 322/352/324 351/366/324 352/369/324
+f 310/357/325 309/356/325 338/370/325 339/371/325
+f 366/60/326 336/358/326 365/372/326
+f 324/359/327 323/355/327 352/369/327 353/373/327
+f 311/360/328 310/357/328 339/371/328 340/374/328
+f 325/361/329 324/359/329 353/373/329 354/375/329
+f 312/362/330 311/360/330 340/374/330 341/376/330
+f 326/333/331 325/361/331 354/375/331 355/377/331
+f 313/335/332 312/362/332 341/376/332 342/378/332
+f 327/334/333 326/333/333 355/377/333 356/379/333
+f 314/336/334 313/335/334 342/378/334 343/380/334
+f 328/337/335 327/334/335 356/379/335 357/381/335
+f 315/338/336 314/336/336 343/380/336 344/382/336
+f 329/339/337 328/337/337 357/381/337 358/383/337
+f 316/340/338 315/338/338 344/382/338 345/384/338
+f 330/341/339 329/339/339 358/383/339 359/385/339
+f 317/342/340 316/340/340 345/384/340 346/386/340
+f 331/343/341 330/341/341 359/385/341 360/387/341
+f 318/344/342 317/342/342 346/386/342 347/388/342
+f 332/345/343 331/343/343 360/387/343 361/389/343
+f 319/346/344 318/344/344 347/388/344 348/390/344
+f 333/347/345 332/345/345 361/389/345 362/391/345
+f 320/348/346 319/346/346 348/390/346 349/392/346
+f 334/349/347 333/347/347 362/391/347 363/363/347
+f 321/350/348 320/348/348 349/392/348 350/365/348
+f 345/384/349 344/382/349 373/393/349 374/394/349
+f 359/385/350 358/383/350 387/395/350 388/396/350
+f 346/386/351 345/384/351 374/394/351 375/397/351
+f 360/387/352 359/385/352 388/396/352 389/398/352
+f 347/388/353 346/386/353 375/397/353 376/399/353
+f 361/389/354 360/387/354 389/398/354 390/400/354
+f 348/390/355 347/388/355 376/399/355 377/401/355
+f 362/391/356 361/389/356 390/400/356 391/402/356
+f 349/392/357 348/390/357 377/401/357 378/403/357
+f 363/363/358 362/391/358 391/402/358 392/404/358
+f 350/365/359 349/392/359 378/403/359 379/405/359
+f 364/364/360 363/363/360 392/404/360 393/406/360
+f 351/366/361 350/365/361 379/405/361 380/407/361
+f 338/367/362 337/54/362 367/408/362
+f 365/368/363 364/364/363 393/406/363 394/409/363
+f 352/369/364 351/366/364 380/407/364 381/410/364
+f 339/371/365 338/370/365 367/411/365 368/412/365
+f 366/60/366 365/372/366 394/413/366
+f 353/373/367 352/369/367 381/410/367 382/414/367
+f 340/374/368 339/371/368 368/412/368 369/415/368
+f 354/375/369 353/373/369 382/414/369 383/416/369
+f 341/376/370 340/374/370 369/415/370 370/417/370
+f 355/377/371 354/375/371 383/416/371 384/418/371
+f 342/378/372 341/376/372 370/417/372 371/419/372
+f 356/379/373 355/377/373 384/418/373 385/420/373
+f 343/380/374 342/378/374 371/419/374 372/421/374
+f 357/381/375 356/379/375 385/420/375 386/422/375
+f 344/382/376 343/380/376 372/421/376 373/393/376
+f 358/383/377 357/381/377 386/422/377 387/395/377
+f 382/414/378 381/410/378 409/423/378 410/424/378
+f 369/415/379 368/412/379 396/425/379 397/426/379
+f 383/416/380 382/414/380 410/424/380 411/427/380
+f 370/417/381 369/415/381 397/426/381 398/428/381
+f 384/418/382 383/416/382 411/427/382 412/429/382
+f 371/419/383 370/417/383 398/428/383 399/430/383
+f 385/420/384 384/418/384 412/429/384 413/431/384
+f 372/421/385 371/419/385 399/430/385 400/432/385
+f 386/422/386 385/420/386 413/431/386 414/433/386
+f 373/393/387 372/421/387 400/432/387 401/434/387
+f 387/395/388 386/422/388 414/433/388 415/435/388
+f 374/394/389 373/393/389 401/434/389 402/436/389
+f 388/396/390 387/395/390 415/435/390 416/437/390
+f 375/397/391 374/394/391 402/436/391 403/438/391
+f 389/398/392 388/396/392 416/437/392 417/439/392
+f 376/399/393 375/397/393 403/438/393 404/440/393
+f 390/400/394 389/398/394 417/439/394 418/441/394
+f 377/401/395 376/399/395 404/440/395 405/442/395
+f 391/402/396 390/400/396 418/441/396 419/443/396
+f 378/403/397 377/401/397 405/442/397 406/444/397
+f 392/404/398 391/402/398 419/443/398 420/445/398
+f 379/405/399 378/403/399 406/444/399 407/446/399
+f 393/406/400 392/404/400 420/445/400 421/447/400
+f 380/407/401 379/405/401 407/446/401 408/448/401
+f 367/408/402 337/54/402 395/449/402
+f 394/409/403 393/406/403 421/447/403 422/450/403
+f 381/410/404 380/407/404 408/448/404 409/423/404
+f 368/412/405 367/411/405 395/451/405 396/425/405
+f 366/60/406 394/413/406 422/452/406
+f 418/441/407 417/439/407 445/453/407 446/454/407
+f 405/442/408 404/440/408 432/455/408 433/456/408
+f 419/443/409 418/441/409 446/454/409 447/457/409
+f 406/444/410 405/442/410 433/456/410 434/458/410
+f 420/445/411 419/443/411 447/457/411 448/459/411
+f 407/446/412 406/444/412 434/458/412 435/460/412
+f 421/447/413 420/445/413 448/459/413 449/461/413
+f 408/448/414 407/446/414 435/460/414 436/462/414
+f 395/449/415 337/54/415 423/463/415
+f 422/450/416 421/447/416 449/461/416 450/464/416
+f 409/423/417 408/448/417 436/462/417 437/465/417
+f 396/425/418 395/451/418 423/466/418 424/467/418
+f 366/60/419 422/452/419 450/468/419
+f 410/424/420 409/423/420 437/465/420 438/469/420
+f 397/426/421 396/425/421 424/467/421 425/470/421
+f 411/427/422 410/424/422 438/469/422 439/471/422
+f 398/428/423 397/426/423 425/470/423 426/472/423
+f 412/429/424 411/427/424 439/471/424 440/473/424
+f 399/430/425 398/428/425 426/472/425 427/474/425
+f 413/431/426 412/429/426 440/473/426 441/475/426
+f 400/432/427 399/430/427 427/474/427 428/476/427
+f 414/433/428 413/431/428 441/475/428 442/477/428
+f 401/434/429 400/432/429 428/476/429 429/478/429
+f 415/435/430 414/433/430 442/477/430 443/479/430
+f 402/436/431 401/434/431 429/478/431 430/480/431
+f 416/437/432 415/435/432 443/479/432 444/481/432
+f 403/438/433 402/436/433 430/480/433 431/482/433
+f 417/439/434 416/437/434 444/481/434 445/453/434
+f 404/440/435 403/438/435 431/482/435 432/455/435
+f 441/475/436 440/473/436 468/483/436 469/484/436
+f 428/476/437 427/474/437 455/485/437 456/486/437
+f 442/477/438 441/475/438 469/484/438 470/487/438
+f 429/478/439 428/476/439 456/486/439 457/488/439
+f 443/479/440 442/477/440 470/487/440 471/489/440
+f 430/480/441 429/478/441 457/488/441 458/490/441
+f 444/481/442 443/479/442 471/489/442 472/491/442
+f 431/482/443 430/480/443 458/490/443 459/492/443
+f 445/453/444 444/481/444 472/491/444 473/493/444
+f 432/455/445 431/482/445 459/492/445 460/494/445
+f 446/454/446 445/453/446 473/493/446 474/495/446
+f 433/456/447 432/455/447 460/494/447 461/496/447
+f 447/457/448 446/454/448 474/495/448 475/497/448
+f 434/458/449 433/456/449 461/496/449 462/498/449
+f 448/459/450 447/457/450 475/497/450 476/499/450
+f 435/460/451 434/458/451 462/498/451 463/500/451
+f 449/461/452 448/459/452 476/499/452 477/501/452
+f 436/462/453 435/460/453 463/500/453 464/502/453
+f 423/463/454 337/503/454 451/504/454
+f 450/464/455 449/461/455 477/501/455 478/505/455
+f 437/465/456 436/462/456 464/502/456 465/506/456
+f 424/467/457 423/466/457 451/507/457 452/508/457
+f 366/60/458 450/468/458 478/509/458
+f 438/469/459 437/465/459 465/506/459 466/510/459
+f 425/470/460 424/467/460 452/508/460 453/511/460
+f 439/471/461 438/469/461 466/510/461 467/512/461
+f 426/472/462 425/470/462 453/511/462 454/513/462
+f 440/473/463 439/471/463 467/512/463 468/483/463
+f 427/474/464 426/472/464 454/513/464 455/485/464
+f 477/501/465 476/499/465 504/514/465 505/515/465
+f 464/502/466 463/500/466 491/516/466 492/517/466
+f 451/504/467 337/503/467 479/518/467
+f 478/505/468 477/501/468 505/515/468 506/519/468
+f 465/506/469 464/502/469 492/517/469 493/520/469
+f 452/508/470 451/507/470 479/521/470 480/522/470
+f 366/523/471 478/509/471 506/524/471
+f 466/510/472 465/506/472 493/520/472 494/525/472
+f 453/511/473 452/508/473 480/522/473 481/526/473
+f 467/512/474 466/510/474 494/525/474 495/527/474
+f 454/513/475 453/511/475 481/526/475 482/528/475
+f 468/483/476 467/512/476 495/527/476 496/529/476
+f 455/485/477 454/513/477 482/528/477 483/530/477
+f 469/484/478 468/483/478 496/529/478 497/531/478
+f 456/486/479 455/485/479 483/530/479 484/532/479
+f 470/487/480 469/484/480 497/531/480 498/533/480
+f 457/488/481 456/486/481 484/532/481 485/534/481
+f 471/489/482 470/487/482 498/533/482 499/535/482
+f 458/490/483 457/488/483 485/534/483 486/536/483
+f 472/491/484 471/489/484 499/535/484 500/537/484
+f 459/492/485 458/490/485 486/536/485 487/538/485
+f 473/493/486 472/491/486 500/537/486 501/539/486
+f 460/494/487 459/492/487 487/538/487 488/540/487
+f 474/495/488 473/493/488 501/539/488 502/541/488
+f 461/496/489 460/494/489 488/540/489 489/542/489
+f 475/497/490 474/495/490 502/541/490 503/543/490
+f 462/498/491 461/496/491 489/542/491 490/544/491
+f 476/499/492 475/497/492 503/543/492 504/514/492
+f 463/500/493 462/498/493 490/544/493 491/516/493
+f 486/536/494 485/534/494 513/545/494 514/546/494
+f 500/537/495 499/535/495 527/547/495 528/548/495
+f 487/538/496 486/536/496 514/546/496 515/549/496
+f 501/539/497 500/537/497 528/548/497 529/550/497
+f 488/540/498 487/538/498 515/549/498 516/551/498
+f 502/541/499 501/539/499 529/550/499 530/552/499
+f 489/542/500 488/540/500 516/551/500 517/553/500
+f 503/543/501 502/541/501 530/552/501 531/554/501
+f 490/544/502 489/542/502 517/553/502 518/555/502
+f 504/514/503 503/543/503 531/554/503 532/556/503
+f 491/516/504 490/544/504 518/555/504 519/557/504
+f 505/515/505 504/514/505 532/556/505 533/558/505
+f 492/517/506 491/516/506 519/557/506 520/559/506
+f 479/518/507 337/503/507 507/560/507
+f 506/519/508 505/515/508 533/558/508 534/561/508
+f 493/520/509 492/517/509 520/559/509 521/562/509
+f 480/522/510 479/521/510 507/563/510 508/564/510
+f 366/523/511 506/524/511 534/565/511
+f 494/525/512 493/520/512 521/562/512 522/566/512
+f 481/526/513 480/522/513 508/564/513 509/567/513
+f 495/527/514 494/525/514 522/566/514 523/568/514
+f 482/528/515 481/526/515 509/567/515 510/569/515
+f 496/529/516 495/527/516 523/568/516 524/570/516
+f 483/530/517 482/528/517 510/569/517 511/571/517
+f 497/531/518 496/529/518 524/570/518 525/572/518
+f 484/532/519 483/530/519 511/571/519 512/573/519
+f 498/533/520 497/531/520 525/572/520 526/574/520
+f 485/534/521 484/532/521 512/573/521 513/545/521
+f 499/535/522 498/533/522 526/574/522 527/547/522
+f 522/566/523 521/562/523 549/575/523 550/576/523
+f 509/567/524 508/564/524 536/577/524 537/578/524
+f 523/568/525 522/566/525 550/576/525 551/579/525
+f 510/569/526 509/567/526 537/578/526 538/580/526
+f 524/570/527 523/568/527 551/579/527 552/581/527
+f 511/571/528 510/569/528 538/580/528 539/582/528
+f 525/572/529 524/570/529 552/581/529 553/583/529
+f 512/573/530 511/571/530 539/582/530 540/584/530
+f 526/574/531 525/572/531 553/583/531 554/585/531
+f 513/545/532 512/573/532 540/584/532 541/586/532
+f 527/547/533 526/574/533 554/585/533 555/587/533
+f 514/546/534 513/545/534 541/586/534 542/588/534
+f 528/548/535 527/547/535 555/587/535 556/589/535
+f 515/549/536 514/546/536 542/588/536 543/590/536
+f 529/550/537 528/548/537 556/589/537 557/591/537
+f 516/551/538 515/549/538 543/590/538 544/592/538
+f 530/552/539 529/550/539 557/591/539 558/593/539
+f 517/553/540 516/551/540 544/592/540 545/594/540
+f 531/554/541 530/552/541 558/593/541 559/595/541
+f 518/555/542 517/553/542 545/594/542 546/596/542
+f 532/556/543 531/554/543 559/595/543 560/597/543
+f 519/557/544 518/555/544 546/596/544 547/598/544
+f 533/558/545 532/556/545 560/597/545 561/599/545
+f 520/559/546 519/557/546 547/598/546 548/600/546
+f 507/560/547 337/503/547 535/601/547
+f 534/561/548 533/558/548 561/599/548 562/602/548
+f 521/562/549 520/559/549 548/600/549 549/575/549
+f 508/564/550 507/563/550 535/603/550 536/577/550
+f 366/523/551 534/565/551 562/604/551
+f 558/593/552 557/591/552 585/605/552 586/606/552
+f 545/594/553 544/592/553 572/607/553 573/608/553
+f 559/595/554 558/593/554 586/606/554 587/609/554
+f 546/596/555 545/594/555 573/608/555 574/610/555
+f 560/597/556 559/595/556 587/609/556 588/611/556
+f 547/598/557 546/596/557 574/610/557 575/612/557
+f 561/599/558 560/597/558 588/611/558 589/613/558
+f 548/600/559 547/598/559 575/612/559 576/614/559
+f 535/601/560 337/503/560 563/615/560
+f 562/602/561 561/599/561 589/613/561 590/616/561
+f 549/575/562 548/600/562 576/614/562 577/617/562
+f 536/577/563 535/603/563 563/618/563 564/619/563
+f 366/523/564 562/604/564 590/620/564
+f 550/576/565 549/575/565 577/617/565 578/621/565
+f 537/578/566 536/577/566 564/619/566 565/622/566
+f 551/579/567 550/576/567 578/621/567 579/623/567
+f 538/580/568 537/578/568 565/622/568 566/624/568
+f 552/581/569 551/579/569 579/623/569 580/625/569
+f 539/582/570 538/580/570 566/624/570 567/626/570
+f 553/583/571 552/581/571 580/625/571 581/627/571
+f 540/584/572 539/582/572 567/626/572 568/628/572
+f 554/585/573 553/583/573 581/627/573 582/629/573
+f 541/586/574 540/584/574 568/628/574 569/630/574
+f 555/587/575 554/585/575 582/629/575 583/631/575
+f 542/588/576 541/586/576 569/630/576 570/632/576
+f 556/589/577 555/587/577 583/631/577 584/633/577
+f 543/590/578 542/588/578 570/632/578 571/634/578
+f 557/591/579 556/589/579 584/633/579 585/605/579
+f 544/592/580 543/590/580 571/634/580 572/607/580
+f 567/626/581 566/624/581 594/635/581 595/636/581
+f 581/627/582 580/625/582 608/637/582 609/638/582
+f 568/628/583 567/626/583 595/636/583 596/639/583
+f 582/629/584 581/627/584 609/638/584 610/640/584
+f 569/630/585 568/628/585 596/639/585 597/641/585
+f 583/631/586 582/629/586 610/640/586 611/642/586
+f 570/632/587 569/630/587 597/641/587 598/643/587
+f 584/633/588 583/631/588 611/642/588 612/644/588
+f 571/634/589 570/632/589 598/643/589 599/645/589
+f 585/605/590 584/633/590 612/644/590 613/646/590
+f 572/607/591 571/634/591 599/645/591 600/647/591
+f 586/606/592 585/605/592 613/646/592 614/648/592
+f 573/608/593 572/607/593 600/647/593 601/649/593
+f 587/609/594 586/606/594 614/648/594 615/650/594
+f 574/610/595 573/608/595 601/649/595 602/651/595
+f 588/611/596 587/609/596 615/650/596 616/652/596
+f 575/612/597 574/610/597 602/651/597 603/653/597
+f 589/613/598 588/611/598 616/652/598 617/654/598
+f 576/614/599 575/612/599 603/653/599 604/655/599
+f 563/615/600 337/503/600 591/656/600
+f 590/616/601 589/613/601 617/654/601 618/657/601
+f 577/617/602 576/614/602 604/655/602 605/658/602
+f 564/619/603 563/618/603 591/659/603 592/660/603
+f 366/523/604 590/620/604 618/661/604
+f 578/621/605 577/617/605 605/658/605 606/662/605
+f 565/622/606 564/619/606 592/660/606 593/663/606
+f 579/623/607 578/621/607 606/662/607 607/664/607
+f 566/624/608 565/622/608 593/663/608 594/635/608
+f 580/625/609 579/623/609 607/664/609 608/637/609
+f 603/653/610 602/651/610 630/665/610 631/666/610
+f 617/654/611 616/652/611 644/667/611 645/668/611
+f 604/655/612 603/653/612 631/666/612 632/669/612
+f 591/656/613 337/503/613 619/670/613
+f 618/657/614 617/654/614 645/668/614 646/671/614
+f 605/658/615 604/655/615 632/669/615 633/672/615
+f 592/660/616 591/659/616 619/673/616 620/674/616
+f 366/523/617 618/661/617 646/675/617
+f 606/662/618 605/658/618 633/672/618 634/676/618
+f 593/663/619 592/660/619 620/674/619 621/677/619
+f 607/664/620 606/662/620 634/676/620 635/678/620
+f 594/635/621 593/663/621 621/677/621 622/679/621
+f 608/637/622 607/664/622 635/678/622 636/680/622
+f 595/636/623 594/635/623 622/679/623 623/681/623
+f 609/638/624 608/637/624 636/680/624 637/682/624
+f 596/639/625 595/636/625 623/681/625 624/683/625
+f 610/640/626 609/638/626 637/682/626 638/684/626
+f 597/641/627 596/639/627 624/683/627 625/685/627
+f 611/642/628 610/640/628 638/684/628 639/686/628
+f 598/643/629 597/641/629 625/685/629 626/687/629
+f 612/644/630 611/642/630 639/686/630 640/688/630
+f 599/645/631 598/643/631 626/687/631 627/689/631
+f 613/646/632 612/644/632 640/688/632 641/690/632
+f 600/647/633 599/645/633 627/689/633 628/691/633
+f 614/648/634 613/646/634 641/690/634 642/692/634
+f 601/649/635 600/647/635 628/691/635 629/693/635
+f 615/650/636 614/648/636 642/692/636 643/694/636
+f 602/651/637 601/649/637 629/693/637 630/665/637
+f 616/652/638 615/650/638 643/694/638 644/667/638
+f 639/686/639 638/684/639 666/695/639 667/696/639
+f 626/687/640 625/685/640 653/697/640 654/698/640
+f 640/688/641 639/686/641 667/696/641 668/699/641
+f 627/689/642 626/687/642 654/698/642 655/700/642
+f 641/690/643 640/688/643 668/699/643 669/701/643
+f 628/691/644 627/689/644 655/700/644 656/702/644
+f 642/692/645 641/690/645 669/701/645 670/703/645
+f 629/693/646 628/691/646 656/702/646 657/704/646
+f 643/694/647 642/692/647 670/703/647 671/705/647
+f 630/665/648 629/693/648 657/704/648 658/706/648
+f 644/667/649 643/694/649 671/705/649 672/707/649
+f 631/666/650 630/665/650 658/706/650 659/708/650
+f 645/668/651 644/667/651 672/707/651 673/709/651
+f 632/669/652 631/666/652 659/708/652 660/710/652
+f 619/670/653 337/503/653 647/711/653
+f 646/671/654 645/668/654 673/709/654 674/712/654
+f 633/672/655 632/669/655 660/710/655 661/713/655
+f 620/674/656 619/673/656 647/714/656 648/715/656
+f 366/523/657 646/675/657 674/716/657
+f 634/676/658 633/672/658 661/713/658 662/717/658
+f 621/677/659 620/674/659 648/715/659 649/718/659
+f 635/678/660 634/676/660 662/717/660 663/719/660
+f 622/679/661 621/677/661 649/718/661 650/720/661
+f 636/680/662 635/678/662 663/719/662 664/721/662
+f 623/681/663 622/679/663 650/720/663 651/722/663
+f 637/682/664 636/680/664 664/721/664 665/723/664
+f 624/683/665 623/681/665 651/722/665 652/724/665
+f 638/684/666 637/682/666 665/723/666 666/695/666
+f 625/685/667 624/683/667 652/724/667 653/697/667
+f 366/523/668 674/716/668 702/725/668
+f 662/717/669 661/713/669 689/726/669 690/727/669
+f 649/718/670 648/715/670 676/728/670 677/729/670
+f 663/719/671 662/717/671 690/727/671 691/730/671
+f 650/720/672 649/718/672 677/729/672 678/731/672
+f 664/721/673 663/719/673 691/730/673 692/732/673
+f 651/722/674 650/720/674 678/731/674 679/733/674
+f 665/723/675 664/721/675 692/732/675 693/734/675
+f 652/724/676 651/722/676 679/733/676 680/735/676
+f 666/695/677 665/723/677 693/734/677 694/736/677
+f 653/697/678 652/724/678 680/735/678 681/737/678
+f 667/696/679 666/695/679 694/736/679 695/738/679
+f 654/698/680 653/697/680 681/737/680 682/739/680
+f 668/699/681 667/696/681 695/738/681 696/740/681
+f 655/700/682 654/698/682 682/739/682 683/741/682
+f 669/701/683 668/699/683 696/740/683 697/742/683
+f 656/702/684 655/700/684 683/741/684 684/743/684
+f 670/703/685 669/701/685 697/742/685 698/744/685
+f 657/704/686 656/702/686 684/743/686 685/745/686
+f 671/705/687 670/703/687 698/744/687 699/746/687
+f 658/706/688 657/704/688 685/745/688 686/747/688
+f 672/707/689 671/705/689 699/746/689 700/748/689
+f 659/708/690 658/706/690 686/747/690 687/749/690
+f 673/709/691 672/707/691 700/748/691 701/750/691
+f 660/710/692 659/708/692 687/749/692 688/751/692
+f 647/711/693 337/503/693 675/752/693
+f 674/712/694 673/709/694 701/750/694 702/753/694
+f 661/713/695 660/710/695 688/751/695 689/726/695
+f 648/715/696 647/714/696 675/754/696 676/728/696
+f 698/744/697 697/742/697 725/755/697 726/756/697
+f 685/745/698 684/743/698 712/757/698 713/758/698
+f 699/746/699 698/744/699 726/756/699 727/759/699
+f 686/747/700 685/745/700 713/758/700 714/760/700
+f 700/748/701 699/746/701 727/759/701 728/761/701
+f 687/749/702 686/747/702 714/760/702 715/762/702
+f 701/750/703 700/748/703 728/761/703 729/763/703
+f 688/751/704 687/749/704 715/762/704 716/764/704
+f 675/752/705 337/503/705 703/765/705
+f 702/753/706 701/750/706 729/763/706 730/766/706
+f 689/726/707 688/751/707 716/764/707 717/767/707
+f 676/728/708 675/754/708 703/768/708 704/769/708
+f 366/523/709 702/725/709 730/770/709
+f 690/727/710 689/726/710 717/767/710 718/771/710
+f 677/729/711 676/728/711 704/769/711 705/772/711
+f 691/730/712 690/727/712 718/771/712 719/773/712
+f 678/731/713 677/729/713 705/772/713 706/774/713
+f 692/732/714 691/730/714 719/773/714 720/775/714
+f 679/733/715 678/731/715 706/774/715 707/776/715
+f 693/734/716 692/732/716 720/775/716 721/777/716
+f 680/735/717 679/733/717 707/776/717 708/778/717
+f 694/736/718 693/734/718 721/777/718 722/779/718
+f 681/737/719 680/735/719 708/778/719 709/780/719
+f 695/738/720 694/736/720 722/779/720 723/781/720
+f 682/739/721 681/737/721 709/780/721 710/782/721
+f 696/740/722 695/738/722 723/781/722 724/783/722
+f 683/741/723 682/739/723 710/782/723 711/784/723
+f 697/742/724 696/740/724 724/783/724 725/755/724
+f 684/743/725 683/741/725 711/784/725 712/757/725
+f 707/776/726 706/774/726 734/785/726 735/786/726
+f 721/777/727 720/775/727 748/787/727 749/788/727
+f 708/778/728 707/776/728 735/786/728 736/789/728
+f 722/779/729 721/777/729 749/788/729 750/790/729
+f 709/780/730 708/778/730 736/789/730 737/791/730
+f 723/781/731 722/779/731 750/790/731 751/792/731
+f 710/782/732 709/780/732 737/791/732 738/793/732
+f 724/783/733 723/781/733 751/792/733 752/794/733
+f 711/784/734 710/782/734 738/793/734 739/795/734
+f 725/755/735 724/783/735 752/794/735 753/796/735
+f 712/757/736 711/784/736 739/795/736 740/797/736
+f 726/756/737 725/755/737 753/796/737 754/798/737
+f 713/758/738 712/757/738 740/797/738 741/799/738
+f 727/759/739 726/756/739 754/798/739 755/800/739
+f 714/760/740 713/758/740 741/799/740 742/801/740
+f 728/761/741 727/759/741 755/800/741 756/802/741
+f 715/762/742 714/760/742 742/801/742 743/803/742
+f 729/763/743 728/761/743 756/802/743 757/804/743
+f 716/764/744 715/762/744 743/803/744 744/805/744
+f 703/765/745 337/503/745 731/806/745
+f 730/766/746 729/763/746 757/804/746 758/807/746
+f 717/767/747 716/764/747 744/805/747 745/808/747
+f 704/769/748 703/768/748 731/809/748 732/810/748
+f 366/523/749 730/770/749 758/811/749
+f 718/771/750 717/767/750 745/808/750 746/812/750
+f 705/772/751 704/769/751 732/810/751 733/813/751
+f 719/773/752 718/771/752 746/812/752 747/814/752
+f 706/774/753 705/772/753 733/813/753 734/785/753
+f 720/775/754 719/773/754 747/814/754 748/787/754
+f 743/803/755 742/801/755 770/815/755 771/816/755
+f 757/804/756 756/802/756 784/817/756 785/818/756
+f 744/805/757 743/803/757 771/816/757 772/819/757
+f 731/806/758 337/503/758 759/820/758
+f 758/807/759 757/804/759 785/818/759 786/821/759
+f 745/808/760 744/805/760 772/819/760 773/822/760
+f 732/810/761 731/809/761 759/823/761 760/824/761
+f 366/523/762 758/811/762 786/825/762
+f 746/812/763 745/808/763 773/822/763 774/826/763
+f 733/813/764 732/810/764 760/824/764 761/827/764
+f 747/814/765 746/812/765 774/826/765 775/828/765
+f 734/785/766 733/813/766 761/827/766 762/829/766
+f 748/787/767 747/814/767 775/828/767 776/830/767
+f 735/786/768 734/785/768 762/829/768 763/831/768
+f 749/788/769 748/787/769 776/830/769 777/832/769
+f 736/789/770 735/786/770 763/831/770 764/833/770
+f 750/790/771 749/788/771 777/832/771 778/834/771
+f 737/791/772 736/789/772 764/833/772 765/835/772
+f 751/792/773 750/790/773 778/834/773 779/836/773
+f 738/793/774 737/791/774 765/835/774 766/837/774
+f 752/794/775 751/792/775 779/836/775 780/838/775
+f 739/795/776 738/793/776 766/837/776 767/839/776
+f 753/796/777 752/794/777 780/838/777 781/840/777
+f 740/797/778 739/795/778 767/839/778 768/841/778
+f 754/798/779 753/796/779 781/840/779 782/842/779
+f 741/799/780 740/797/780 768/841/780 769/843/780
+f 755/800/781 754/798/781 782/842/781 783/844/781
+f 742/801/782 741/799/782 769/843/782 770/815/782
+f 756/802/783 755/800/783 783/844/783 784/817/783
+f 779/836/784 778/834/784 806/845/784 807/846/784
+f 766/837/785 765/835/785 793/847/785 794/848/785
+f 780/838/786 779/836/786 807/846/786 808/849/786
+f 767/839/787 766/837/787 794/848/787 795/850/787
+f 781/840/788 780/838/788 808/849/788 809/851/788
+f 768/841/789 767/839/789 795/850/789 796/852/789
+f 782/842/790 781/840/790 809/851/790 810/853/790
+f 769/843/791 768/841/791 796/852/791 797/854/791
+f 783/844/792 782/842/792 810/853/792 811/855/792
+f 770/815/793 769/843/793 797/854/793 798/856/793
+f 784/817/794 783/844/794 811/855/794 812/857/794
+f 771/816/795 770/815/795 798/856/795 799/858/795
+f 785/818/796 784/817/796 812/857/796 813/859/796
+f 772/819/797 771/816/797 799/858/797 800/860/797
+f 759/820/798 337/503/798 787/861/798
+f 786/821/799 785/818/799 813/859/799 814/862/799
+f 773/822/800 772/819/800 800/860/800 801/863/800
+f 760/824/801 759/823/801 787/864/801 788/865/801
+f 366/523/802 786/825/802 814/866/802
+f 774/826/803 773/822/803 801/863/803 802/867/803
+f 761/827/804 760/824/804 788/865/804 789/868/804
+f 775/828/805 774/826/805 802/867/805 803/869/805
+f 762/829/806 761/827/806 789/868/806 790/870/806
+f 776/830/807 775/828/807 803/869/807 804/871/807
+f 763/831/808 762/829/808 790/870/808 791/872/808
+f 777/832/809 776/830/809 804/871/809 805/873/809
+f 764/833/810 763/831/810 791/872/810 792/874/810
+f 778/834/811 777/832/811 805/873/811 806/845/811
+f 765/835/812 764/833/812 792/874/812 793/847/812
+f 788/865/813 787/864/813 815/875/813 816/876/813
+f 802/867/814 801/863/814 829/877/814 830/878/814
+f 814/862/815 813/859/815 841/879/815 842/880/815
+f 789/868/816 788/865/816 816/876/816 817/881/816
+f 366/523/817 814/866/817 842/882/817
+f 790/870/818 789/868/818 817/881/818 818/883/818
+f 791/872/819 790/870/819 818/883/819 819/884/819
+f 803/869/820 802/867/820 830/878/820 831/885/820
+f 792/874/821 791/872/821 819/884/821 820/886/821
+f 804/871/822 803/869/822 831/885/822 832/887/822
+f 793/847/823 792/874/823 820/886/823 821/888/823
+f 805/873/824 804/871/824 832/887/824 833/889/824
+f 794/848/825 793/847/825 821/888/825 822/890/825
+f 806/845/826 805/873/826 833/889/826 834/891/826
+f 795/850/827 794/848/827 822/890/827 823/892/827
+f 807/846/828 806/845/828 834/891/828 835/893/828
+f 796/852/829 795/850/829 823/892/829 824/894/829
+f 808/849/830 807/846/830 835/893/830 836/895/830
+f 797/854/831 796/852/831 824/894/831 825/896/831
+f 809/851/832 808/849/832 836/895/832 837/897/832
+f 798/856/833 797/854/833 825/896/833 826/898/833
+f 810/853/834 809/851/834 837/897/834 838/899/834
+f 799/858/835 798/856/835 826/898/835 827/900/835
+f 811/855/836 810/853/836 838/899/836 839/901/836
+f 800/860/837 799/858/837 827/900/837 828/902/837
+f 812/857/838 811/855/838 839/901/838 840/903/838
+f 787/861/839 337/503/839 815/904/839
+f 801/863/840 800/860/840 828/902/840 829/877/840
+f 813/859/841 812/857/841 840/903/841 841/879/841
+f 836/895/842 835/893/842 863/905/842 864/906/842
+f 823/892/843 822/890/843 850/907/843 851/908/843
+f 837/897/844 836/895/844 864/906/844 865/909/844
+f 824/894/845 823/892/845 851/908/845 852/910/845
+f 838/899/846 837/897/846 865/909/846 866/911/846
+f 825/896/847 824/894/847 852/910/847 853/912/847
+f 839/901/848 838/899/848 866/911/848 867/913/848
+f 826/898/849 825/896/849 853/912/849 854/914/849
+f 840/903/850 839/901/850 867/913/850 868/915/850
+f 827/900/851 826/898/851 854/914/851 855/916/851
+f 841/879/852 840/903/852 868/915/852 869/917/852
+f 828/902/853 827/900/853 855/916/853 856/918/853
+f 815/904/854 337/503/854 843/919/854
+f 842/880/855 841/879/855 869/917/855 870/920/855
+f 829/877/856 828/902/856 856/918/856 857/921/856
+f 816/876/857 815/875/857 843/922/857 844/923/857
+f 366/523/858 842/882/858 870/924/858
+f 830/878/859 829/877/859 857/921/859 858/925/859
+f 817/881/860 816/876/860 844/923/860 845/926/860
+f 831/885/861 830/878/861 858/925/861 859/927/861
+f 818/883/862 817/881/862 845/926/862 846/928/862
+f 832/887/863 831/885/863 859/927/863 860/929/863
+f 819/884/864 818/883/864 846/928/864 847/930/864
+f 833/889/865 832/887/865 860/929/865 861/931/865
+f 820/886/866 819/884/866 847/930/866 848/932/866
+f 834/891/867 833/889/867 861/931/867 862/933/867
+f 821/888/868 820/886/868 848/932/868 849/934/868
+f 835/893/869 834/891/869 862/933/869 863/905/869
+f 822/890/870 821/888/870 849/934/870 850/907/870
+f 859/927/871 858/925/871 886/935/871 887/936/871
+f 846/928/872 845/926/872 873/937/872 874/938/872
+f 860/929/873 859/927/873 887/936/873 888/939/873
+f 847/930/874 846/928/874 874/938/874 875/940/874
+f 861/931/875 860/929/875 888/939/875 889/941/875
+f 848/932/876 847/930/876 875/940/876 876/942/876
+f 862/933/877 861/931/877 889/941/877 890/943/877
+f 849/934/878 848/932/878 876/942/878 877/944/878
+f 863/905/879 862/933/879 890/943/879 891/945/879
+f 850/907/880 849/934/880 877/944/880 878/946/880
+f 864/906/881 863/905/881 891/945/881 892/947/881
+f 851/908/882 850/907/882 878/946/882 879/948/882
+f 865/909/883 864/906/883 892/947/883 893/949/883
+f 852/910/884 851/908/884 879/948/884 880/950/884
+f 866/911/885 865/909/885 893/949/885 894/951/885
+f 853/912/886 852/910/886 880/950/886 881/952/886
+f 867/913/887 866/911/887 894/951/887 895/953/887
+f 854/914/888 853/912/888 881/952/888 882/954/888
+f 868/915/889 867/913/889 895/953/889 896/955/889
+f 855/916/890 854/914/890 882/954/890 883/956/890
+f 869/917/891 868/915/891 896/955/891 897/957/891
+f 856/918/892 855/916/892 883/956/892 884/958/892
+f 843/919/893 337/503/893 871/959/893
+f 870/920/894 869/917/894 897/957/894 898/960/894
+f 857/921/895 856/918/895 884/958/895 885/961/895
+f 844/923/896 843/922/896 871/962/896 872/963/896
+f 366/523/897 870/924/897 898/964/897
+f 858/925/898 857/921/898 885/961/898 886/935/898
+f 845/926/899 844/923/899 872/963/899 873/937/899
+f 895/953/900 894/951/900 24/965/900 25/966/900
+f 882/954/901 881/952/901 11/967/901 12/968/901
+f 896/955/902 895/953/902 25/966/902 26/969/902
+f 883/956/903 882/954/903 12/968/903 13/970/903
+f 897/957/904 896/955/904 26/969/904 27/971/904
+f 884/958/905 883/956/905 13/970/905 14/972/905
+f 871/973/906 337/54/906 1/53/906
+f 898/960/907 897/957/907 27/971/907 28/974/907
+f 885/961/908 884/958/908 14/972/908 15/975/908
+f 872/976/909 871/977/909 1/58/909 2/6/909
+f 366/523/910 898/964/910 28/978/910
+f 886/935/911 885/961/911 15/975/911 16/979/911
+f 873/980/912 872/976/912 2/6/912 3/5/912
+f 887/936/913 886/935/913 16/979/913 17/981/913
+f 874/938/914 873/937/914 3/982/914 4/983/914
+f 888/939/915 887/936/915 17/981/915 18/984/915
+f 875/940/916 874/938/916 4/983/916 5/985/916
+f 889/941/917 888/939/917 18/984/917 19/986/917
+f 876/942/918 875/940/918 5/985/918 6/987/918
+f 890/943/919 889/941/919 19/986/919 20/988/919
+f 877/944/920 876/942/920 6/987/920 7/989/920
+f 891/945/921 890/943/921 20/988/921 21/990/921
+f 878/946/922 877/944/922 7/989/922 8/991/922
+f 892/947/923 891/945/923 21/990/923 22/992/923
+f 879/948/924 878/946/924 8/991/924 9/993/924
+f 893/949/925 892/947/925 22/992/925 23/994/925
+f 880/950/926 879/948/926 9/993/926 10/995/926
+f 894/951/927 893/949/927 23/994/927 24/965/927
+f 881/952/928 880/950/928 10/995/928 11/967/928
diff --git a/data/textured_sphere_smooth.mtl b/data/textured_sphere_smooth.mtl
new file mode 100644
index 0000000..f95aa33
--- /dev/null
+++ b/data/textured_sphere_smooth.mtl
@@ -0,0 +1,11 @@
+# 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
+map_Kd checker_huge.gif
diff --git a/data/textured_sphere_smooth.obj b/data/textured_sphere_smooth.obj
new file mode 100644
index 0000000..5830c77
--- /dev/null
+++ b/data/textured_sphere_smooth.obj
@@ -0,0 +1,3725 @@
+# Blender v2.77 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib textured_sphere_smooth.mtl
+o Sphere_Sphere.001
+v -0.108119 0.994138 0.000000
+v -0.214970 0.976621 0.000000
+v -0.319302 0.947653 0.000000
+v -0.419889 0.907575 0.000000
+v -0.515554 0.856857 0.000000
+v -0.605174 0.796093 0.000000
+v -0.687699 0.725995 0.000000
+v -0.762162 0.647386 0.000000
+v -0.827689 0.561187 0.000000
+v -0.883512 0.468408 0.000000
+v -0.928977 0.370138 0.000000
+v -0.963550 0.267528 0.000000
+v -0.986827 0.161782 0.000000
+v -0.998533 0.054139 0.000000
+v -0.998533 -0.054139 0.000000
+v -0.986827 -0.161782 0.000000
+v -0.963550 -0.267528 0.000000
+v -0.928977 -0.370138 0.000000
+v -0.883512 -0.468408 0.000000
+v -0.827689 -0.561187 0.000000
+v -0.762162 -0.647386 0.000000
+v -0.687700 -0.725995 0.000000
+v -0.605175 -0.796093 0.000000
+v -0.515554 -0.856857 0.000000
+v -0.419890 -0.907575 0.000000
+v -0.319302 -0.947653 0.000000
+v -0.214971 -0.976620 0.000000
+v -0.108120 -0.994138 0.000000
+v -0.106042 0.994138 -0.021093
+v -0.210840 0.976621 -0.041938
+v -0.313166 0.947653 -0.062292
+v -0.411821 0.907575 -0.081916
+v -0.505648 0.856857 -0.100579
+v -0.593546 0.796093 -0.118063
+v -0.674486 0.725995 -0.134163
+v -0.747517 0.647386 -0.148690
+v -0.811785 0.561187 -0.161474
+v -0.866536 0.468408 -0.172364
+v -0.911127 0.370138 -0.181234
+v -0.945036 0.267528 -0.187979
+v -0.967865 0.161782 -0.192520
+v -0.979347 0.054139 -0.194804
+v -0.979347 -0.054139 -0.194804
+v -0.967865 -0.161782 -0.192520
+v -0.945036 -0.267528 -0.187979
+v -0.911127 -0.370138 -0.181234
+v -0.866536 -0.468408 -0.172365
+v -0.811785 -0.561187 -0.161474
+v -0.747518 -0.647386 -0.148690
+v -0.674486 -0.725995 -0.134163
+v -0.593546 -0.796093 -0.118064
+v -0.505648 -0.856857 -0.100580
+v -0.411822 -0.907575 -0.081916
+v -0.313167 -0.947653 -0.062293
+v -0.210841 -0.976620 -0.041939
+v -0.106042 -0.994138 -0.021093
+v -0.099889 0.994138 -0.041375
+v -0.198607 0.976621 -0.082265
+v -0.294996 0.947653 -0.122191
+v -0.387927 0.907575 -0.160684
+v -0.476310 0.856857 -0.197294
+v -0.559108 0.796093 -0.231590
+v -0.635351 0.725995 -0.263171
+v -0.704146 0.647386 -0.291666
+v -0.764685 0.561187 -0.316743
+v -0.816259 0.468408 -0.338105
+v -0.858263 0.370138 -0.355504
+v -0.890204 0.267528 -0.368734
+v -0.911709 0.161782 -0.377642
+v -0.922525 0.054139 -0.382122
+v -0.922525 -0.054139 -0.382122
+v -0.911709 -0.161782 -0.377642
+v -0.890204 -0.267528 -0.368734
+v -0.858263 -0.370138 -0.355504
+v -0.816259 -0.468408 -0.338105
+v -0.764685 -0.561187 -0.316743
+v -0.704146 -0.647386 -0.291667
+v -0.635352 -0.725995 -0.263171
+v -0.559108 -0.796093 -0.231590
+v -0.476310 -0.856857 -0.197294
+v -0.387928 -0.907575 -0.160684
+v -0.294997 -0.947653 -0.122191
+v -0.198608 -0.976620 -0.082266
+v -0.099890 -0.994138 -0.041375
+v -0.089898 0.994138 -0.060067
+v -0.178742 0.976621 -0.119431
+v -0.265490 0.947653 -0.177394
+v -0.349125 0.907575 -0.233277
+v -0.428668 0.856857 -0.286426
+v -0.503184 0.796093 -0.336216
+v -0.571801 0.725995 -0.382065
+v -0.633715 0.647386 -0.423434
+v -0.688198 0.561187 -0.459839
+v -0.734614 0.468408 -0.490852
+v -0.772416 0.370138 -0.516111
+v -0.801163 0.267528 -0.535319
+v -0.820516 0.161782 -0.548251
+v -0.830250 0.054139 -0.554755
+v -0.830250 -0.054139 -0.554755
+v -0.820516 -0.161782 -0.548251
+v -0.801163 -0.267528 -0.535319
+v -0.772416 -0.370138 -0.516111
+v -0.734614 -0.468408 -0.490853
+v -0.688198 -0.561187 -0.459839
+v -0.633715 -0.647386 -0.423434
+v -0.571801 -0.725995 -0.382065
+v -0.503184 -0.796093 -0.336217
+v -0.428668 -0.856857 -0.286426
+v -0.349126 -0.907575 -0.233278
+v -0.265490 -0.947653 -0.177394
+v -0.178742 -0.976620 -0.119431
+v -0.089898 -0.994138 -0.060068
+v -0.076452 0.994138 -0.076451
+v -0.152007 0.976621 -0.152006
+v -0.225781 0.947653 -0.225780
+v -0.296907 0.907575 -0.296906
+v -0.364552 0.856857 -0.364551
+v -0.427923 0.796093 -0.427922
+v -0.486277 0.725995 -0.486276
+v -0.538930 0.647386 -0.538929
+v -0.585265 0.561187 -0.585264
+v -0.624738 0.468408 -0.624737
+v -0.656886 0.370138 -0.656885
+v -0.681333 0.267528 -0.681332
+v -0.697792 0.161782 -0.697791
+v -0.706070 0.054139 -0.706069
+v -0.706070 -0.054139 -0.706069
+v -0.697792 -0.161782 -0.697791
+v -0.681333 -0.267528 -0.681332
+v -0.656886 -0.370138 -0.656885
+v -0.624738 -0.468408 -0.624737
+v -0.585265 -0.561187 -0.585264
+v -0.538930 -0.647386 -0.538929
+v -0.486277 -0.725995 -0.486276
+v -0.427923 -0.796093 -0.427922
+v -0.364552 -0.856857 -0.364551
+v -0.296907 -0.907575 -0.296906
+v -0.225781 -0.947653 -0.225780
+v -0.152008 -0.976620 -0.152007
+v -0.076452 -0.994138 -0.076452
+v -0.060068 0.994138 -0.089897
+v -0.119432 0.976621 -0.178741
+v -0.177395 0.947653 -0.265489
+v -0.233278 0.907575 -0.349124
+v -0.286427 0.856857 -0.428667
+v -0.336217 0.796093 -0.503183
+v -0.382066 0.725995 -0.571800
+v -0.423435 0.647386 -0.633714
+v -0.459840 0.561187 -0.688197
+v -0.490853 0.468408 -0.734613
+v -0.516112 0.370138 -0.772415
+v -0.535320 0.267528 -0.801162
+v -0.548252 0.161782 -0.820515
+v -0.554756 0.054139 -0.830249
+v -0.554756 -0.054139 -0.830249
+v -0.548252 -0.161782 -0.820515
+v -0.535320 -0.267528 -0.801162
+v -0.516112 -0.370138 -0.772415
+v -0.490853 -0.468408 -0.734613
+v -0.459840 -0.561187 -0.688197
+v -0.423435 -0.647386 -0.633714
+v -0.382066 -0.725995 -0.571801
+v -0.336217 -0.796093 -0.503183
+v -0.286427 -0.856857 -0.428667
+v -0.233279 -0.907575 -0.349125
+v -0.177395 -0.947653 -0.265489
+v -0.119432 -0.976620 -0.178741
+v -0.060069 -0.994138 -0.089898
+v -0.041376 0.994138 -0.099888
+v -0.082266 0.976621 -0.198606
+v -0.122192 0.947653 -0.294995
+v -0.160685 0.907575 -0.387926
+v -0.197294 0.856857 -0.476309
+v -0.231591 0.796093 -0.559107
+v -0.263172 0.725995 -0.635351
+v -0.291667 0.647386 -0.704145
+v -0.316743 0.561187 -0.764684
+v -0.338106 0.468408 -0.816258
+v -0.355504 0.370138 -0.858262
+v -0.368735 0.267528 -0.890203
+v -0.377643 0.161782 -0.911708
+v -0.382123 0.054139 -0.922524
+v -0.382123 -0.054139 -0.922524
+v -0.377643 -0.161782 -0.911708
+v -0.368735 -0.267528 -0.890203
+v -0.355504 -0.370138 -0.858262
+v -0.338106 -0.468408 -0.816258
+v -0.316743 -0.561187 -0.764684
+v -0.291667 -0.647386 -0.704145
+v -0.263172 -0.725995 -0.635351
+v -0.231591 -0.796093 -0.559107
+v -0.197295 -0.856857 -0.476309
+v -0.160685 -0.907575 -0.387927
+v -0.122192 -0.947653 -0.294996
+v -0.082266 -0.976620 -0.198607
+v -0.041376 -0.994138 -0.099889
+v -0.021094 0.994138 -0.106041
+v -0.041939 0.976621 -0.210839
+v -0.062293 0.947653 -0.313165
+v -0.081917 0.907575 -0.411820
+v -0.100580 0.856857 -0.505647
+v -0.118064 0.796093 -0.593545
+v -0.134164 0.725995 -0.674484
+v -0.148691 0.647386 -0.747516
+v -0.161475 0.561187 -0.811784
+v -0.172365 0.468408 -0.866535
+v -0.181235 0.370138 -0.911126
+v -0.187980 0.267528 -0.945035
+v -0.192521 0.161782 -0.967864
+v -0.194805 0.054139 -0.979346
+v -0.194805 -0.054139 -0.979346
+v -0.192521 -0.161782 -0.967864
+v -0.187980 -0.267528 -0.945035
+v -0.181235 -0.370138 -0.911126
+v -0.172365 -0.468408 -0.866535
+v -0.161475 -0.561187 -0.811784
+v -0.148691 -0.647386 -0.747517
+v -0.134164 -0.725995 -0.674485
+v -0.118064 -0.796093 -0.593545
+v -0.100580 -0.856857 -0.505647
+v -0.081917 -0.907575 -0.411821
+v -0.062293 -0.947653 -0.313166
+v -0.041939 -0.976620 -0.210840
+v -0.021094 -0.994138 -0.106041
+v -0.000001 0.994138 -0.108118
+v -0.000001 0.976621 -0.214970
+v -0.000001 0.947653 -0.319301
+v -0.000001 0.907575 -0.419888
+v -0.000001 0.856857 -0.515553
+v -0.000001 0.796093 -0.605173
+v -0.000001 0.725995 -0.687698
+v -0.000001 0.647386 -0.762161
+v -0.000001 0.561187 -0.827688
+v -0.000001 0.468408 -0.883511
+v -0.000001 0.370138 -0.928976
+v -0.000001 0.267528 -0.963549
+v -0.000001 0.161782 -0.986825
+v -0.000001 0.054139 -0.998532
+v -0.000001 -0.054139 -0.998532
+v -0.000001 -0.161782 -0.986825
+v -0.000001 -0.267528 -0.963549
+v -0.000001 -0.370138 -0.928976
+v -0.000001 -0.468408 -0.883511
+v -0.000001 -0.561187 -0.827688
+v -0.000001 -0.647386 -0.762161
+v -0.000001 -0.725995 -0.687699
+v -0.000001 -0.796093 -0.605174
+v -0.000001 -0.856857 -0.515553
+v -0.000001 -0.907575 -0.419889
+v -0.000001 -0.947653 -0.319301
+v -0.000001 -0.976620 -0.214970
+v -0.000001 -0.994138 -0.108119
+v 0.021092 0.994138 -0.106041
+v 0.041938 0.976621 -0.210839
+v 0.062292 0.947653 -0.313165
+v 0.081915 0.907575 -0.411820
+v 0.100579 0.856857 -0.505647
+v 0.118063 0.796093 -0.593545
+v 0.134162 0.725995 -0.674484
+v 0.148689 0.647386 -0.747516
+v 0.161473 0.561187 -0.811784
+v 0.172364 0.468408 -0.866535
+v 0.181233 0.370138 -0.911126
+v 0.187978 0.267528 -0.945035
+v 0.192519 0.161782 -0.967864
+v 0.194803 0.054139 -0.979346
+v 0.194803 -0.054139 -0.979346
+v 0.192519 -0.161782 -0.967864
+v 0.187978 -0.267528 -0.945035
+v 0.181233 -0.370138 -0.911126
+v 0.172364 -0.468408 -0.866535
+v 0.161473 -0.561187 -0.811784
+v 0.148689 -0.647386 -0.747516
+v 0.134162 -0.725995 -0.674485
+v 0.118063 -0.796093 -0.593545
+v 0.100579 -0.856857 -0.505647
+v 0.081915 -0.907575 -0.411821
+v 0.062292 -0.947653 -0.313166
+v 0.041938 -0.976620 -0.210840
+v 0.021092 -0.994138 -0.106041
+v 0.041374 0.994138 -0.099888
+v 0.082264 0.976621 -0.198606
+v 0.122190 0.947653 -0.294995
+v 0.160683 0.907575 -0.387926
+v 0.197293 0.856857 -0.476309
+v 0.231589 0.796093 -0.559107
+v 0.263170 0.725995 -0.635350
+v 0.291665 0.647386 -0.704145
+v 0.316742 0.561187 -0.764684
+v 0.338104 0.468408 -0.816258
+v 0.355503 0.370138 -0.858261
+v 0.368733 0.267528 -0.890203
+v 0.377641 0.161782 -0.911708
+v 0.382121 0.054139 -0.922523
+v 0.382121 -0.054139 -0.922523
+v 0.377641 -0.161782 -0.911708
+v 0.368733 -0.267528 -0.890203
+v 0.355503 -0.370138 -0.858261
+v 0.338104 -0.468408 -0.816258
+v 0.316742 -0.561187 -0.764684
+v 0.291666 -0.647386 -0.704145
+v 0.263170 -0.725995 -0.635351
+v 0.231589 -0.796093 -0.559107
+v 0.197293 -0.856857 -0.476309
+v 0.160684 -0.907575 -0.387927
+v 0.122190 -0.947653 -0.294996
+v 0.082265 -0.976620 -0.198607
+v 0.041374 -0.994138 -0.099889
+v 0.060066 0.994138 -0.089897
+v 0.119430 0.976621 -0.178741
+v 0.177393 0.947653 -0.265489
+v 0.233276 0.907575 -0.349124
+v 0.286425 0.856857 -0.428667
+v 0.336215 0.796093 -0.503183
+v 0.382064 0.725995 -0.571800
+v 0.423433 0.647386 -0.633714
+v 0.459838 0.561187 -0.688197
+v 0.490851 0.468408 -0.734612
+v 0.516110 0.370138 -0.772415
+v 0.535318 0.267528 -0.801161
+v 0.548250 0.161782 -0.820515
+v 0.554754 0.054139 -0.830249
+v 0.554754 -0.054139 -0.830249
+v 0.548250 -0.161782 -0.820515
+v 0.535318 -0.267528 -0.801161
+v 0.516110 -0.370138 -0.772415
+v 0.490851 -0.468408 -0.734612
+v 0.459838 -0.561187 -0.688197
+v 0.423433 -0.647386 -0.633714
+v 0.382064 -0.725995 -0.571800
+v 0.336216 -0.796093 -0.503183
+v 0.286425 -0.856857 -0.428667
+v 0.233277 -0.907575 -0.349125
+v 0.177393 -0.947653 -0.265489
+v 0.119430 -0.976620 -0.178741
+v 0.060067 -0.994138 -0.089898
+v -0.000001 1.000000 0.000001
+v 0.076450 0.994138 -0.076451
+v 0.152006 0.976621 -0.152006
+v 0.225779 0.947653 -0.225780
+v 0.296905 0.907575 -0.296906
+v 0.364550 0.856857 -0.364551
+v 0.427921 0.796093 -0.427922
+v 0.486275 0.725995 -0.486276
+v 0.538928 0.647386 -0.538929
+v 0.585263 0.561187 -0.585264
+v 0.624736 0.468408 -0.624736
+v 0.656884 0.370138 -0.656885
+v 0.681331 0.267528 -0.681332
+v 0.697790 0.161782 -0.697791
+v 0.706068 0.054139 -0.706069
+v 0.706068 -0.054139 -0.706069
+v 0.697790 -0.161782 -0.697791
+v 0.681331 -0.267528 -0.681332
+v 0.656884 -0.370138 -0.656885
+v 0.624736 -0.468408 -0.624736
+v 0.585263 -0.561187 -0.585264
+v 0.538929 -0.647386 -0.538929
+v 0.486275 -0.725995 -0.486276
+v 0.427921 -0.796093 -0.427922
+v 0.364550 -0.856857 -0.364551
+v 0.296905 -0.907575 -0.296906
+v 0.225779 -0.947653 -0.225780
+v 0.152006 -0.976620 -0.152007
+v 0.076451 -0.994138 -0.076452
+v -0.000001 -1.000000 0.000000
+v 0.089896 0.994138 -0.060067
+v 0.178740 0.976621 -0.119431
+v 0.265488 0.947653 -0.177394
+v 0.349123 0.907575 -0.233277
+v 0.428666 0.856857 -0.286426
+v 0.503182 0.796093 -0.336216
+v 0.571799 0.725995 -0.382065
+v 0.633713 0.647386 -0.423434
+v 0.688196 0.561187 -0.459839
+v 0.734612 0.468408 -0.490852
+v 0.772414 0.370138 -0.516111
+v 0.801161 0.267528 -0.535319
+v 0.820514 0.161782 -0.548251
+v 0.830248 0.054139 -0.554755
+v 0.830248 -0.054139 -0.554755
+v 0.820514 -0.161782 -0.548251
+v 0.801161 -0.267528 -0.535319
+v 0.772414 -0.370138 -0.516111
+v 0.734612 -0.468408 -0.490852
+v 0.688197 -0.561187 -0.459839
+v 0.633713 -0.647386 -0.423434
+v 0.571800 -0.725995 -0.382065
+v 0.503182 -0.796093 -0.336216
+v 0.428666 -0.856857 -0.286426
+v 0.349124 -0.907575 -0.233278
+v 0.265488 -0.947653 -0.177394
+v 0.178740 -0.976620 -0.119431
+v 0.089897 -0.994138 -0.060068
+v 0.099887 0.994138 -0.041375
+v 0.198605 0.976621 -0.082265
+v 0.294994 0.947653 -0.122191
+v 0.387925 0.907575 -0.160684
+v 0.476308 0.856857 -0.197294
+v 0.559106 0.796093 -0.231590
+v 0.635349 0.725995 -0.263171
+v 0.704144 0.647386 -0.291666
+v 0.764683 0.561187 -0.316742
+v 0.816257 0.468408 -0.338105
+v 0.858261 0.370138 -0.355503
+v 0.890202 0.267528 -0.368734
+v 0.911707 0.161782 -0.377642
+v 0.922523 0.054139 -0.382122
+v 0.922523 -0.054139 -0.382122
+v 0.911707 -0.161782 -0.377642
+v 0.890202 -0.267528 -0.368734
+v 0.858261 -0.370138 -0.355503
+v 0.816257 -0.468408 -0.338105
+v 0.764683 -0.561187 -0.316742
+v 0.704144 -0.647386 -0.291666
+v 0.635350 -0.725995 -0.263171
+v 0.559106 -0.796093 -0.231590
+v 0.476308 -0.856857 -0.197294
+v 0.387926 -0.907575 -0.160684
+v 0.294995 -0.947653 -0.122191
+v 0.198606 -0.976620 -0.082266
+v 0.099888 -0.994138 -0.041375
+v 0.106040 0.994138 -0.021093
+v 0.210838 0.976621 -0.041938
+v 0.313164 0.947653 -0.062292
+v 0.411819 0.907575 -0.081916
+v 0.505646 0.856857 -0.100579
+v 0.593544 0.796093 -0.118063
+v 0.674483 0.725995 -0.134163
+v 0.747515 0.647386 -0.148690
+v 0.811783 0.561187 -0.161474
+v 0.866534 0.468408 -0.172364
+v 0.911125 0.370138 -0.181234
+v 0.945034 0.267528 -0.187979
+v 0.967863 0.161782 -0.192520
+v 0.979345 0.054139 -0.194804
+v 0.979345 -0.054139 -0.194804
+v 0.967863 -0.161782 -0.192520
+v 0.945034 -0.267528 -0.187979
+v 0.911125 -0.370138 -0.181234
+v 0.866534 -0.468408 -0.172364
+v 0.811783 -0.561187 -0.161474
+v 0.747516 -0.647386 -0.148690
+v 0.674484 -0.725995 -0.134163
+v 0.593544 -0.796093 -0.118063
+v 0.505646 -0.856857 -0.100579
+v 0.411820 -0.907575 -0.081916
+v 0.313165 -0.947653 -0.062293
+v 0.210839 -0.976620 -0.041939
+v 0.106041 -0.994138 -0.021093
+v 0.108117 0.994138 0.000000
+v 0.214969 0.976621 0.000000
+v 0.319300 0.947653 0.000000
+v 0.419887 0.907575 0.000000
+v 0.515552 0.856857 -0.000000
+v 0.605172 0.796093 0.000000
+v 0.687697 0.725995 -0.000000
+v 0.762160 0.647386 0.000000
+v 0.827687 0.561187 0.000000
+v 0.883510 0.468408 0.000000
+v 0.928975 0.370138 0.000000
+v 0.963548 0.267528 0.000000
+v 0.986824 0.161782 0.000000
+v 0.998531 0.054139 0.000000
+v 0.998531 -0.054139 0.000000
+v 0.986824 -0.161782 0.000000
+v 0.963548 -0.267528 0.000000
+v 0.928975 -0.370138 0.000000
+v 0.883510 -0.468408 0.000000
+v 0.827687 -0.561187 0.000000
+v 0.762160 -0.647386 0.000000
+v 0.687698 -0.725995 0.000000
+v 0.605173 -0.796093 0.000000
+v 0.515552 -0.856857 0.000000
+v 0.419888 -0.907575 0.000000
+v 0.319300 -0.947653 -0.000000
+v 0.214969 -0.976620 0.000000
+v 0.108118 -0.994138 0.000000
+v 0.106040 0.994138 0.021093
+v 0.210838 0.976621 0.041939
+v 0.313164 0.947653 0.062292
+v 0.411819 0.907575 0.081916
+v 0.505646 0.856857 0.100579
+v 0.593544 0.796093 0.118063
+v 0.674483 0.725995 0.134163
+v 0.747515 0.647386 0.148690
+v 0.811783 0.561187 0.161474
+v 0.866534 0.468408 0.172364
+v 0.911125 0.370138 0.181234
+v 0.945033 0.267528 0.187979
+v 0.967863 0.161782 0.192520
+v 0.979345 0.054139 0.194804
+v 0.979345 -0.054139 0.194804
+v 0.967863 -0.161782 0.192520
+v 0.945033 -0.267528 0.187979
+v 0.911125 -0.370138 0.181234
+v 0.866534 -0.468408 0.172364
+v 0.811783 -0.561187 0.161474
+v 0.747515 -0.647386 0.148690
+v 0.674484 -0.725995 0.134163
+v 0.593544 -0.796093 0.118064
+v 0.505646 -0.856857 0.100579
+v 0.411820 -0.907575 0.081916
+v 0.313165 -0.947653 0.062293
+v 0.210839 -0.976620 0.041939
+v 0.106041 -0.994138 0.021093
+v 0.099887 0.994138 0.041375
+v 0.198605 0.976621 0.082265
+v 0.294994 0.947653 0.122191
+v 0.387925 0.907575 0.160684
+v 0.476308 0.856857 0.197293
+v 0.559106 0.796093 0.231590
+v 0.635349 0.725995 0.263171
+v 0.704144 0.647386 0.291666
+v 0.764683 0.561187 0.316742
+v 0.816257 0.468408 0.338105
+v 0.858260 0.370138 0.355504
+v 0.890202 0.267528 0.368734
+v 0.911707 0.161782 0.377642
+v 0.922522 0.054139 0.382122
+v 0.922522 -0.054139 0.382122
+v 0.911707 -0.161782 0.377642
+v 0.890202 -0.267528 0.368734
+v 0.858260 -0.370138 0.355504
+v 0.816257 -0.468408 0.338105
+v 0.764683 -0.561187 0.316743
+v 0.704144 -0.647386 0.291667
+v 0.635350 -0.725995 0.263171
+v 0.559106 -0.796093 0.231590
+v 0.476308 -0.856857 0.197294
+v 0.387926 -0.907575 0.160684
+v 0.294995 -0.947653 0.122191
+v 0.198606 -0.976620 0.082266
+v 0.099888 -0.994138 0.041375
+v 0.089896 0.994138 0.060067
+v 0.178740 0.976621 0.119431
+v 0.265488 0.947653 0.177394
+v 0.349123 0.907575 0.233277
+v 0.428666 0.856857 0.286426
+v 0.503182 0.796093 0.336216
+v 0.571799 0.725995 0.382065
+v 0.633713 0.647386 0.423434
+v 0.688196 0.561187 0.459839
+v 0.734612 0.468408 0.490852
+v 0.772414 0.370138 0.516111
+v 0.801161 0.267528 0.535319
+v 0.820514 0.161782 0.548251
+v 0.830248 0.054139 0.554755
+v 0.830248 -0.054139 0.554755
+v 0.820514 -0.161782 0.548251
+v 0.801161 -0.267528 0.535319
+v 0.772414 -0.370138 0.516111
+v 0.734612 -0.468408 0.490852
+v 0.688196 -0.561187 0.459839
+v 0.633713 -0.647386 0.423434
+v 0.571800 -0.725995 0.382065
+v 0.503182 -0.796093 0.336216
+v 0.428666 -0.856857 0.286426
+v 0.349124 -0.907575 0.233278
+v 0.265488 -0.947653 0.177394
+v 0.178740 -0.976620 0.119431
+v 0.089897 -0.994138 0.060068
+v 0.076450 0.994138 0.076451
+v 0.152006 0.976621 0.152006
+v 0.225779 0.947653 0.225779
+v 0.296905 0.907575 0.296906
+v 0.364550 0.856857 0.364551
+v 0.427921 0.796093 0.427922
+v 0.486275 0.725995 0.486276
+v 0.538928 0.647386 0.538929
+v 0.585263 0.561187 0.585264
+v 0.624736 0.468408 0.624736
+v 0.656884 0.370138 0.656885
+v 0.681331 0.267528 0.681332
+v 0.697790 0.161782 0.697791
+v 0.706068 0.054139 0.706069
+v 0.706068 -0.054139 0.706069
+v 0.697790 -0.161782 0.697791
+v 0.681331 -0.267528 0.681332
+v 0.656884 -0.370138 0.656885
+v 0.624736 -0.468408 0.624736
+v 0.585263 -0.561187 0.585264
+v 0.538928 -0.647386 0.538929
+v 0.486275 -0.725995 0.486276
+v 0.427921 -0.796093 0.427922
+v 0.364550 -0.856857 0.364551
+v 0.296905 -0.907575 0.296906
+v 0.225779 -0.947653 0.225780
+v 0.152006 -0.976620 0.152007
+v 0.076451 -0.994138 0.076452
+v 0.060066 0.994138 0.089897
+v 0.119430 0.976621 0.178741
+v 0.177393 0.947653 0.265489
+v 0.233276 0.907575 0.349124
+v 0.286425 0.856857 0.428666
+v 0.336215 0.796093 0.503183
+v 0.382064 0.725995 0.571800
+v 0.423433 0.647386 0.633714
+v 0.459838 0.561187 0.688197
+v 0.490851 0.468408 0.734612
+v 0.516110 0.370138 0.772415
+v 0.535318 0.267528 0.801162
+v 0.548250 0.161782 0.820515
+v 0.554754 0.054139 0.830249
+v 0.554754 -0.054139 0.830249
+v 0.548250 -0.161782 0.820515
+v 0.535318 -0.267528 0.801162
+v 0.516110 -0.370138 0.772415
+v 0.490851 -0.468408 0.734612
+v 0.459838 -0.561187 0.688197
+v 0.423433 -0.647386 0.633714
+v 0.382064 -0.725995 0.571800
+v 0.336215 -0.796093 0.503183
+v 0.286425 -0.856857 0.428667
+v 0.233277 -0.907575 0.349125
+v 0.177393 -0.947653 0.265489
+v 0.119430 -0.976620 0.178741
+v 0.060067 -0.994138 0.089898
+v 0.041374 0.994138 0.099888
+v 0.082264 0.976621 0.198606
+v 0.122190 0.947653 0.294995
+v 0.160683 0.907575 0.387926
+v 0.197293 0.856857 0.476309
+v 0.231589 0.796093 0.559107
+v 0.263170 0.725995 0.635350
+v 0.291665 0.647386 0.704145
+v 0.316741 0.561187 0.764684
+v 0.338104 0.468408 0.816258
+v 0.355502 0.370138 0.858261
+v 0.368733 0.267528 0.890203
+v 0.377641 0.161782 0.911707
+v 0.382120 0.054139 0.922523
+v 0.382120 -0.054139 0.922523
+v 0.377641 -0.161782 0.911707
+v 0.368733 -0.267528 0.890203
+v 0.355502 -0.370138 0.858261
+v 0.338104 -0.468408 0.816258
+v 0.316741 -0.561187 0.764684
+v 0.291665 -0.647386 0.704145
+v 0.263170 -0.725995 0.635351
+v 0.231589 -0.796093 0.559107
+v 0.197293 -0.856857 0.476309
+v 0.160683 -0.907575 0.387926
+v 0.122190 -0.947653 0.294996
+v 0.082265 -0.976620 0.198607
+v 0.041374 -0.994138 0.099889
+v 0.021092 0.994138 0.106041
+v 0.041938 0.976621 0.210839
+v 0.062291 0.947653 0.313165
+v 0.081915 0.907575 0.411820
+v 0.100578 0.856857 0.505646
+v 0.118063 0.796093 0.593545
+v 0.134162 0.725995 0.674484
+v 0.148689 0.647386 0.747516
+v 0.161473 0.561187 0.811784
+v 0.172363 0.468408 0.866534
+v 0.181233 0.370138 0.911125
+v 0.187978 0.267528 0.945034
+v 0.192519 0.161782 0.967863
+v 0.194803 0.054139 0.979346
+v 0.194803 -0.054139 0.979346
+v 0.192519 -0.161782 0.967863
+v 0.187978 -0.267528 0.945034
+v 0.181233 -0.370138 0.911125
+v 0.172363 -0.468408 0.866534
+v 0.161473 -0.561187 0.811784
+v 0.148689 -0.647386 0.747516
+v 0.134162 -0.725995 0.674485
+v 0.118062 -0.796093 0.593545
+v 0.100578 -0.856857 0.505647
+v 0.081915 -0.907575 0.411820
+v 0.062292 -0.947653 0.313166
+v 0.041938 -0.976620 0.210840
+v 0.021092 -0.994138 0.106041
+v -0.000001 0.994138 0.108118
+v -0.000001 0.976621 0.214969
+v -0.000001 0.947653 0.319300
+v -0.000001 0.907575 0.419888
+v -0.000001 0.856857 0.515553
+v -0.000001 0.796093 0.605173
+v -0.000001 0.725995 0.687698
+v -0.000001 0.647386 0.762161
+v -0.000001 0.561187 0.827688
+v -0.000001 0.468408 0.883511
+v -0.000001 0.370138 0.928975
+v -0.000001 0.267528 0.963549
+v -0.000001 0.161782 0.986825
+v -0.000001 0.054139 0.998532
+v -0.000001 -0.054139 0.998532
+v -0.000001 -0.161782 0.986825
+v -0.000001 -0.267528 0.963549
+v -0.000001 -0.370138 0.928975
+v -0.000001 -0.468408 0.883511
+v -0.000001 -0.561187 0.827688
+v -0.000001 -0.647386 0.762161
+v -0.000001 -0.725995 0.687698
+v -0.000001 -0.796093 0.605173
+v -0.000001 -0.856857 0.515553
+v -0.000001 -0.907575 0.419888
+v -0.000001 -0.947653 0.319301
+v -0.000001 -0.976620 0.214970
+v -0.000001 -0.994138 0.108119
+v -0.021094 0.994138 0.106041
+v -0.041939 0.976621 0.210839
+v -0.062293 0.947653 0.313165
+v -0.081917 0.907575 0.411820
+v -0.100580 0.856857 0.505646
+v -0.118064 0.796093 0.593545
+v -0.134164 0.725995 0.674484
+v -0.148691 0.647386 0.747516
+v -0.161475 0.561187 0.811784
+v -0.172365 0.468408 0.866534
+v -0.181235 0.370138 0.911125
+v -0.187980 0.267528 0.945034
+v -0.192521 0.161782 0.967863
+v -0.194805 0.054139 0.979345
+v -0.194805 -0.054139 0.979345
+v -0.192521 -0.161782 0.967863
+v -0.187980 -0.267528 0.945034
+v -0.181235 -0.370138 0.911125
+v -0.172365 -0.468408 0.866534
+v -0.161475 -0.561187 0.811784
+v -0.148691 -0.647386 0.747516
+v -0.134164 -0.725995 0.674484
+v -0.118065 -0.796093 0.593545
+v -0.100580 -0.856857 0.505647
+v -0.081917 -0.907575 0.411820
+v -0.062293 -0.947653 0.313166
+v -0.041940 -0.976620 0.210840
+v -0.021094 -0.994138 0.106041
+v -0.041376 0.994138 0.099888
+v -0.082266 0.976621 0.198606
+v -0.122192 0.947653 0.294995
+v -0.160685 0.907575 0.387926
+v -0.197294 0.856857 0.476308
+v -0.231591 0.796093 0.559107
+v -0.263172 0.725995 0.635350
+v -0.291667 0.647386 0.704145
+v -0.316743 0.561187 0.764684
+v -0.338106 0.468408 0.816257
+v -0.355505 0.370138 0.858261
+v -0.368735 0.267528 0.890203
+v -0.377642 0.161782 0.911707
+v -0.382123 0.054139 0.922523
+v -0.382123 -0.054139 0.922523
+v -0.377642 -0.161782 0.911707
+v -0.368735 -0.267528 0.890203
+v -0.355505 -0.370138 0.858261
+v -0.338106 -0.468408 0.816257
+v -0.316743 -0.561187 0.764684
+v -0.291667 -0.647386 0.704145
+v -0.263172 -0.725995 0.635350
+v -0.231591 -0.796093 0.559107
+v -0.197295 -0.856857 0.476309
+v -0.160685 -0.907575 0.387926
+v -0.122192 -0.947653 0.294996
+v -0.082266 -0.976620 0.198607
+v -0.041376 -0.994138 0.099889
+v -0.060068 0.994138 0.089897
+v -0.119432 0.976621 0.178741
+v -0.177395 0.947653 0.265489
+v -0.233278 0.907575 0.349124
+v -0.286427 0.856857 0.428666
+v -0.336217 0.796093 0.503183
+v -0.382066 0.725995 0.571800
+v -0.423435 0.647386 0.633713
+v -0.459840 0.561187 0.688197
+v -0.490853 0.468408 0.734612
+v -0.516112 0.370138 0.772415
+v -0.535320 0.267528 0.801161
+v -0.548251 0.161782 0.820515
+v -0.554756 0.054139 0.830249
+v -0.554756 -0.054139 0.830249
+v -0.548251 -0.161782 0.820515
+v -0.535320 -0.267528 0.801161
+v -0.516112 -0.370138 0.772415
+v -0.490853 -0.468408 0.734612
+v -0.459840 -0.561187 0.688197
+v -0.423435 -0.647386 0.633713
+v -0.382066 -0.725995 0.571800
+v -0.336217 -0.796093 0.503183
+v -0.286427 -0.856857 0.428667
+v -0.233278 -0.907575 0.349124
+v -0.177395 -0.947653 0.265489
+v -0.119432 -0.976620 0.178741
+v -0.060069 -0.994138 0.089898
+v -0.076452 0.994138 0.076451
+v -0.152007 0.976621 0.152006
+v -0.225780 0.947653 0.225779
+v -0.296907 0.907575 0.296905
+v -0.364552 0.856857 0.364551
+v -0.427923 0.796093 0.427922
+v -0.486277 0.725995 0.486276
+v -0.538930 0.647386 0.538929
+v -0.585265 0.561187 0.585263
+v -0.624737 0.468408 0.624736
+v -0.656886 0.370138 0.656885
+v -0.681333 0.267528 0.681332
+v -0.697791 0.161782 0.697791
+v -0.706070 0.054139 0.706068
+v -0.706070 -0.054139 0.706068
+v -0.697791 -0.161782 0.697791
+v -0.681333 -0.267528 0.681332
+v -0.656886 -0.370138 0.656885
+v -0.624737 -0.468408 0.624736
+v -0.585265 -0.561187 0.585264
+v -0.538930 -0.647386 0.538929
+v -0.486277 -0.725995 0.486276
+v -0.427923 -0.796093 0.427922
+v -0.364552 -0.856857 0.364551
+v -0.296907 -0.907575 0.296906
+v -0.225781 -0.947653 0.225780
+v -0.152008 -0.976620 0.152007
+v -0.076452 -0.994138 0.076452
+v -0.089898 0.994138 0.060067
+v -0.178741 0.976621 0.119431
+v -0.265489 0.947653 0.177394
+v -0.349125 0.907575 0.233277
+v -0.428667 0.856857 0.286426
+v -0.503184 0.796093 0.336216
+v -0.571801 0.725995 0.382064
+v -0.633714 0.647386 0.423434
+v -0.688198 0.561187 0.459839
+v -0.734613 0.468408 0.490852
+v -0.772416 0.370138 0.516111
+v -0.801162 0.267528 0.535319
+v -0.820516 0.161782 0.548250
+v -0.830250 0.054139 0.554754
+v -0.830250 -0.054139 0.554754
+v -0.820516 -0.161782 0.548250
+v -0.801162 -0.267528 0.535319
+v -0.772416 -0.370138 0.516111
+v -0.734613 -0.468408 0.490852
+v -0.688198 -0.561187 0.459839
+v -0.633715 -0.647386 0.423434
+v -0.571801 -0.725995 0.382065
+v -0.503184 -0.796093 0.336216
+v -0.428668 -0.856857 0.286426
+v -0.349125 -0.907575 0.233277
+v -0.265490 -0.947653 0.177394
+v -0.178742 -0.976620 0.119431
+v -0.089898 -0.994138 0.060068
+v -0.099889 0.994138 0.041375
+v -0.198607 0.976621 0.082265
+v -0.294996 0.947653 0.122191
+v -0.387927 0.907575 0.160684
+v -0.476309 0.856857 0.197293
+v -0.559108 0.796093 0.231590
+v -0.635351 0.725995 0.263171
+v -0.704146 0.647386 0.291666
+v -0.764685 0.561187 0.316742
+v -0.816258 0.468408 0.338105
+v -0.858262 0.370138 0.355503
+v -0.890204 0.267528 0.368734
+v -0.911708 0.161782 0.377641
+v -0.922524 0.054139 0.382121
+v -0.922524 -0.054139 0.382121
+v -0.911708 -0.161782 0.377641
+v -0.890204 -0.267528 0.368734
+v -0.858262 -0.370138 0.355503
+v -0.816258 -0.468408 0.338105
+v -0.764685 -0.561187 0.316742
+v -0.704146 -0.647386 0.291666
+v -0.635351 -0.725995 0.263171
+v -0.559108 -0.796093 0.231590
+v -0.476310 -0.856857 0.197293
+v -0.387927 -0.907575 0.160684
+v -0.294997 -0.947653 0.122191
+v -0.198607 -0.976620 0.082265
+v -0.099890 -0.994138 0.041375
+v -0.106041 0.994138 0.021093
+v -0.210840 0.976621 0.041938
+v -0.313166 0.947653 0.062292
+v -0.411821 0.907575 0.081916
+v -0.505647 0.856857 0.100579
+v -0.593546 0.796093 0.118063
+v -0.674485 0.725995 0.134163
+v -0.747517 0.647386 0.148690
+v -0.811785 0.561187 0.161474
+v -0.866535 0.468408 0.172364
+v -0.911126 0.370138 0.181234
+v -0.945035 0.267528 0.187979
+v -0.967864 0.161782 0.192520
+v -0.979346 0.054139 0.194804
+v -0.979346 -0.054139 0.194804
+v -0.967864 -0.161782 0.192520
+v -0.945035 -0.267528 0.187979
+v -0.911126 -0.370138 0.181234
+v -0.866535 -0.468408 0.172364
+v -0.811785 -0.561187 0.161474
+v -0.747517 -0.647386 0.148690
+v -0.674485 -0.725995 0.134163
+v -0.593546 -0.796093 0.118063
+v -0.505648 -0.856857 0.100579
+v -0.411821 -0.907575 0.081916
+v -0.313167 -0.947653 0.062293
+v -0.210840 -0.976620 0.041939
+v -0.106042 -0.994138 0.021093
+vt 1.0000 0.4483
+vt 1.0000 0.4828
+vt 0.9688 0.4828
+vt 0.9688 0.4483
+vt 1.0000 0.8966
+vt 1.0000 0.9310
+vt 0.9688 0.9310
+vt 0.9688 0.8966
+vt 1.0000 0.4138
+vt 0.9688 0.4138
+vt 1.0000 0.8621
+vt 0.9688 0.8621
+vt 1.0000 0.3793
+vt 0.9688 0.3793
+vt 1.0000 0.8276
+vt 0.9688 0.8276
+vt 1.0000 0.3448
+vt 0.9688 0.3448
+vt 1.0000 0.7931
+vt 0.9688 0.7931
+vt 1.0000 0.3103
+vt 0.9688 0.3103
+vt 1.0000 0.7586
+vt 0.9688 0.7586
+vt 1.0000 0.2759
+vt 0.9688 0.2759
+vt 1.0000 0.7241
+vt 0.9688 0.7241
+vt 1.0000 0.2414
+vt 0.9688 0.2414
+vt 1.0000 0.6897
+vt 0.9688 0.6897
+vt 1.0000 0.2069
+vt 0.9688 0.2069
+vt 1.0000 0.6552
+vt 0.9688 0.6552
+vt 1.0000 0.1724
+vt 0.9688 0.1724
+vt 1.0000 0.6207
+vt 0.9688 0.6207
+vt 1.0000 0.1379
+vt 0.9688 0.1379
+vt 1.0000 0.5862
+vt 0.9688 0.5862
+vt 1.0000 0.1034
+vt 0.9688 0.1034
+vt 1.0000 0.5517
+vt 0.9688 0.5517
+vt 1.0000 0.0690
+vt 0.9688 0.0690
+vt 1.0000 0.5172
+vt 0.9688 0.5172
+vt 0.4272 0.9623
+vt 0.4239 0.9603
+vt 0.4290 0.9623
+vt 1.0000 0.0345
+vt 0.9688 0.0345
+vt 1.0000 0.9655
+vt 0.9688 0.9655
+vt 0.4955 0.0510
+vt 0.4942 0.0538
+vt 0.4916 0.0538
+vt 0.9375 0.5517
+vt 0.9375 0.5172
+vt 0.4308 0.9623
+vt 0.9375 0.0690
+vt 0.9375 0.0345
+vt 0.9375 0.4828
+vt 0.9375 0.9655
+vt 0.9375 0.9310
+vt 0.4890 0.0538
+vt 0.9375 0.4483
+vt 0.9375 0.8966
+vt 0.9375 0.4138
+vt 0.9375 0.8621
+vt 0.9375 0.3793
+vt 0.9375 0.8276
+vt 0.9375 0.3448
+vt 0.9375 0.7931
+vt 0.9375 0.3103
+vt 0.9375 0.7586
+vt 0.9375 0.2759
+vt 0.9375 0.7241
+vt 0.9375 0.2414
+vt 0.9375 0.6897
+vt 0.9375 0.2069
+vt 0.9375 0.6552
+vt 0.9375 0.1724
+vt 0.9375 0.6207
+vt 0.9375 0.1379
+vt 0.9375 0.5862
+vt 0.9375 0.1034
+vt 0.9063 0.2759
+vt 0.9063 0.2414
+vt 0.9063 0.7241
+vt 0.9063 0.6897
+vt 0.9063 0.2069
+vt 0.9063 0.6552
+vt 0.9063 0.1724
+vt 0.9062 0.6207
+vt 0.9063 0.1379
+vt 0.9063 0.5862
+vt 0.9063 0.1034
+vt 0.9063 0.5517
+vt 0.9063 0.0690
+vt 0.9063 0.5172
+vt 0.4326 0.9623
+vt 0.9063 0.0345
+vt 0.9063 0.4828
+vt 0.9063 0.9655
+vt 0.9063 0.9310
+vt 0.4864 0.0538
+vt 0.9063 0.4483
+vt 0.9063 0.8966
+vt 0.9063 0.4138
+vt 0.9063 0.8621
+vt 0.9063 0.3793
+vt 0.9063 0.8276
+vt 0.9063 0.3448
+vt 0.9063 0.7931
+vt 0.9063 0.3103
+vt 0.9063 0.7586
+vt 0.8750 0.9310
+vt 0.8750 0.8966
+vt 0.8750 0.4483
+vt 0.8750 0.4138
+vt 0.8750 0.8621
+vt 0.8750 0.3793
+vt 0.8750 0.8276
+vt 0.8750 0.3448
+vt 0.8750 0.7931
+vt 0.8750 0.3103
+vt 0.8750 0.7586
+vt 0.8750 0.2759
+vt 0.8750 0.7241
+vt 0.8750 0.2414
+vt 0.8750 0.6897
+vt 0.8750 0.2069
+vt 0.8750 0.6552
+vt 0.8750 0.1724
+vt 0.8750 0.6207
+vt 0.8750 0.1379
+vt 0.8750 0.5862
+vt 0.8750 0.1034
+vt 0.8750 0.5517
+vt 0.8750 0.0690
+vt 0.8750 0.5172
+vt 0.4344 0.9623
+vt 0.8750 0.0345
+vt 0.8750 0.4828
+vt 0.8750 0.9655
+vt 0.4837 0.0538
+vt 0.8438 0.6552
+vt 0.8438 0.6207
+vt 0.8438 0.1724
+vt 0.8438 0.1379
+vt 0.8438 0.5862
+vt 0.8438 0.1034
+vt 0.8438 0.5517
+vt 0.8438 0.0690
+vt 0.8438 0.5172
+vt 0.4362 0.9623
+vt 0.8438 0.0345
+vt 0.8438 0.4828
+vt 0.8438 0.9655
+vt 0.8438 0.9310
+vt 0.4811 0.0538
+vt 0.8438 0.4483
+vt 0.8438 0.8966
+vt 0.8438 0.4138
+vt 0.8438 0.8621
+vt 0.8438 0.3793
+vt 0.8438 0.8276
+vt 0.8438 0.3448
+vt 0.8438 0.7931
+vt 0.8438 0.3103
+vt 0.8438 0.7586
+vt 0.8438 0.2759
+vt 0.8438 0.7241
+vt 0.8438 0.2414
+vt 0.8438 0.6897
+vt 0.8438 0.2069
+vt 0.8125 0.3793
+vt 0.8125 0.3448
+vt 0.8125 0.8276
+vt 0.8125 0.7931
+vt 0.8125 0.3103
+vt 0.8125 0.7586
+vt 0.8125 0.2759
+vt 0.8125 0.7241
+vt 0.8125 0.2414
+vt 0.8125 0.6897
+vt 0.8125 0.2069
+vt 0.8125 0.6552
+vt 0.8125 0.1724
+vt 0.8125 0.6207
+vt 0.8125 0.1379
+vt 0.8125 0.5862
+vt 0.8125 0.1034
+vt 0.8125 0.5517
+vt 0.8125 0.0690
+vt 0.8125 0.5172
+vt 0.4381 0.9623
+vt 0.8125 0.0345
+vt 0.8125 0.4828
+vt 0.8125 0.9655
+vt 0.8125 0.9310
+vt 0.4785 0.0538
+vt 0.8125 0.4483
+vt 0.8125 0.8966
+vt 0.8125 0.4138
+vt 0.8125 0.8621
+vt 0.7813 0.1034
+vt 0.7813 0.0690
+vt 0.7813 0.5517
+vt 0.7813 0.5172
+vt 0.4399 0.9623
+vt 0.7813 0.0345
+vt 0.7813 0.4828
+vt 0.7813 0.9655
+vt 0.7813 0.9310
+vt 0.4759 0.0538
+vt 0.7813 0.4483
+vt 0.7813 0.8966
+vt 0.7813 0.4138
+vt 0.7813 0.8621
+vt 0.7813 0.3793
+vt 0.7813 0.8276
+vt 0.7813 0.3448
+vt 0.7813 0.7931
+vt 0.7813 0.3103
+vt 0.7813 0.7586
+vt 0.7813 0.2759
+vt 0.7813 0.7241
+vt 0.7813 0.2414
+vt 0.7813 0.6897
+vt 0.7813 0.2069
+vt 0.7813 0.6552
+vt 0.7813 0.1724
+vt 0.7813 0.6207
+vt 0.7813 0.1379
+vt 0.7813 0.5862
+vt 0.7500 0.2759
+vt 0.7500 0.2414
+vt 0.7500 0.7241
+vt 0.7500 0.6897
+vt 0.7500 0.2069
+vt 0.7500 0.6552
+vt 0.7500 0.1724
+vt 0.7500 0.6207
+vt 0.7500 0.1379
+vt 0.7500 0.5862
+vt 0.7500 0.1034
+vt 0.7500 0.5517
+vt 0.7500 0.0690
+vt 0.7500 0.5172
+vt 0.4417 0.9623
+vt 0.7500 0.0345
+vt 0.7500 0.4828
+vt 0.7500 0.9655
+vt 0.7500 0.9310
+vt 0.4733 0.0538
+vt 0.7500 0.4483
+vt 0.7500 0.8966
+vt 0.7500 0.4138
+vt 0.7500 0.8621
+vt 0.7500 0.3793
+vt 0.7500 0.8276
+vt 0.7500 0.3448
+vt 0.7500 0.7931
+vt 0.7500 0.3103
+vt 0.7500 0.7586
+vt 0.7188 0.9310
+vt 0.7188 0.8966
+vt 0.7188 0.4483
+vt 0.7188 0.4138
+vt 0.7188 0.8621
+vt 0.7188 0.3793
+vt 0.7188 0.8276
+vt 0.7188 0.3448
+vt 0.7188 0.7931
+vt 0.7188 0.3103
+vt 0.7188 0.7586
+vt 0.7188 0.2759
+vt 0.7188 0.7241
+vt 0.7188 0.2414
+vt 0.7188 0.6897
+vt 0.7188 0.2069
+vt 0.7188 0.6552
+vt 0.7188 0.1724
+vt 0.7188 0.6207
+vt 0.7188 0.1379
+vt 0.7188 0.5862
+vt 0.7188 0.1034
+vt 0.7188 0.5517
+vt 0.7188 0.0690
+vt 0.7188 0.5172
+vt 0.4435 0.9623
+vt 0.7188 0.0345
+vt 0.7188 0.4828
+vt 0.7188 0.9655
+vt 0.4706 0.0538
+vt 0.6875 0.6552
+vt 0.6875 0.6207
+vt 0.6875 0.1724
+vt 0.6875 0.1379
+vt 0.6875 0.5862
+vt 0.6875 0.1034
+vt 0.6875 0.5517
+vt 0.6875 0.0690
+vt 0.6875 0.5172
+vt 0.4453 0.9623
+vt 0.6875 0.0345
+vt 0.6875 0.4828
+vt 0.6875 0.9655
+vt 0.6875 0.9310
+vt 0.4680 0.0538
+vt 0.6875 0.4483
+vt 0.6875 0.8966
+vt 0.6875 0.4138
+vt 0.6875 0.8621
+vt 0.6875 0.3793
+vt 0.6875 0.8276
+vt 0.6875 0.3448
+vt 0.6875 0.7931
+vt 0.6875 0.3103
+vt 0.6875 0.7586
+vt 0.6875 0.2759
+vt 0.6875 0.7241
+vt 0.6875 0.2414
+vt 0.6875 0.6897
+vt 0.6875 0.2069
+vt 0.6563 0.3793
+vt 0.6563 0.3448
+vt 0.6563 0.8276
+vt 0.6563 0.7931
+vt 0.6563 0.3103
+vt 0.6563 0.7586
+vt 0.6563 0.2759
+vt 0.6563 0.7241
+vt 0.6563 0.2414
+vt 0.6563 0.6897
+vt 0.6563 0.2069
+vt 0.6563 0.6552
+vt 0.6563 0.1724
+vt 0.6563 0.6207
+vt 0.6563 0.1379
+vt 0.6563 0.5862
+vt 0.6563 0.1034
+vt 0.6563 0.5517
+vt 0.6563 0.0690
+vt 0.6563 0.5172
+vt 0.4471 0.9623
+vt 0.6563 0.0345
+vt 0.6563 0.4828
+vt 0.6563 0.9655
+vt 0.6563 0.9310
+vt 0.4654 0.0538
+vt 0.6563 0.4483
+vt 0.6563 0.8966
+vt 0.6563 0.4138
+vt 0.6563 0.8621
+vt 0.6250 0.1034
+vt 0.6250 0.0690
+vt 0.6250 0.5517
+vt 0.6250 0.5172
+vt 0.4490 0.9623
+vt 0.6250 0.0345
+vt 0.6250 0.4828
+vt 0.6250 0.9655
+vt 0.6250 0.9310
+vt 0.4628 0.0538
+vt 0.6250 0.4483
+vt 0.6250 0.8966
+vt 0.6250 0.4138
+vt 0.6250 0.8621
+vt 0.6250 0.3793
+vt 0.6250 0.8276
+vt 0.6250 0.3448
+vt 0.6250 0.7931
+vt 0.6250 0.3103
+vt 0.6250 0.7586
+vt 0.6250 0.2759
+vt 0.6250 0.7241
+vt 0.6250 0.2414
+vt 0.6250 0.6897
+vt 0.6250 0.2069
+vt 0.6250 0.6552
+vt 0.6250 0.1724
+vt 0.6250 0.6207
+vt 0.6250 0.1379
+vt 0.6250 0.5862
+vt 0.5938 0.7586
+vt 0.5938 0.7241
+vt 0.5938 0.2759
+vt 0.5938 0.2414
+vt 0.5938 0.6897
+vt 0.5938 0.2069
+vt 0.5938 0.6552
+vt 0.5938 0.1724
+vt 0.5938 0.6207
+vt 0.5938 0.1379
+vt 0.5938 0.5862
+vt 0.5938 0.1034
+vt 0.5938 0.5517
+vt 0.5938 0.0690
+vt 0.5938 0.5172
+vt 0.4508 0.9623
+vt 0.5938 0.0345
+vt 0.5938 0.4828
+vt 0.5938 0.9655
+vt 0.5938 0.9310
+vt 0.4602 0.0538
+vt 0.5938 0.4483
+vt 0.5938 0.8966
+vt 0.5938 0.4138
+vt 0.5938 0.8621
+vt 0.5938 0.3793
+vt 0.5938 0.8276
+vt 0.5938 0.3448
+vt 0.5938 0.7931
+vt 0.5938 0.3103
+vt 0.5625 0.4828
+vt 0.5625 0.4483
+vt 0.5625 0.9310
+vt 0.5625 0.8966
+vt 0.5625 0.4138
+vt 0.5625 0.8621
+vt 0.5625 0.3793
+vt 0.5625 0.8276
+vt 0.5625 0.3448
+vt 0.5625 0.7931
+vt 0.5625 0.3103
+vt 0.5625 0.7586
+vt 0.5625 0.2759
+vt 0.5625 0.7241
+vt 0.5625 0.2414
+vt 0.5625 0.6897
+vt 0.5625 0.2069
+vt 0.5625 0.6552
+vt 0.5625 0.1724
+vt 0.5625 0.6207
+vt 0.5625 0.1379
+vt 0.5625 0.5862
+vt 0.5625 0.1034
+vt 0.5625 0.5517
+vt 0.5625 0.0690
+vt 0.5625 0.5172
+vt 0.4526 0.9623
+vt 0.5625 0.0345
+vt 0.5625 0.9655
+vt 0.4575 0.0538
+vt 0.5312 0.2069
+vt 0.5312 0.1724
+vt 0.5312 0.6552
+vt 0.5312 0.6207
+vt 0.5312 0.1379
+vt 0.5312 0.5862
+vt 0.5313 0.1034
+vt 0.5312 0.5517
+vt 0.5313 0.0690
+vt 0.5312 0.5172
+vt 0.4544 0.9623
+vt 0.5313 0.0345
+vt 0.5312 0.4828
+vt 0.5313 0.9655
+vt 0.5313 0.9310
+vt 0.4549 0.0538
+vt 0.5312 0.4483
+vt 0.5313 0.8966
+vt 0.5312 0.4138
+vt 0.5313 0.8621
+vt 0.5312 0.3793
+vt 0.5313 0.8276
+vt 0.5312 0.3448
+vt 0.5313 0.7931
+vt 0.5312 0.3103
+vt 0.5313 0.7586
+vt 0.5312 0.2759
+vt 0.5312 0.7241
+vt 0.5312 0.2414
+vt 0.5313 0.6897
+vt 0.5000 0.3793
+vt 0.5000 0.3448
+vt 0.5000 0.8276
+vt 0.5000 0.7931
+vt 0.5000 0.3103
+vt 0.5000 0.7586
+vt 0.5000 0.2759
+vt 0.5000 0.7241
+vt 0.5000 0.2414
+vt 0.5000 0.6897
+vt 0.5000 0.2069
+vt 0.5000 0.6552
+vt 0.5000 0.1724
+vt 0.5000 0.6207
+vt 0.5000 0.1379
+vt 0.5000 0.5862
+vt 0.5000 0.1034
+vt 0.5000 0.5517
+vt 0.5000 0.0690
+vt 0.5000 0.5172
+vt 0.4820 0.9603
+vt 0.4562 0.9623
+vt 0.5000 0.0345
+vt 0.5000 0.4828
+vt 0.5000 0.9655
+vt 0.5000 0.9310
+vt 0.4523 0.0538
+vt 0.5000 0.4483
+vt 0.5000 0.8966
+vt 0.5000 0.4138
+vt 0.5000 0.8621
+vt 0.4687 0.1034
+vt 0.4687 0.0690
+vt 0.4687 0.5517
+vt 0.4687 0.5172
+vt 0.4580 0.9623
+vt 0.4687 0.0345
+vt 0.4687 0.4828
+vt 0.4687 0.9655
+vt 0.4687 0.9310
+vt 0.4117 0.0510
+vt 0.4497 0.0538
+vt 0.4687 0.4483
+vt 0.4687 0.8966
+vt 0.4687 0.4138
+vt 0.4687 0.8621
+vt 0.4687 0.3793
+vt 0.4687 0.8276
+vt 0.4687 0.3448
+vt 0.4687 0.7931
+vt 0.4687 0.3103
+vt 0.4688 0.7586
+vt 0.4687 0.2759
+vt 0.4687 0.7241
+vt 0.4687 0.2414
+vt 0.4687 0.6897
+vt 0.4687 0.2069
+vt 0.4687 0.6552
+vt 0.4687 0.1724
+vt 0.4687 0.6207
+vt 0.4687 0.1379
+vt 0.4687 0.5862
+vt 0.4375 0.7586
+vt 0.4375 0.7241
+vt 0.4375 0.2759
+vt 0.4375 0.2414
+vt 0.4375 0.6897
+vt 0.4375 0.2069
+vt 0.4375 0.6552
+vt 0.4375 0.1724
+vt 0.4375 0.6207
+vt 0.4375 0.1379
+vt 0.4375 0.5862
+vt 0.4375 0.1034
+vt 0.4375 0.5517
+vt 0.4375 0.0690
+vt 0.4375 0.5172
+vt 0.4599 0.9623
+vt 0.4375 0.0345
+vt 0.4375 0.4828
+vt 0.4375 0.9655
+vt 0.4375 0.9310
+vt 0.4471 0.0538
+vt 0.4375 0.4483
+vt 0.4375 0.8966
+vt 0.4375 0.4138
+vt 0.4375 0.8621
+vt 0.4375 0.3793
+vt 0.4375 0.8276
+vt 0.4375 0.3448
+vt 0.4375 0.7931
+vt 0.4375 0.3103
+vt 0.4062 0.4828
+vt 0.4062 0.4483
+vt 0.4062 0.9310
+vt 0.4062 0.8966
+vt 0.4062 0.4138
+vt 0.4062 0.8621
+vt 0.4062 0.3793
+vt 0.4062 0.8276
+vt 0.4062 0.3448
+vt 0.4062 0.7931
+vt 0.4062 0.3103
+vt 0.4062 0.7586
+vt 0.4062 0.2759
+vt 0.4062 0.7241
+vt 0.4062 0.2414
+vt 0.4062 0.6897
+vt 0.4062 0.2069
+vt 0.4062 0.6552
+vt 0.4062 0.1724
+vt 0.4062 0.6207
+vt 0.4062 0.1379
+vt 0.4062 0.5862
+vt 0.4062 0.1034
+vt 0.4062 0.5517
+vt 0.4062 0.0690
+vt 0.4062 0.5172
+vt 0.4617 0.9623
+vt 0.4062 0.0345
+vt 0.4062 0.9655
+vt 0.4444 0.0538
+vt 0.3750 0.2069
+vt 0.3750 0.1724
+vt 0.3750 0.6552
+vt 0.3750 0.6207
+vt 0.3750 0.1379
+vt 0.3750 0.5862
+vt 0.3750 0.1034
+vt 0.3750 0.5517
+vt 0.3750 0.0690
+vt 0.3750 0.5172
+vt 0.4635 0.9623
+vt 0.3750 0.0345
+vt 0.3750 0.4828
+vt 0.3750 0.9655
+vt 0.3750 0.9310
+vt 0.4418 0.0538
+vt 0.3750 0.4483
+vt 0.3750 0.8966
+vt 0.3750 0.4138
+vt 0.3750 0.8621
+vt 0.3750 0.3793
+vt 0.3750 0.8276
+vt 0.3750 0.3448
+vt 0.3750 0.7931
+vt 0.3750 0.3103
+vt 0.3750 0.7586
+vt 0.3750 0.2759
+vt 0.3750 0.7241
+vt 0.3750 0.2414
+vt 0.3750 0.6897
+vt 0.3437 0.8621
+vt 0.3437 0.8276
+vt 0.3437 0.3793
+vt 0.3437 0.3448
+vt 0.3437 0.7931
+vt 0.3437 0.3103
+vt 0.3437 0.7586
+vt 0.3437 0.2759
+vt 0.3437 0.7241
+vt 0.3437 0.2414
+vt 0.3437 0.6897
+vt 0.3437 0.2069
+vt 0.3437 0.6552
+vt 0.3437 0.1724
+vt 0.3437 0.6207
+vt 0.3437 0.1379
+vt 0.3437 0.5862
+vt 0.3437 0.1034
+vt 0.3437 0.5517
+vt 0.3437 0.0690
+vt 0.3437 0.5172
+vt 0.4653 0.9623
+vt 0.3437 0.0345
+vt 0.3437 0.4828
+vt 0.3437 0.9655
+vt 0.3437 0.9310
+vt 0.4392 0.0538
+vt 0.3437 0.4483
+vt 0.3437 0.8966
+vt 0.3437 0.4138
+vt 0.3125 0.5862
+vt 0.3125 0.5517
+vt 0.3125 0.1034
+vt 0.3125 0.0690
+vt 0.3125 0.5172
+vt 0.4671 0.9623
+vt 0.3125 0.0345
+vt 0.3125 0.4828
+vt 0.3125 0.9655
+vt 0.3125 0.9310
+vt 0.4366 0.0538
+vt 0.3125 0.4483
+vt 0.3125 0.8966
+vt 0.3125 0.4138
+vt 0.3125 0.8621
+vt 0.3125 0.3793
+vt 0.3125 0.8276
+vt 0.3125 0.3448
+vt 0.3125 0.7931
+vt 0.3125 0.3103
+vt 0.3125 0.7586
+vt 0.3125 0.2759
+vt 0.3125 0.7241
+vt 0.3125 0.2414
+vt 0.3125 0.6897
+vt 0.3125 0.2069
+vt 0.3125 0.6552
+vt 0.3125 0.1724
+vt 0.3125 0.6207
+vt 0.3125 0.1379
+vt 0.2812 0.3103
+vt 0.2812 0.2759
+vt 0.2812 0.7586
+vt 0.2812 0.7241
+vt 0.2812 0.2414
+vt 0.2812 0.6897
+vt 0.2812 0.2069
+vt 0.2812 0.6552
+vt 0.2812 0.1724
+vt 0.2812 0.6207
+vt 0.2812 0.1379
+vt 0.2812 0.5862
+vt 0.2812 0.1034
+vt 0.2812 0.5517
+vt 0.2812 0.0690
+vt 0.2812 0.5172
+vt 0.4689 0.9623
+vt 0.2812 0.0345
+vt 0.2812 0.4828
+vt 0.2812 0.9655
+vt 0.2812 0.9310
+vt 0.4340 0.0538
+vt 0.2812 0.4483
+vt 0.2812 0.8966
+vt 0.2812 0.4138
+vt 0.2812 0.8621
+vt 0.2812 0.3793
+vt 0.2812 0.8276
+vt 0.2812 0.3448
+vt 0.2812 0.7931
+vt 0.4313 0.0538
+vt 0.2500 0.4828
+vt 0.2500 0.4483
+vt 0.2500 0.9310
+vt 0.2500 0.8966
+vt 0.2500 0.4138
+vt 0.2500 0.8621
+vt 0.2500 0.3793
+vt 0.2500 0.8276
+vt 0.2500 0.3448
+vt 0.2500 0.7931
+vt 0.2500 0.3103
+vt 0.2500 0.7586
+vt 0.2500 0.2759
+vt 0.2500 0.7241
+vt 0.2500 0.2414
+vt 0.2500 0.6897
+vt 0.2500 0.2069
+vt 0.2500 0.6552
+vt 0.2500 0.1724
+vt 0.2500 0.6207
+vt 0.2500 0.1379
+vt 0.2500 0.5862
+vt 0.2500 0.1034
+vt 0.2500 0.5517
+vt 0.2500 0.0690
+vt 0.2500 0.5172
+vt 0.4708 0.9623
+vt 0.2500 0.0345
+vt 0.2500 0.9655
+vt 0.2187 0.2069
+vt 0.2187 0.1724
+vt 0.2187 0.6552
+vt 0.2187 0.6207
+vt 0.2187 0.1379
+vt 0.2187 0.5862
+vt 0.2187 0.1034
+vt 0.2187 0.5517
+vt 0.2187 0.0690
+vt 0.2187 0.5172
+vt 0.4726 0.9623
+vt 0.2187 0.0345
+vt 0.2187 0.4828
+vt 0.2187 0.9655
+vt 0.2187 0.9310
+vt 0.4287 0.0538
+vt 0.2187 0.4483
+vt 0.2187 0.8966
+vt 0.2187 0.4138
+vt 0.2187 0.8621
+vt 0.2187 0.3793
+vt 0.2187 0.8276
+vt 0.2187 0.3448
+vt 0.2187 0.7931
+vt 0.2187 0.3103
+vt 0.2187 0.7586
+vt 0.2187 0.2759
+vt 0.2187 0.7241
+vt 0.2187 0.2414
+vt 0.2187 0.6897
+vt 0.1875 0.8621
+vt 0.1875 0.8276
+vt 0.1875 0.3793
+vt 0.1875 0.3448
+vt 0.1875 0.7931
+vt 0.1875 0.3103
+vt 0.1875 0.7586
+vt 0.1875 0.2759
+vt 0.1875 0.7241
+vt 0.1875 0.2414
+vt 0.1875 0.6897
+vt 0.1875 0.2069
+vt 0.1875 0.6552
+vt 0.1875 0.1724
+vt 0.1875 0.6207
+vt 0.1875 0.1379
+vt 0.1875 0.5862
+vt 0.1875 0.1034
+vt 0.1875 0.5517
+vt 0.1875 0.0690
+vt 0.1875 0.5172
+vt 0.4744 0.9623
+vt 0.1875 0.0345
+vt 0.1875 0.4828
+vt 0.1875 0.9655
+vt 0.1875 0.9310
+vt 0.4261 0.0538
+vt 0.1875 0.4483
+vt 0.1875 0.8966
+vt 0.1875 0.4138
+vt 0.1562 0.5862
+vt 0.1562 0.5517
+vt 0.1562 0.1034
+vt 0.1562 0.0690
+vt 0.1562 0.5172
+vt 0.4762 0.9623
+vt 0.1562 0.0345
+vt 0.1562 0.4828
+vt 0.1562 0.9655
+vt 0.1562 0.9310
+vt 0.4235 0.0538
+vt 0.1562 0.4483
+vt 0.1562 0.8966
+vt 0.1562 0.4138
+vt 0.1562 0.8621
+vt 0.1562 0.3793
+vt 0.1562 0.8276
+vt 0.1562 0.3448
+vt 0.1562 0.7931
+vt 0.1562 0.3103
+vt 0.1562 0.7586
+vt 0.1562 0.2759
+vt 0.1562 0.7241
+vt 0.1562 0.2414
+vt 0.1562 0.6897
+vt 0.1562 0.2069
+vt 0.1562 0.6552
+vt 0.1562 0.1724
+vt 0.1562 0.6207
+vt 0.1562 0.1379
+vt 0.1250 0.3103
+vt 0.1250 0.2759
+vt 0.1250 0.7586
+vt 0.1250 0.7241
+vt 0.1250 0.2414
+vt 0.1250 0.6897
+vt 0.1250 0.2069
+vt 0.1250 0.6552
+vt 0.1250 0.1724
+vt 0.1250 0.6207
+vt 0.1250 0.1379
+vt 0.1250 0.5862
+vt 0.1250 0.1034
+vt 0.1250 0.5517
+vt 0.1250 0.0690
+vt 0.1250 0.5172
+vt 0.4780 0.9623
+vt 0.1250 0.0345
+vt 0.1250 0.4828
+vt 0.1250 0.9655
+vt 0.1250 0.9310
+vt 0.4209 0.0538
+vt 0.1250 0.4483
+vt 0.1250 0.8966
+vt 0.1250 0.4138
+vt 0.1250 0.8621
+vt 0.1250 0.3793
+vt 0.1250 0.8276
+vt 0.1250 0.3448
+vt 0.1250 0.7931
+vt 0.0937 0.9655
+vt 0.0937 0.9310
+vt 0.0937 0.4828
+vt 0.0937 0.4483
+vt 0.0937 0.0690
+vt 0.0937 0.0345
+vt 0.0937 0.8966
+vt 0.4182 0.0538
+vt 0.0937 0.8621
+vt 0.0937 0.8276
+vt 0.0937 0.4138
+vt 0.0937 0.7931
+vt 0.0937 0.3793
+vt 0.0937 0.7586
+vt 0.0937 0.3448
+vt 0.0937 0.7241
+vt 0.0937 0.3103
+vt 0.0937 0.6897
+vt 0.0937 0.2759
+vt 0.0937 0.6552
+vt 0.0937 0.2414
+vt 0.0937 0.6207
+vt 0.0937 0.2069
+vt 0.0937 0.5862
+vt 0.0937 0.1724
+vt 0.0937 0.5517
+vt 0.0937 0.1379
+vt 0.0937 0.5172
+vt 0.0937 0.1034
+vt 0.4798 0.9623
+vt 0.0625 0.2759
+vt 0.0625 0.2414
+vt 0.0625 0.7241
+vt 0.0625 0.6897
+vt 0.0625 0.2069
+vt 0.0625 0.6552
+vt 0.0625 0.1724
+vt 0.0625 0.6207
+vt 0.0625 0.1379
+vt 0.0625 0.5862
+vt 0.0625 0.1034
+vt 0.0625 0.5517
+vt 0.0625 0.0690
+vt 0.0625 0.5172
+vt 0.4817 0.9623
+vt 0.0625 0.0345
+vt 0.0625 0.4828
+vt 0.0625 0.9655
+vt 0.0625 0.9310
+vt 0.4156 0.0538
+vt 0.0625 0.4483
+vt 0.0625 0.8966
+vt 0.0625 0.4138
+vt 0.0625 0.8621
+vt 0.0625 0.3793
+vt 0.0625 0.8276
+vt 0.0625 0.3448
+vt 0.0625 0.7931
+vt 0.0625 0.3103
+vt 0.0625 0.7586
+vt 0.0312 0.4483
+vt 0.0312 0.4138
+vt 0.0312 0.8966
+vt 0.0312 0.8621
+vt 0.0312 0.3793
+vt 0.0312 0.8276
+vt 0.0312 0.3448
+vt 0.0312 0.7931
+vt 0.0312 0.3103
+vt 0.0312 0.7586
+vt 0.0312 0.2759
+vt 0.0312 0.7241
+vt 0.0312 0.2414
+vt 0.0312 0.6897
+vt 0.0312 0.2069
+vt 0.0312 0.6552
+vt 0.0312 0.1724
+vt 0.0312 0.6207
+vt 0.0312 0.1379
+vt 0.0312 0.5862
+vt 0.0312 0.1034
+vt 0.0312 0.5517
+vt 0.0312 0.0690
+vt 0.0312 0.5172
+vt 0.4835 0.9623
+vt 0.0312 0.0345
+vt 0.0312 0.4828
+vt 0.0312 0.9655
+vt 0.0312 0.9310
+vt 0.4130 0.0538
+vt 0.0000 0.1724
+vt 0.0000 0.1379
+vt 0.0000 0.6207
+vt 0.0000 0.5862
+vt 0.0000 0.1034
+vt 0.0000 0.5517
+vt 0.0000 0.0690
+vt 0.0000 0.5172
+vt 0.4253 0.9623
+vt 0.0000 0.0345
+vt 0.0000 0.4828
+vt 1.0312 0.9310
+vt 1.0312 0.9655
+vt 0.4104 0.0538
+vt 0.0000 0.4483
+vt 1.0312 0.8966
+vt 0.0000 0.4138
+vt 0.0000 0.8966
+vt 0.0000 0.8621
+vt 0.0000 0.3793
+vt 0.0000 0.8276
+vt 0.0000 0.3448
+vt 0.0000 0.7931
+vt 0.0000 0.3103
+vt 0.0000 0.7586
+vt 0.0000 0.2759
+vt 0.0000 0.7241
+vt 0.0000 0.2414
+vt 0.0000 0.6897
+vt 0.0000 0.2069
+vt 0.0000 0.6552
+vn -0.9869 -0.1612 0.0000
+vn -0.9985 -0.0540 0.0000
+vn -0.9793 -0.0540 -0.1948
+vn -0.9679 -0.1612 -0.1925
+vn -0.3223 0.9466 0.0000
+vn -0.2182 0.9759 0.0000
+vn -0.2140 0.9759 -0.0425
+vn -0.3161 0.9466 -0.0629
+vn -0.9638 -0.2666 0.0000
+vn -0.9452 -0.2666 -0.1880
+vn -0.4227 0.9063 0.0000
+vn -0.4145 0.9063 -0.0824
+vn -0.9294 -0.3690 0.0000
+vn -0.9116 -0.3690 -0.1813
+vn -0.5180 0.8553 0.0000
+vn -0.5081 0.8553 -0.1010
+vn -0.8842 -0.4670 0.0000
+vn -0.8672 -0.4670 -0.1725
+vn -0.6073 0.7945 0.0000
+vn -0.5956 0.7945 -0.1185
+vn -0.8287 -0.5596 0.0000
+vn -0.8128 -0.5596 -0.1617
+vn -0.6894 0.7243 0.0000
+vn -0.6762 0.7243 -0.1345
+vn -0.7635 -0.6457 0.0000
+vn -0.7489 -0.6457 -0.1490
+vn -0.7635 0.6457 0.0000
+vn -0.7489 0.6457 -0.1490
+vn -0.6894 -0.7243 0.0000
+vn -0.6762 -0.7243 -0.1345
+vn -0.8287 0.5596 0.0000
+vn -0.8128 0.5596 -0.1617
+vn -0.6073 -0.7945 0.0000
+vn -0.5956 -0.7945 -0.1185
+vn -0.8842 0.4670 0.0000
+vn -0.8672 0.4670 -0.1725
+vn -0.5180 -0.8553 0.0000
+vn -0.5081 -0.8553 -0.1010
+vn -0.9294 0.3690 0.0000
+vn -0.9116 0.3690 -0.1813
+vn -0.4227 -0.9063 0.0000
+vn -0.4145 -0.9063 -0.0824
+vn -0.9638 0.2666 0.0000
+vn -0.9452 0.2666 -0.1880
+vn -0.3223 -0.9466 0.0000
+vn -0.3161 -0.9466 -0.0629
+vn -0.9869 0.1612 0.0000
+vn -0.9679 0.1612 -0.1925
+vn -0.2182 -0.9759 0.0000
+vn -0.2140 -0.9759 -0.0425
+vn -0.9985 0.0540 0.0000
+vn -0.9793 0.0540 -0.1948
+vn -0.1115 0.9937 0.0000
+vn 0.0000 1.0000 0.0000
+vn -0.1093 0.9937 -0.0217
+vn -0.1115 -0.9937 0.0000
+vn -0.1093 -0.9937 -0.0217
+vn 0.0000 -1.0000 0.0000
+vn -0.9118 0.1612 -0.3777
+vn -0.9225 0.0540 -0.3821
+vn -0.1030 0.9937 -0.0426
+vn -0.2016 -0.9759 -0.0835
+vn -0.1030 -0.9937 -0.0426
+vn -0.9225 -0.0540 -0.3821
+vn -0.2016 0.9759 -0.0835
+vn -0.9118 -0.1612 -0.3777
+vn -0.2978 0.9466 -0.1233
+vn -0.8904 -0.2666 -0.3688
+vn -0.3905 0.9063 -0.1617
+vn -0.8587 -0.3690 -0.3557
+vn -0.4786 0.8553 -0.1982
+vn -0.8169 -0.4670 -0.3384
+vn -0.5611 0.7945 -0.2324
+vn -0.7656 -0.5596 -0.3171
+vn -0.6370 0.7243 -0.2638
+vn -0.7054 -0.6457 -0.2922
+vn -0.7054 0.6457 -0.2922
+vn -0.6370 -0.7243 -0.2638
+vn -0.7656 0.5596 -0.3171
+vn -0.5611 -0.7945 -0.2324
+vn -0.8169 0.4670 -0.3384
+vn -0.4786 -0.8553 -0.1982
+vn -0.8587 0.3690 -0.3557
+vn -0.3905 -0.9063 -0.1617
+vn -0.8904 0.2666 -0.3688
+vn -0.2978 -0.9466 -0.1233
+vn -0.6349 -0.6457 -0.4242
+vn -0.5733 -0.7243 -0.3830
+vn -0.6349 0.6457 -0.4242
+vn -0.6891 0.5596 -0.4604
+vn -0.5050 -0.7945 -0.3374
+vn -0.7352 0.4670 -0.4913
+vn -0.4307 -0.8553 -0.2878
+vn -0.7728 0.3690 -0.5163
+vn -0.3514 -0.9063 -0.2348
+vn -0.8014 0.2666 -0.5354
+vn -0.2680 -0.9466 -0.1791
+vn -0.8206 0.1612 -0.5483
+vn -0.1814 -0.9759 -0.1212
+vn -0.8303 0.0540 -0.5547
+vn -0.0927 0.9937 -0.0619
+vn -0.0927 -0.9937 -0.0619
+vn -0.8303 -0.0540 -0.5547
+vn -0.1814 0.9759 -0.1212
+vn -0.8206 -0.1612 -0.5483
+vn -0.2680 0.9466 -0.1791
+vn -0.8014 -0.2666 -0.5354
+vn -0.3514 0.9063 -0.2348
+vn -0.7728 -0.3690 -0.5163
+vn -0.4307 0.8553 -0.2878
+vn -0.7352 -0.4670 -0.4913
+vn -0.5049 0.7945 -0.3374
+vn -0.6891 -0.5596 -0.4604
+vn -0.5733 0.7243 -0.3830
+vn -0.1543 0.9759 -0.1543
+vn -0.2279 0.9466 -0.2279
+vn -0.6978 -0.1612 -0.6978
+vn -0.6815 -0.2666 -0.6815
+vn -0.2989 0.9063 -0.2989
+vn -0.6572 -0.3690 -0.6572
+vn -0.3663 0.8553 -0.3663
+vn -0.6252 -0.4670 -0.6252
+vn -0.4294 0.7945 -0.4294
+vn -0.5860 -0.5596 -0.5860
+vn -0.4875 0.7243 -0.4875
+vn -0.5399 -0.6457 -0.5399
+vn -0.5399 0.6457 -0.5399
+vn -0.4875 -0.7243 -0.4875
+vn -0.5860 0.5596 -0.5860
+vn -0.4294 -0.7945 -0.4294
+vn -0.6252 0.4670 -0.6252
+vn -0.3663 -0.8553 -0.3663
+vn -0.6572 0.3690 -0.6572
+vn -0.2989 -0.9063 -0.2989
+vn -0.6815 0.2666 -0.6815
+vn -0.2279 -0.9466 -0.2279
+vn -0.6978 0.1612 -0.6978
+vn -0.1543 -0.9759 -0.1543
+vn -0.7061 0.0540 -0.7061
+vn -0.0788 0.9937 -0.0788
+vn -0.0788 -0.9937 -0.0788
+vn -0.7061 -0.0540 -0.7061
+vn -0.4913 0.4670 -0.7352
+vn -0.5163 0.3690 -0.7728
+vn -0.2878 -0.8553 -0.4307
+vn -0.2348 -0.9063 -0.3514
+vn -0.5354 0.2666 -0.8014
+vn -0.1791 -0.9466 -0.2680
+vn -0.5483 0.1612 -0.8206
+vn -0.1212 -0.9759 -0.1814
+vn -0.5547 0.0540 -0.8303
+vn -0.0619 0.9937 -0.0927
+vn -0.0619 -0.9937 -0.0927
+vn -0.5547 -0.0540 -0.8303
+vn -0.1212 0.9759 -0.1814
+vn -0.5483 -0.1612 -0.8206
+vn -0.1791 0.9466 -0.2680
+vn -0.5354 -0.2666 -0.8014
+vn -0.2348 0.9063 -0.3514
+vn -0.5163 -0.3690 -0.7728
+vn -0.2878 0.8553 -0.4307
+vn -0.4913 -0.4670 -0.7352
+vn -0.3374 0.7945 -0.5049
+vn -0.4604 -0.5596 -0.6891
+vn -0.3830 0.7243 -0.5733
+vn -0.4242 -0.6457 -0.6349
+vn -0.4242 0.6457 -0.6349
+vn -0.3830 -0.7243 -0.5733
+vn -0.4604 0.5596 -0.6891
+vn -0.3374 -0.7945 -0.5050
+vn -0.3557 -0.3690 -0.8587
+vn -0.3384 -0.4670 -0.8169
+vn -0.1982 0.8553 -0.4786
+vn -0.2324 0.7945 -0.5611
+vn -0.3171 -0.5596 -0.7656
+vn -0.2638 0.7243 -0.6370
+vn -0.2922 -0.6457 -0.7054
+vn -0.2922 0.6457 -0.7054
+vn -0.2638 -0.7243 -0.6370
+vn -0.3171 0.5596 -0.7656
+vn -0.2324 -0.7945 -0.5611
+vn -0.3384 0.4670 -0.8169
+vn -0.1982 -0.8553 -0.4786
+vn -0.3557 0.3690 -0.8587
+vn -0.1617 -0.9063 -0.3905
+vn -0.3688 0.2666 -0.8904
+vn -0.1233 -0.9466 -0.2978
+vn -0.3777 0.1612 -0.9118
+vn -0.0835 -0.9759 -0.2016
+vn -0.3821 0.0540 -0.9225
+vn -0.0426 0.9937 -0.1030
+vn -0.0426 -0.9937 -0.1030
+vn -0.3821 -0.0540 -0.9225
+vn -0.0835 0.9759 -0.2016
+vn -0.3777 -0.1612 -0.9118
+vn -0.1233 0.9466 -0.2978
+vn -0.3688 -0.2666 -0.8904
+vn -0.1617 0.9063 -0.3905
+vn -0.0629 -0.9466 -0.3161
+vn -0.0425 -0.9759 -0.2140
+vn -0.1925 0.1612 -0.9679
+vn -0.1948 0.0540 -0.9793
+vn -0.0217 0.9937 -0.1093
+vn -0.0217 -0.9937 -0.1093
+vn -0.1948 -0.0540 -0.9793
+vn -0.0425 0.9759 -0.2140
+vn -0.1925 -0.1612 -0.9679
+vn -0.0629 0.9466 -0.3161
+vn -0.1880 -0.2666 -0.9452
+vn -0.0824 0.9063 -0.4145
+vn -0.1813 -0.3690 -0.9116
+vn -0.1010 0.8553 -0.5081
+vn -0.1725 -0.4670 -0.8672
+vn -0.1185 0.7945 -0.5956
+vn -0.1617 -0.5596 -0.8128
+vn -0.1345 0.7243 -0.6762
+vn -0.1490 -0.6457 -0.7489
+vn -0.1490 0.6457 -0.7489
+vn -0.1345 -0.7243 -0.6762
+vn -0.1617 0.5596 -0.8128
+vn -0.1185 -0.7945 -0.5956
+vn -0.1725 0.4670 -0.8672
+vn -0.1010 -0.8553 -0.5081
+vn -0.1813 0.3690 -0.9116
+vn -0.0824 -0.9063 -0.4145
+vn -0.1880 0.2666 -0.9452
+vn 0.0000 -0.6457 -0.7635
+vn 0.0000 -0.7243 -0.6894
+vn 0.0000 0.6457 -0.7635
+vn 0.0000 0.5596 -0.8287
+vn 0.0000 -0.7945 -0.6073
+vn 0.0000 0.4670 -0.8842
+vn 0.0000 -0.8553 -0.5180
+vn 0.0000 0.3690 -0.9294
+vn 0.0000 -0.9063 -0.4227
+vn 0.0000 0.2666 -0.9638
+vn 0.0000 -0.9466 -0.3223
+vn 0.0000 0.1612 -0.9869
+vn 0.0000 -0.9759 -0.2182
+vn 0.0000 0.0540 -0.9985
+vn 0.0000 0.9937 -0.1115
+vn 0.0000 -0.9937 -0.1115
+vn 0.0000 -0.0540 -0.9985
+vn 0.0000 0.9759 -0.2182
+vn 0.0000 -0.1612 -0.9869
+vn 0.0000 0.9466 -0.3223
+vn 0.0000 -0.2666 -0.9638
+vn 0.0000 0.9063 -0.4227
+vn 0.0000 -0.3690 -0.9294
+vn 0.0000 0.8553 -0.5180
+vn 0.0000 -0.4670 -0.8842
+vn 0.0000 0.7945 -0.6073
+vn 0.0000 -0.5596 -0.8287
+vn 0.0000 0.7243 -0.6894
+vn 0.0425 0.9759 -0.2140
+vn 0.0629 0.9466 -0.3161
+vn 0.1925 -0.1612 -0.9679
+vn 0.1880 -0.2666 -0.9452
+vn 0.0824 0.9063 -0.4145
+vn 0.1813 -0.3690 -0.9116
+vn 0.1010 0.8553 -0.5081
+vn 0.1725 -0.4670 -0.8672
+vn 0.1185 0.7945 -0.5956
+vn 0.1617 -0.5596 -0.8128
+vn 0.1345 0.7243 -0.6762
+vn 0.1490 -0.6457 -0.7489
+vn 0.1490 0.6457 -0.7489
+vn 0.1345 -0.7243 -0.6762
+vn 0.1617 0.5596 -0.8128
+vn 0.1185 -0.7945 -0.5956
+vn 0.1725 0.4670 -0.8672
+vn 0.1010 -0.8553 -0.5081
+vn 0.1813 0.3690 -0.9116
+vn 0.0824 -0.9063 -0.4145
+vn 0.1880 0.2666 -0.9452
+vn 0.0629 -0.9466 -0.3161
+vn 0.1925 0.1612 -0.9679
+vn 0.0425 -0.9759 -0.2140
+vn 0.1948 0.0540 -0.9793
+vn 0.0217 0.9937 -0.1093
+vn 0.0217 -0.9937 -0.1093
+vn 0.1948 -0.0540 -0.9793
+vn 0.3384 0.4670 -0.8169
+vn 0.3557 0.3690 -0.8587
+vn 0.1982 -0.8553 -0.4786
+vn 0.1617 -0.9063 -0.3905
+vn 0.3688 0.2666 -0.8904
+vn 0.1233 -0.9466 -0.2978
+vn 0.3777 0.1612 -0.9118
+vn 0.0835 -0.9759 -0.2016
+vn 0.3821 0.0540 -0.9225
+vn 0.0426 0.9937 -0.1030
+vn 0.0426 -0.9937 -0.1030
+vn 0.3821 -0.0540 -0.9225
+vn 0.0835 0.9759 -0.2016
+vn 0.3777 -0.1612 -0.9118
+vn 0.1233 0.9466 -0.2978
+vn 0.3688 -0.2666 -0.8904
+vn 0.1617 0.9063 -0.3905
+vn 0.3557 -0.3690 -0.8587
+vn 0.1982 0.8553 -0.4786
+vn 0.3384 -0.4670 -0.8169
+vn 0.2324 0.7945 -0.5611
+vn 0.3171 -0.5596 -0.7656
+vn 0.2638 0.7243 -0.6370
+vn 0.2922 -0.6457 -0.7054
+vn 0.2922 0.6457 -0.7054
+vn 0.2638 -0.7243 -0.6370
+vn 0.3171 0.5596 -0.7656
+vn 0.2324 -0.7945 -0.5611
+vn 0.5163 -0.3690 -0.7728
+vn 0.4913 -0.4670 -0.7352
+vn 0.2878 0.8553 -0.4307
+vn 0.3374 0.7945 -0.5049
+vn 0.4604 -0.5596 -0.6891
+vn 0.3830 0.7243 -0.5733
+vn 0.4242 -0.6457 -0.6349
+vn 0.4242 0.6457 -0.6349
+vn 0.3830 -0.7243 -0.5733
+vn 0.4604 0.5596 -0.6891
+vn 0.3374 -0.7945 -0.5050
+vn 0.4913 0.4670 -0.7352
+vn 0.2878 -0.8553 -0.4307
+vn 0.5163 0.3690 -0.7728
+vn 0.2348 -0.9063 -0.3514
+vn 0.5354 0.2666 -0.8014
+vn 0.1791 -0.9466 -0.2680
+vn 0.5483 0.1612 -0.8206
+vn 0.1212 -0.9759 -0.1814
+vn 0.5547 0.0540 -0.8303
+vn 0.0619 0.9937 -0.0927
+vn 0.0619 -0.9937 -0.0927
+vn 0.5547 -0.0540 -0.8303
+vn 0.1212 0.9759 -0.1814
+vn 0.5483 -0.1612 -0.8206
+vn 0.1791 0.9466 -0.2680
+vn 0.5354 -0.2666 -0.8014
+vn 0.2348 0.9063 -0.3514
+vn 0.2279 -0.9466 -0.2279
+vn 0.1543 -0.9759 -0.1543
+vn 0.6978 0.1612 -0.6978
+vn 0.7061 0.0540 -0.7061
+vn 0.0788 0.9937 -0.0788
+vn 0.0788 -0.9937 -0.0788
+vn 0.7061 -0.0540 -0.7061
+vn 0.1543 0.9759 -0.1543
+vn 0.6978 -0.1612 -0.6978
+vn 0.2279 0.9466 -0.2279
+vn 0.6815 -0.2666 -0.6815
+vn 0.2989 0.9063 -0.2989
+vn 0.6572 -0.3690 -0.6572
+vn 0.3663 0.8553 -0.3663
+vn 0.6252 -0.4670 -0.6252
+vn 0.4294 0.7945 -0.4294
+vn 0.5860 -0.5596 -0.5860
+vn 0.4875 0.7243 -0.4875
+vn 0.5399 -0.6457 -0.5399
+vn 0.5399 0.6457 -0.5399
+vn 0.4875 -0.7243 -0.4875
+vn 0.5860 0.5596 -0.5860
+vn 0.4294 -0.7945 -0.4294
+vn 0.6252 0.4670 -0.6252
+vn 0.3663 -0.8553 -0.3663
+vn 0.6572 0.3690 -0.6572
+vn 0.2989 -0.9063 -0.2989
+vn 0.6815 0.2666 -0.6815
+vn 0.5733 0.7243 -0.3830
+vn 0.6349 0.6457 -0.4242
+vn 0.6349 -0.6457 -0.4242
+vn 0.5733 -0.7243 -0.3830
+vn 0.6891 0.5596 -0.4604
+vn 0.5050 -0.7945 -0.3374
+vn 0.7352 0.4670 -0.4913
+vn 0.4307 -0.8553 -0.2878
+vn 0.7728 0.3690 -0.5163
+vn 0.3514 -0.9063 -0.2348
+vn 0.8014 0.2666 -0.5354
+vn 0.2680 -0.9466 -0.1791
+vn 0.8206 0.1612 -0.5483
+vn 0.1814 -0.9759 -0.1212
+vn 0.8303 0.0540 -0.5547
+vn 0.0927 0.9937 -0.0619
+vn 0.0927 -0.9937 -0.0619
+vn 0.8303 -0.0540 -0.5547
+vn 0.1814 0.9759 -0.1212
+vn 0.8206 -0.1612 -0.5483
+vn 0.2680 0.9466 -0.1791
+vn 0.8014 -0.2666 -0.5354
+vn 0.3514 0.9063 -0.2348
+vn 0.7728 -0.3690 -0.5163
+vn 0.4307 0.8553 -0.2878
+vn 0.7352 -0.4670 -0.4913
+vn 0.5049 0.7945 -0.3374
+vn 0.6891 -0.5596 -0.4604
+vn 0.9225 -0.0540 -0.3821
+vn 0.9118 -0.1612 -0.3777
+vn 0.2016 0.9759 -0.0835
+vn 0.2978 0.9466 -0.1233
+vn 0.8904 -0.2666 -0.3688
+vn 0.3905 0.9063 -0.1617
+vn 0.8587 -0.3690 -0.3557
+vn 0.4786 0.8553 -0.1982
+vn 0.8169 -0.4670 -0.3384
+vn 0.5611 0.7945 -0.2324
+vn 0.7656 -0.5596 -0.3171
+vn 0.6370 0.7243 -0.2638
+vn 0.7054 -0.6457 -0.2922
+vn 0.7054 0.6457 -0.2922
+vn 0.6370 -0.7243 -0.2638
+vn 0.7656 0.5596 -0.3171
+vn 0.5611 -0.7945 -0.2324
+vn 0.8169 0.4670 -0.3384
+vn 0.4786 -0.8553 -0.1982
+vn 0.8587 0.3690 -0.3557
+vn 0.3905 -0.9063 -0.1617
+vn 0.8904 0.2666 -0.3688
+vn 0.2978 -0.9466 -0.1233
+vn 0.9118 0.1612 -0.3777
+vn 0.2016 -0.9759 -0.0835
+vn 0.9225 0.0540 -0.3821
+vn 0.1030 0.9937 -0.0426
+vn 0.1030 -0.9937 -0.0426
+vn 0.5956 -0.7945 -0.1185
+vn 0.5081 -0.8553 -0.1010
+vn 0.8672 0.4670 -0.1725
+vn 0.9116 0.3690 -0.1813
+vn 0.4145 -0.9063 -0.0824
+vn 0.9452 0.2666 -0.1880
+vn 0.3161 -0.9466 -0.0629
+vn 0.9679 0.1612 -0.1925
+vn 0.2140 -0.9759 -0.0425
+vn 0.9793 0.0540 -0.1948
+vn 0.1093 0.9937 -0.0217
+vn 0.1093 -0.9937 -0.0217
+vn 0.9793 -0.0540 -0.1948
+vn 0.2140 0.9759 -0.0425
+vn 0.9679 -0.1612 -0.1925
+vn 0.3161 0.9466 -0.0629
+vn 0.9452 -0.2666 -0.1880
+vn 0.4145 0.9063 -0.0824
+vn 0.9116 -0.3690 -0.1813
+vn 0.5081 0.8553 -0.1010
+vn 0.8672 -0.4670 -0.1725
+vn 0.5956 0.7945 -0.1185
+vn 0.8128 -0.5596 -0.1617
+vn 0.6762 0.7243 -0.1345
+vn 0.7489 -0.6457 -0.1490
+vn 0.7489 0.6457 -0.1490
+vn 0.6762 -0.7243 -0.1345
+vn 0.8128 0.5596 -0.1617
+vn 0.9294 -0.3690 0.0000
+vn 0.8842 -0.4670 0.0000
+vn 0.5180 0.8553 0.0000
+vn 0.6073 0.7945 0.0000
+vn 0.8287 -0.5596 0.0000
+vn 0.6894 0.7243 0.0000
+vn 0.7635 -0.6457 0.0000
+vn 0.7635 0.6457 0.0000
+vn 0.6894 -0.7243 0.0000
+vn 0.8287 0.5596 0.0000
+vn 0.6073 -0.7945 0.0000
+vn 0.8842 0.4670 0.0000
+vn 0.5180 -0.8553 0.0000
+vn 0.9294 0.3690 0.0000
+vn 0.4227 -0.9063 0.0000
+vn 0.9638 0.2666 0.0000
+vn 0.3223 -0.9466 0.0000
+vn 0.9869 0.1612 0.0000
+vn 0.2182 -0.9759 0.0000
+vn 0.9985 0.0540 0.0000
+vn 0.1115 0.9937 0.0000
+vn 0.1115 -0.9937 0.0000
+vn 0.9985 -0.0540 0.0000
+vn 0.2182 0.9759 0.0000
+vn 0.9869 -0.1612 0.0000
+vn 0.3223 0.9466 0.0000
+vn 0.9638 -0.2666 0.0000
+vn 0.4227 0.9063 0.0000
+vn 0.3161 -0.9466 0.0629
+vn 0.2140 -0.9759 0.0425
+vn 0.9679 0.1612 0.1925
+vn 0.9793 0.0540 0.1948
+vn 0.1093 0.9937 0.0217
+vn 0.1093 -0.9937 0.0217
+vn 0.9793 -0.0540 0.1948
+vn 0.2140 0.9759 0.0425
+vn 0.9679 -0.1612 0.1925
+vn 0.3161 0.9466 0.0629
+vn 0.9452 -0.2666 0.1880
+vn 0.4145 0.9063 0.0824
+vn 0.9116 -0.3690 0.1813
+vn 0.5081 0.8553 0.1010
+vn 0.8672 -0.4670 0.1725
+vn 0.5956 0.7945 0.1185
+vn 0.8128 -0.5596 0.1617
+vn 0.6762 0.7243 0.1345
+vn 0.7489 -0.6457 0.1490
+vn 0.7489 0.6457 0.1490
+vn 0.6762 -0.7243 0.1345
+vn 0.8128 0.5596 0.1617
+vn 0.5956 -0.7945 0.1185
+vn 0.8672 0.4670 0.1725
+vn 0.5081 -0.8553 0.1010
+vn 0.9116 0.3690 0.1813
+vn 0.4145 -0.9063 0.0824
+vn 0.9452 0.2666 0.1880
+vn 0.6370 0.7243 0.2638
+vn 0.7054 0.6457 0.2922
+vn 0.7054 -0.6457 0.2922
+vn 0.6370 -0.7243 0.2638
+vn 0.7656 0.5596 0.3171
+vn 0.5611 -0.7945 0.2324
+vn 0.8169 0.4670 0.3384
+vn 0.4786 -0.8553 0.1982
+vn 0.8587 0.3690 0.3557
+vn 0.3905 -0.9063 0.1617
+vn 0.8904 0.2666 0.3688
+vn 0.2978 -0.9466 0.1233
+vn 0.9118 0.1612 0.3777
+vn 0.2016 -0.9759 0.0835
+vn 0.9225 0.0540 0.3821
+vn 0.1030 0.9937 0.0426
+vn 0.1030 -0.9937 0.0426
+vn 0.9225 -0.0540 0.3821
+vn 0.2016 0.9759 0.0835
+vn 0.9118 -0.1612 0.3777
+vn 0.2978 0.9466 0.1233
+vn 0.8904 -0.2666 0.3688
+vn 0.3905 0.9063 0.1617
+vn 0.8587 -0.3690 0.3557
+vn 0.4786 0.8553 0.1982
+vn 0.8169 -0.4670 0.3384
+vn 0.5611 0.7945 0.2324
+vn 0.7656 -0.5596 0.3171
+vn 0.8303 -0.0540 0.5547
+vn 0.8206 -0.1612 0.5483
+vn 0.1814 0.9759 0.1212
+vn 0.2680 0.9466 0.1791
+vn 0.8014 -0.2666 0.5354
+vn 0.3514 0.9063 0.2348
+vn 0.7728 -0.3690 0.5163
+vn 0.4307 0.8553 0.2878
+vn 0.7352 -0.4670 0.4913
+vn 0.5050 0.7945 0.3374
+vn 0.6891 -0.5596 0.4604
+vn 0.5733 0.7243 0.3830
+vn 0.6349 -0.6457 0.4242
+vn 0.6349 0.6457 0.4242
+vn 0.5733 -0.7243 0.3830
+vn 0.6891 0.5596 0.4604
+vn 0.5049 -0.7945 0.3374
+vn 0.7352 0.4670 0.4913
+vn 0.4307 -0.8553 0.2878
+vn 0.7728 0.3690 0.5163
+vn 0.3514 -0.9063 0.2348
+vn 0.8014 0.2666 0.5354
+vn 0.2680 -0.9466 0.1791
+vn 0.8206 0.1612 0.5483
+vn 0.1814 -0.9759 0.1212
+vn 0.8303 0.0540 0.5547
+vn 0.0927 0.9937 0.0619
+vn 0.0927 -0.9937 0.0619
+vn 0.4294 -0.7945 0.4294
+vn 0.3663 -0.8553 0.3663
+vn 0.6252 0.4670 0.6252
+vn 0.6572 0.3690 0.6572
+vn 0.2989 -0.9063 0.2989
+vn 0.6815 0.2666 0.6815
+vn 0.2279 -0.9466 0.2279
+vn 0.6978 0.1612 0.6978
+vn 0.1543 -0.9759 0.1543
+vn 0.7061 0.0540 0.7061
+vn 0.0788 0.9937 0.0788
+vn 0.0788 -0.9937 0.0788
+vn 0.7061 -0.0540 0.7061
+vn 0.1543 0.9759 0.1543
+vn 0.6978 -0.1612 0.6978
+vn 0.2279 0.9466 0.2279
+vn 0.6815 -0.2666 0.6815
+vn 0.2989 0.9063 0.2989
+vn 0.6572 -0.3690 0.6572
+vn 0.3663 0.8553 0.3663
+vn 0.6252 -0.4670 0.6252
+vn 0.4294 0.7945 0.4294
+vn 0.5860 -0.5596 0.5860
+vn 0.4875 0.7243 0.4875
+vn 0.5399 -0.6457 0.5399
+vn 0.5399 0.6457 0.5399
+vn 0.4875 -0.7243 0.4875
+vn 0.5860 0.5596 0.5860
+vn 0.2348 0.9063 0.3514
+vn 0.2878 0.8553 0.4307
+vn 0.5163 -0.3690 0.7728
+vn 0.4913 -0.4670 0.7352
+vn 0.3374 0.7945 0.5050
+vn 0.4604 -0.5596 0.6891
+vn 0.3830 0.7243 0.5733
+vn 0.4242 -0.6457 0.6349
+vn 0.4242 0.6457 0.6349
+vn 0.3830 -0.7243 0.5733
+vn 0.4604 0.5596 0.6891
+vn 0.3374 -0.7945 0.5050
+vn 0.4913 0.4670 0.7352
+vn 0.2878 -0.8553 0.4307
+vn 0.5163 0.3690 0.7728
+vn 0.2348 -0.9063 0.3514
+vn 0.5354 0.2666 0.8014
+vn 0.1791 -0.9466 0.2680
+vn 0.5483 0.1612 0.8206
+vn 0.1212 -0.9759 0.1814
+vn 0.5547 0.0540 0.8303
+vn 0.0619 0.9937 0.0927
+vn 0.0619 -0.9937 0.0927
+vn 0.5547 -0.0540 0.8303
+vn 0.1212 0.9759 0.1814
+vn 0.5483 -0.1612 0.8206
+vn 0.1791 0.9466 0.2680
+vn 0.5354 -0.2666 0.8014
+vn 0.3688 0.2666 0.8904
+vn 0.3777 0.1612 0.9118
+vn 0.1233 -0.9466 0.2978
+vn 0.0835 -0.9759 0.2016
+vn 0.3821 0.0540 0.9225
+vn 0.0426 0.9937 0.1030
+vn 0.0426 -0.9937 0.1030
+vn 0.3821 -0.0540 0.9225
+vn 0.0835 0.9759 0.2016
+vn 0.3777 -0.1612 0.9118
+vn 0.1233 0.9466 0.2978
+vn 0.3688 -0.2666 0.8904
+vn 0.1617 0.9063 0.3905
+vn 0.3557 -0.3690 0.8587
+vn 0.1982 0.8553 0.4786
+vn 0.3384 -0.4670 0.8169
+vn 0.2324 0.7945 0.5611
+vn 0.3171 -0.5596 0.7656
+vn 0.2638 0.7243 0.6370
+vn 0.2922 -0.6457 0.7054
+vn 0.2922 0.6457 0.7054
+vn 0.2638 -0.7243 0.6370
+vn 0.3171 0.5596 0.7656
+vn 0.2324 -0.7945 0.5611
+vn 0.3384 0.4670 0.8169
+vn 0.1982 -0.8553 0.4786
+vn 0.3557 0.3690 0.8587
+vn 0.1617 -0.9063 0.3905
+vn 0.1617 -0.5596 0.8128
+vn 0.1490 -0.6457 0.7489
+vn 0.1345 0.7243 0.6762
+vn 0.1490 0.6457 0.7489
+vn 0.1345 -0.7243 0.6762
+vn 0.1617 0.5596 0.8128
+vn 0.1185 -0.7945 0.5956
+vn 0.1725 0.4670 0.8672
+vn 0.1010 -0.8553 0.5081
+vn 0.1813 0.3690 0.9116
+vn 0.0824 -0.9063 0.4145
+vn 0.1880 0.2666 0.9452
+vn 0.0629 -0.9466 0.3161
+vn 0.1925 0.1612 0.9679
+vn 0.0425 -0.9759 0.2140
+vn 0.1948 0.0540 0.9793
+vn 0.0217 0.9937 0.1093
+vn 0.0217 -0.9937 0.1093
+vn 0.1948 -0.0540 0.9793
+vn 0.0425 0.9759 0.2140
+vn 0.1925 -0.1612 0.9679
+vn 0.0629 0.9466 0.3161
+vn 0.1880 -0.2666 0.9452
+vn 0.0824 0.9063 0.4145
+vn 0.1813 -0.3690 0.9116
+vn 0.1010 0.8553 0.5081
+vn 0.1725 -0.4670 0.8672
+vn 0.1185 0.7945 0.5956
+vn 0.0000 -0.9937 0.1115
+vn 0.0000 -0.0540 0.9985
+vn 0.0000 -0.1612 0.9869
+vn 0.0000 0.9759 0.2182
+vn 0.0000 0.9466 0.3223
+vn 0.0000 -0.2666 0.9638
+vn 0.0000 0.9063 0.4227
+vn 0.0000 -0.3690 0.9294
+vn 0.0000 0.8553 0.5180
+vn 0.0000 -0.4670 0.8842
+vn 0.0000 0.7945 0.6073
+vn 0.0000 -0.5596 0.8287
+vn 0.0000 0.7243 0.6894
+vn 0.0000 -0.6457 0.7635
+vn 0.0000 0.6457 0.7635
+vn 0.0000 -0.7243 0.6895
+vn 0.0000 0.5596 0.8287
+vn 0.0000 -0.7945 0.6073
+vn 0.0000 0.4670 0.8842
+vn 0.0000 -0.8553 0.5180
+vn 0.0000 0.3690 0.9294
+vn 0.0000 -0.9063 0.4227
+vn 0.0000 0.2666 0.9638
+vn 0.0000 -0.9466 0.3223
+vn 0.0000 0.1612 0.9869
+vn 0.0000 -0.9759 0.2182
+vn 0.0000 0.0540 0.9985
+vn 0.0000 0.9937 0.1115
+vn -0.1185 -0.7945 0.5956
+vn -0.1010 -0.8553 0.5081
+vn -0.1725 0.4670 0.8672
+vn -0.1813 0.3690 0.9116
+vn -0.0824 -0.9063 0.4145
+vn -0.1880 0.2666 0.9452
+vn -0.0629 -0.9466 0.3161
+vn -0.1925 0.1612 0.9679
+vn -0.0425 -0.9759 0.2140
+vn -0.1948 0.0540 0.9793
+vn -0.0217 0.9937 0.1093
+vn -0.0217 -0.9937 0.1093
+vn -0.1948 -0.0540 0.9793
+vn -0.0425 0.9759 0.2140
+vn -0.1925 -0.1612 0.9679
+vn -0.0629 0.9466 0.3161
+vn -0.1880 -0.2666 0.9452
+vn -0.0824 0.9063 0.4145
+vn -0.1813 -0.3690 0.9116
+vn -0.1010 0.8553 0.5081
+vn -0.1725 -0.4670 0.8672
+vn -0.1185 0.7945 0.5956
+vn -0.1617 -0.5596 0.8128
+vn -0.1345 0.7243 0.6762
+vn -0.1490 -0.6457 0.7489
+vn -0.1490 0.6457 0.7489
+vn -0.1345 -0.7243 0.6762
+vn -0.1617 0.5596 0.8128
+vn -0.1617 0.9063 0.3905
+vn -0.1982 0.8553 0.4786
+vn -0.3557 -0.3690 0.8587
+vn -0.3384 -0.4670 0.8169
+vn -0.2324 0.7945 0.5611
+vn -0.3171 -0.5596 0.7656
+vn -0.2638 0.7243 0.6370
+vn -0.2922 -0.6457 0.7054
+vn -0.2922 0.6457 0.7054
+vn -0.2638 -0.7243 0.6370
+vn -0.3171 0.5596 0.7656
+vn -0.2324 -0.7945 0.5611
+vn -0.3384 0.4670 0.8169
+vn -0.1982 -0.8553 0.4786
+vn -0.3557 0.3690 0.8587
+vn -0.1617 -0.9063 0.3905
+vn -0.3688 0.2666 0.8904
+vn -0.1233 -0.9466 0.2978
+vn -0.3777 0.1612 0.9118
+vn -0.0835 -0.9759 0.2016
+vn -0.3821 0.0540 0.9225
+vn -0.0426 0.9937 0.1030
+vn -0.0426 -0.9937 0.1030
+vn -0.3821 -0.0540 0.9225
+vn -0.0835 0.9759 0.2016
+vn -0.3777 -0.1612 0.9118
+vn -0.1233 0.9466 0.2978
+vn -0.3688 -0.2666 0.8904
+vn -0.5354 0.2666 0.8014
+vn -0.5483 0.1612 0.8206
+vn -0.1791 -0.9466 0.2680
+vn -0.1212 -0.9759 0.1814
+vn -0.5547 0.0540 0.8303
+vn -0.0619 0.9937 0.0927
+vn -0.0619 -0.9937 0.0927
+vn -0.5547 -0.0540 0.8303
+vn -0.1212 0.9759 0.1814
+vn -0.5483 -0.1612 0.8206
+vn -0.1791 0.9466 0.2680
+vn -0.5354 -0.2666 0.8014
+vn -0.2348 0.9063 0.3514
+vn -0.5163 -0.3690 0.7728
+vn -0.2878 0.8553 0.4307
+vn -0.4913 -0.4670 0.7352
+vn -0.3374 0.7945 0.5050
+vn -0.4604 -0.5596 0.6891
+vn -0.3830 0.7243 0.5733
+vn -0.4242 -0.6457 0.6349
+vn -0.4242 0.6457 0.6349
+vn -0.3830 -0.7243 0.5733
+vn -0.4604 0.5596 0.6891
+vn -0.3374 -0.7945 0.5049
+vn -0.4913 0.4670 0.7352
+vn -0.2878 -0.8553 0.4307
+vn -0.5163 0.3690 0.7728
+vn -0.2348 -0.9063 0.3514
+vn -0.5860 -0.5596 0.5860
+vn -0.5399 -0.6457 0.5399
+vn -0.4875 0.7243 0.4875
+vn -0.5399 0.6457 0.5399
+vn -0.4875 -0.7243 0.4875
+vn -0.5860 0.5596 0.5860
+vn -0.4294 -0.7945 0.4294
+vn -0.6252 0.4670 0.6252
+vn -0.3663 -0.8553 0.3663
+vn -0.6572 0.3690 0.6572
+vn -0.2989 -0.9063 0.2989
+vn -0.6815 0.2666 0.6815
+vn -0.2279 -0.9466 0.2279
+vn -0.6978 0.1612 0.6978
+vn -0.1543 -0.9759 0.1543
+vn -0.7061 0.0540 0.7061
+vn -0.0788 0.9937 0.0788
+vn -0.0788 -0.9937 0.0788
+vn -0.7061 -0.0540 0.7061
+vn -0.1543 0.9759 0.1543
+vn -0.6978 -0.1612 0.6978
+vn -0.2279 0.9466 0.2279
+vn -0.6815 -0.2666 0.6815
+vn -0.2989 0.9063 0.2989
+vn -0.6572 -0.3690 0.6572
+vn -0.3663 0.8553 0.3663
+vn -0.6252 -0.4670 0.6252
+vn -0.4294 0.7945 0.4294
+vn -0.0927 0.9937 0.0619
+vn -0.1814 0.9759 0.1212
+vn -0.8303 -0.0540 0.5547
+vn -0.8206 -0.1612 0.5483
+vn -0.1814 -0.9759 0.1212
+vn -0.0927 -0.9937 0.0619
+vn -0.2680 0.9466 0.1791
+vn -0.3514 0.9063 0.2348
+vn -0.4307 0.8553 0.2878
+vn -0.8014 -0.2666 0.5354
+vn -0.5050 0.7945 0.3374
+vn -0.7728 -0.3690 0.5163
+vn -0.5733 0.7243 0.3830
+vn -0.7352 -0.4670 0.4913
+vn -0.6349 0.6457 0.4242
+vn -0.6891 -0.5596 0.4604
+vn -0.6891 0.5596 0.4604
+vn -0.6349 -0.6457 0.4242
+vn -0.7352 0.4670 0.4913
+vn -0.5733 -0.7243 0.3830
+vn -0.7728 0.3690 0.5163
+vn -0.5050 -0.7945 0.3374
+vn -0.8014 0.2666 0.5354
+vn -0.4307 -0.8553 0.2878
+vn -0.8206 0.1612 0.5483
+vn -0.3514 -0.9063 0.2348
+vn -0.8303 0.0540 0.5547
+vn -0.2680 -0.9466 0.1791
+vn -0.7054 -0.6457 0.2922
+vn -0.6370 -0.7243 0.2638
+vn -0.7054 0.6457 0.2922
+vn -0.7656 0.5596 0.3171
+vn -0.5611 -0.7945 0.2324
+vn -0.8169 0.4670 0.3384
+vn -0.4786 -0.8553 0.1982
+vn -0.8587 0.3690 0.3557
+vn -0.3905 -0.9063 0.1617
+vn -0.8904 0.2666 0.3688
+vn -0.2978 -0.9466 0.1233
+vn -0.9118 0.1612 0.3777
+vn -0.2016 -0.9759 0.0835
+vn -0.9225 0.0540 0.3821
+vn -0.1030 0.9937 0.0426
+vn -0.1030 -0.9937 0.0426
+vn -0.9225 -0.0540 0.3821
+vn -0.2016 0.9759 0.0835
+vn -0.9118 -0.1612 0.3777
+vn -0.2978 0.9466 0.1233
+vn -0.8904 -0.2666 0.3688
+vn -0.3905 0.9063 0.1617
+vn -0.8587 -0.3690 0.3557
+vn -0.4786 0.8553 0.1982
+vn -0.8169 -0.4670 0.3384
+vn -0.5611 0.7945 0.2324
+vn -0.7656 -0.5596 0.3171
+vn -0.6370 0.7243 0.2638
+vn -0.9679 -0.1612 0.1925
+vn -0.9452 -0.2666 0.1880
+vn -0.3161 0.9466 0.0629
+vn -0.4145 0.9063 0.0824
+vn -0.9116 -0.3690 0.1813
+vn -0.5081 0.8553 0.1010
+vn -0.8672 -0.4670 0.1725
+vn -0.5956 0.7945 0.1185
+vn -0.8128 -0.5596 0.1617
+vn -0.6762 0.7243 0.1345
+vn -0.7489 -0.6457 0.1490
+vn -0.7489 0.6457 0.1490
+vn -0.6762 -0.7243 0.1345
+vn -0.8128 0.5596 0.1617
+vn -0.5956 -0.7945 0.1185
+vn -0.8672 0.4670 0.1725
+vn -0.5081 -0.8553 0.1010
+vn -0.9116 0.3690 0.1813
+vn -0.4145 -0.9063 0.0824
+vn -0.9452 0.2666 0.1880
+vn -0.3161 -0.9466 0.0629
+vn -0.9679 0.1612 0.1925
+vn -0.2140 -0.9759 0.0425
+vn -0.9793 0.0540 0.1948
+vn -0.1093 0.9937 0.0217
+vn -0.1093 -0.9937 0.0217
+vn -0.9793 -0.0540 0.1948
+vn -0.2140 0.9759 0.0425
+usemtl None
+s 1
+f 16/1/1 15/2/2 43/3/3 44/4/4
+f 3/5/5 2/6/6 30/7/7 31/8/8
+f 17/9/9 16/1/1 44/4/4 45/10/10
+f 4/11/11 3/5/5 31/8/8 32/12/12
+f 18/13/13 17/9/9 45/10/10 46/14/14
+f 5/15/15 4/11/11 32/12/12 33/16/16
+f 19/17/17 18/13/13 46/14/14 47/18/18
+f 6/19/19 5/15/15 33/16/16 34/20/20
+f 20/21/21 19/17/17 47/18/18 48/22/22
+f 7/23/23 6/19/19 34/20/20 35/24/24
+f 21/25/25 20/21/21 48/22/22 49/26/26
+f 8/27/27 7/23/23 35/24/24 36/28/28
+f 22/29/29 21/25/25 49/26/26 50/30/30
+f 9/31/31 8/27/27 36/28/28 37/32/32
+f 23/33/33 22/29/29 50/30/30 51/34/34
+f 10/35/35 9/31/31 37/32/32 38/36/36
+f 24/37/37 23/33/33 51/34/34 52/38/38
+f 11/39/39 10/35/35 38/36/36 39/40/40
+f 25/41/41 24/37/37 52/38/38 53/42/42
+f 12/43/43 11/39/39 39/40/40 40/44/44
+f 26/45/45 25/41/41 53/42/42 54/46/46
+f 13/47/47 12/43/43 40/44/44 41/48/48
+f 27/49/49 26/45/45 54/46/46 55/50/50
+f 14/51/51 13/47/47 41/48/48 42/52/52
+f 1/53/53 337/54/54 29/55/55
+f 28/56/56 27/49/49 55/50/50 56/57/57
+f 15/2/2 14/51/51 42/52/52 43/3/3
+f 2/6/6 1/58/53 29/59/55 30/7/7
+f 366/60/58 28/61/56 56/62/57
+f 42/52/52 41/48/48 69/63/59 70/64/60
+f 29/55/55 337/54/54 57/65/61
+f 56/57/57 55/50/50 83/66/62 84/67/63
+f 43/3/3 42/52/52 70/64/60 71/68/64
+f 30/7/7 29/59/55 57/69/61 58/70/65
+f 366/60/58 56/62/57 84/71/63
+f 44/4/4 43/3/3 71/68/64 72/72/66
+f 31/8/8 30/7/7 58/70/65 59/73/67
+f 45/10/10 44/4/4 72/72/66 73/74/68
+f 32/12/12 31/8/8 59/73/67 60/75/69
+f 46/14/14 45/10/10 73/74/68 74/76/70
+f 33/16/16 32/12/12 60/75/69 61/77/71
+f 47/18/18 46/14/14 74/76/70 75/78/72
+f 34/20/20 33/16/16 61/77/71 62/79/73
+f 48/22/22 47/18/18 75/78/72 76/80/74
+f 35/24/24 34/20/20 62/79/73 63/81/75
+f 49/26/26 48/22/22 76/80/74 77/82/76
+f 36/28/28 35/24/24 63/81/75 64/83/77
+f 50/30/30 49/26/26 77/82/76 78/84/78
+f 37/32/32 36/28/28 64/83/77 65/85/79
+f 51/34/34 50/30/30 78/84/78 79/86/80
+f 38/36/36 37/32/32 65/85/79 66/87/81
+f 52/38/38 51/34/34 79/86/80 80/88/82
+f 39/40/40 38/36/36 66/87/81 67/89/83
+f 53/42/42 52/38/38 80/88/82 81/90/84
+f 40/44/44 39/40/40 67/89/83 68/91/85
+f 54/46/46 53/42/42 81/90/84 82/92/86
+f 41/48/48 40/44/44 68/91/85 69/63/59
+f 55/50/50 54/46/46 82/92/86 83/66/62
+f 78/84/78 77/82/76 105/93/87 106/94/88
+f 65/85/79 64/83/77 92/95/89 93/96/90
+f 79/86/80 78/84/78 106/94/88 107/97/91
+f 66/87/81 65/85/79 93/96/90 94/98/92
+f 80/88/82 79/86/80 107/97/91 108/99/93
+f 67/89/83 66/87/81 94/98/92 95/100/94
+f 81/90/84 80/88/82 108/99/93 109/101/95
+f 68/91/85 67/89/83 95/100/94 96/102/96
+f 82/92/86 81/90/84 109/101/95 110/103/97
+f 69/63/59 68/91/85 96/102/96 97/104/98
+f 83/66/62 82/92/86 110/103/97 111/105/99
+f 70/64/60 69/63/59 97/104/98 98/106/100
+f 57/65/61 337/54/54 85/107/101
+f 84/67/63 83/66/62 111/105/99 112/108/102
+f 71/68/64 70/64/60 98/106/100 99/109/103
+f 58/70/65 57/69/61 85/110/101 86/111/104
+f 366/60/58 84/71/63 112/112/102
+f 72/72/66 71/68/64 99/109/103 100/113/105
+f 59/73/67 58/70/65 86/111/104 87/114/106
+f 73/74/68 72/72/66 100/113/105 101/115/107
+f 60/75/69 59/73/67 87/114/106 88/116/108
+f 74/76/70 73/74/68 101/115/107 102/117/109
+f 61/77/71 60/75/69 88/116/108 89/118/110
+f 75/78/72 74/76/70 102/117/109 103/119/111
+f 62/79/73 61/77/71 89/118/110 90/120/112
+f 76/80/74 75/78/72 103/119/111 104/121/113
+f 63/81/75 62/79/73 90/120/112 91/122/114
+f 77/82/76 76/80/74 104/121/113 105/93/87
+f 64/83/77 63/81/75 91/122/114 92/95/89
+f 87/114/106 86/111/104 114/123/115 115/124/116
+f 101/115/107 100/113/105 128/125/117 129/126/118
+f 88/116/108 87/114/106 115/124/116 116/127/119
+f 102/117/109 101/115/107 129/126/118 130/128/120
+f 89/118/110 88/116/108 116/127/119 117/129/121
+f 103/119/111 102/117/109 130/128/120 131/130/122
+f 90/120/112 89/118/110 117/129/121 118/131/123
+f 104/121/113 103/119/111 131/130/122 132/132/124
+f 91/122/114 90/120/112 118/131/123 119/133/125
+f 105/93/87 104/121/113 132/132/124 133/134/126
+f 92/95/89 91/122/114 119/133/125 120/135/127
+f 106/94/88 105/93/87 133/134/126 134/136/128
+f 93/96/90 92/95/89 120/135/127 121/137/129
+f 107/97/91 106/94/88 134/136/128 135/138/130
+f 94/98/92 93/96/90 121/137/129 122/139/131
+f 108/99/93 107/97/91 135/138/130 136/140/132
+f 95/100/94 94/98/92 122/139/131 123/141/133
+f 109/101/95 108/99/93 136/140/132 137/142/134
+f 96/102/96 95/100/94 123/141/133 124/143/135
+f 110/103/97 109/101/95 137/142/134 138/144/136
+f 97/104/98 96/102/96 124/143/135 125/145/137
+f 111/105/99 110/103/97 138/144/136 139/146/138
+f 98/106/100 97/104/98 125/145/137 126/147/139
+f 85/107/101 337/54/54 113/148/140
+f 112/108/102 111/105/99 139/146/138 140/149/141
+f 99/109/103 98/106/100 126/147/139 127/150/142
+f 86/111/104 85/110/101 113/151/140 114/123/115
+f 366/60/58 112/112/102 140/152/141
+f 100/113/105 99/109/103 127/150/142 128/125/117
+f 123/141/133 122/139/131 150/153/143 151/154/144
+f 137/142/134 136/140/132 164/155/145 165/156/146
+f 124/143/135 123/141/133 151/154/144 152/157/147
+f 138/144/136 137/142/134 165/156/146 166/158/148
+f 125/145/137 124/143/135 152/157/147 153/159/149
+f 139/146/138 138/144/136 166/158/148 167/160/150
+f 126/147/139 125/145/137 153/159/149 154/161/151
+f 113/148/140 337/54/54 141/162/152
+f 140/149/141 139/146/138 167/160/150 168/163/153
+f 127/150/142 126/147/139 154/161/151 155/164/154
+f 114/123/115 113/151/140 141/165/152 142/166/155
+f 366/60/58 140/152/141 168/167/153
+f 128/125/117 127/150/142 155/164/154 156/168/156
+f 115/124/116 114/123/115 142/166/155 143/169/157
+f 129/126/118 128/125/117 156/168/156 157/170/158
+f 116/127/119 115/124/116 143/169/157 144/171/159
+f 130/128/120 129/126/118 157/170/158 158/172/160
+f 117/129/121 116/127/119 144/171/159 145/173/161
+f 131/130/122 130/128/120 158/172/160 159/174/162
+f 118/131/123 117/129/121 145/173/161 146/175/163
+f 132/132/124 131/130/122 159/174/162 160/176/164
+f 119/133/125 118/131/123 146/175/163 147/177/165
+f 133/134/126 132/132/124 160/176/164 161/178/166
+f 120/135/127 119/133/125 147/177/165 148/179/167
+f 134/136/128 133/134/126 161/178/166 162/180/168
+f 121/137/129 120/135/127 148/179/167 149/181/169
+f 135/138/130 134/136/128 162/180/168 163/182/170
+f 122/139/131 121/137/129 149/181/169 150/153/143
+f 136/140/132 135/138/130 163/182/170 164/155/145
+f 159/174/162 158/172/160 186/183/171 187/184/172
+f 146/175/163 145/173/161 173/185/173 174/186/174
+f 160/176/164 159/174/162 187/184/172 188/187/175
+f 147/177/165 146/175/163 174/186/174 175/188/176
+f 161/178/166 160/176/164 188/187/175 189/189/177
+f 148/179/167 147/177/165 175/188/176 176/190/178
+f 162/180/168 161/178/166 189/189/177 190/191/179
+f 149/181/169 148/179/167 176/190/178 177/192/180
+f 163/182/170 162/180/168 190/191/179 191/193/181
+f 150/153/143 149/181/169 177/192/180 178/194/182
+f 164/155/145 163/182/170 191/193/181 192/195/183
+f 151/154/144 150/153/143 178/194/182 179/196/184
+f 165/156/146 164/155/145 192/195/183 193/197/185
+f 152/157/147 151/154/144 179/196/184 180/198/186
+f 166/158/148 165/156/146 193/197/185 194/199/187
+f 153/159/149 152/157/147 180/198/186 181/200/188
+f 167/160/150 166/158/148 194/199/187 195/201/189
+f 154/161/151 153/159/149 181/200/188 182/202/190
+f 141/162/152 337/54/54 169/203/191
+f 168/163/153 167/160/150 195/201/189 196/204/192
+f 155/164/154 154/161/151 182/202/190 183/205/193
+f 142/166/155 141/165/152 169/206/191 170/207/194
+f 366/60/58 168/167/153 196/208/192
+f 156/168/156 155/164/154 183/205/193 184/209/195
+f 143/169/157 142/166/155 170/207/194 171/210/196
+f 157/170/158 156/168/156 184/209/195 185/211/197
+f 144/171/159 143/169/157 171/210/196 172/212/198
+f 158/172/160 157/170/158 185/211/197 186/183/171
+f 145/173/161 144/171/159 172/212/198 173/185/173
+f 195/201/189 194/199/187 222/213/199 223/214/200
+f 182/202/190 181/200/188 209/215/201 210/216/202
+f 169/203/191 337/54/54 197/217/203
+f 196/204/192 195/201/189 223/214/200 224/218/204
+f 183/205/193 182/202/190 210/216/202 211/219/205
+f 170/207/194 169/206/191 197/220/203 198/221/206
+f 366/60/58 196/208/192 224/222/204
+f 184/209/195 183/205/193 211/219/205 212/223/207
+f 171/210/196 170/207/194 198/221/206 199/224/208
+f 185/211/197 184/209/195 212/223/207 213/225/209
+f 172/212/198 171/210/196 199/224/208 200/226/210
+f 186/183/171 185/211/197 213/225/209 214/227/211
+f 173/185/173 172/212/198 200/226/210 201/228/212
+f 187/184/172 186/183/171 214/227/211 215/229/213
+f 174/186/174 173/185/173 201/228/212 202/230/214
+f 188/187/175 187/184/172 215/229/213 216/231/215
+f 175/188/176 174/186/174 202/230/214 203/232/216
+f 189/189/177 188/187/175 216/231/215 217/233/217
+f 176/190/178 175/188/176 203/232/216 204/234/218
+f 190/191/179 189/189/177 217/233/217 218/235/219
+f 177/192/180 176/190/178 204/234/218 205/236/220
+f 191/193/181 190/191/179 218/235/219 219/237/221
+f 178/194/182 177/192/180 205/236/220 206/238/222
+f 192/195/183 191/193/181 219/237/221 220/239/223
+f 179/196/184 178/194/182 206/238/222 207/240/224
+f 193/197/185 192/195/183 220/239/223 221/241/225
+f 180/198/186 179/196/184 207/240/224 208/242/226
+f 194/199/187 193/197/185 221/241/225 222/213/199
+f 181/200/188 180/198/186 208/242/226 209/215/201
+f 218/235/219 217/233/217 245/243/227 246/244/228
+f 205/236/220 204/234/218 232/245/229 233/246/230
+f 219/237/221 218/235/219 246/244/228 247/247/231
+f 206/238/222 205/236/220 233/246/230 234/248/232
+f 220/239/223 219/237/221 247/247/231 248/249/233
+f 207/240/224 206/238/222 234/248/232 235/250/234
+f 221/241/225 220/239/223 248/249/233 249/251/235
+f 208/242/226 207/240/224 235/250/234 236/252/236
+f 222/213/199 221/241/225 249/251/235 250/253/237
+f 209/215/201 208/242/226 236/252/236 237/254/238
+f 223/214/200 222/213/199 250/253/237 251/255/239
+f 210/216/202 209/215/201 237/254/238 238/256/240
+f 197/217/203 337/54/54 225/257/241
+f 224/218/204 223/214/200 251/255/239 252/258/242
+f 211/219/205 210/216/202 238/256/240 239/259/243
+f 198/221/206 197/220/203 225/260/241 226/261/244
+f 366/60/58 224/222/204 252/262/242
+f 212/223/207 211/219/205 239/259/243 240/263/245
+f 199/224/208 198/221/206 226/261/244 227/264/246
+f 213/225/209 212/223/207 240/263/245 241/265/247
+f 200/226/210 199/224/208 227/264/246 228/266/248
+f 214/227/211 213/225/209 241/265/247 242/267/249
+f 201/228/212 200/226/210 228/266/248 229/268/250
+f 215/229/213 214/227/211 242/267/249 243/269/251
+f 202/230/214 201/228/212 229/268/250 230/270/252
+f 216/231/215 215/229/213 243/269/251 244/271/253
+f 203/232/216 202/230/214 230/270/252 231/272/254
+f 217/233/217 216/231/215 244/271/253 245/243/227
+f 204/234/218 203/232/216 231/272/254 232/245/229
+f 227/264/246 226/261/244 254/273/255 255/274/256
+f 241/265/247 240/263/245 268/275/257 269/276/258
+f 228/266/248 227/264/246 255/274/256 256/277/259
+f 242/267/249 241/265/247 269/276/258 270/278/260
+f 229/268/250 228/266/248 256/277/259 257/279/261
+f 243/269/251 242/267/249 270/278/260 271/280/262
+f 230/270/252 229/268/250 257/279/261 258/281/263
+f 244/271/253 243/269/251 271/280/262 272/282/264
+f 231/272/254 230/270/252 258/281/263 259/283/265
+f 245/243/227 244/271/253 272/282/264 273/284/266
+f 232/245/229 231/272/254 259/283/265 260/285/267
+f 246/244/228 245/243/227 273/284/266 274/286/268
+f 233/246/230 232/245/229 260/285/267 261/287/269
+f 247/247/231 246/244/228 274/286/268 275/288/270
+f 234/248/232 233/246/230 261/287/269 262/289/271
+f 248/249/233 247/247/231 275/288/270 276/290/272
+f 235/250/234 234/248/232 262/289/271 263/291/273
+f 249/251/235 248/249/233 276/290/272 277/292/274
+f 236/252/236 235/250/234 263/291/273 264/293/275
+f 250/253/237 249/251/235 277/292/274 278/294/276
+f 237/254/238 236/252/236 264/293/275 265/295/277
+f 251/255/239 250/253/237 278/294/276 279/296/278
+f 238/256/240 237/254/238 265/295/277 266/297/279
+f 225/257/241 337/54/54 253/298/280
+f 252/258/242 251/255/239 279/296/278 280/299/281
+f 239/259/243 238/256/240 266/297/279 267/300/282
+f 226/261/244 225/260/241 253/301/280 254/273/255
+f 366/60/58 252/262/242 280/302/281
+f 240/263/245 239/259/243 267/300/282 268/275/257
+f 263/291/273 262/289/271 290/303/283 291/304/284
+f 277/292/274 276/290/272 304/305/285 305/306/286
+f 264/293/275 263/291/273 291/304/284 292/307/287
+f 278/294/276 277/292/274 305/306/286 306/308/288
+f 265/295/277 264/293/275 292/307/287 293/309/289
+f 279/296/278 278/294/276 306/308/288 307/310/290
+f 266/297/279 265/295/277 293/309/289 294/311/291
+f 253/298/280 337/54/54 281/312/292
+f 280/299/281 279/296/278 307/310/290 308/313/293
+f 267/300/282 266/297/279 294/311/291 295/314/294
+f 254/273/255 253/301/280 281/315/292 282/316/295
+f 366/60/58 280/302/281 308/317/293
+f 268/275/257 267/300/282 295/314/294 296/318/296
+f 255/274/256 254/273/255 282/316/295 283/319/297
+f 269/276/258 268/275/257 296/318/296 297/320/298
+f 256/277/259 255/274/256 283/319/297 284/321/299
+f 270/278/260 269/276/258 297/320/298 298/322/300
+f 257/279/261 256/277/259 284/321/299 285/323/301
+f 271/280/262 270/278/260 298/322/300 299/324/302
+f 258/281/263 257/279/261 285/323/301 286/325/303
+f 272/282/264 271/280/262 299/324/302 300/326/304
+f 259/283/265 258/281/263 286/325/303 287/327/305
+f 273/284/266 272/282/264 300/326/304 301/328/306
+f 260/285/267 259/283/265 287/327/305 288/329/307
+f 274/286/268 273/284/266 301/328/306 302/330/308
+f 261/287/269 260/285/267 288/329/307 289/331/309
+f 275/288/270 274/286/268 302/330/308 303/332/310
+f 262/289/271 261/287/269 289/331/309 290/303/283
+f 276/290/272 275/288/270 303/332/310 304/305/285
+f 299/324/302 298/322/300 326/333/311 327/334/312
+f 286/325/303 285/323/301 313/335/313 314/336/314
+f 300/326/304 299/324/302 327/334/312 328/337/315
+f 287/327/305 286/325/303 314/336/314 315/338/316
+f 301/328/306 300/326/304 328/337/315 329/339/317
+f 288/329/307 287/327/305 315/338/316 316/340/318
+f 302/330/308 301/328/306 329/339/317 330/341/319
+f 289/331/309 288/329/307 316/340/318 317/342/320
+f 303/332/310 302/330/308 330/341/319 331/343/321
+f 290/303/283 289/331/309 317/342/320 318/344/322
+f 304/305/285 303/332/310 331/343/321 332/345/323
+f 291/304/284 290/303/283 318/344/322 319/346/324
+f 305/306/286 304/305/285 332/345/323 333/347/325
+f 292/307/287 291/304/284 319/346/324 320/348/326
+f 306/308/288 305/306/286 333/347/325 334/349/327
+f 293/309/289 292/307/287 320/348/326 321/350/328
+f 307/310/290 306/308/288 334/349/327 335/351/329
+f 294/311/291 293/309/289 321/350/328 322/352/330
+f 281/312/292 337/54/54 309/353/331
+f 308/313/293 307/310/290 335/351/329 336/354/332
+f 295/314/294 294/311/291 322/352/330 323/355/333
+f 282/316/295 281/315/292 309/356/331 310/357/334
+f 366/60/58 308/317/293 336/358/332
+f 296/318/296 295/314/294 323/355/333 324/359/335
+f 283/319/297 282/316/295 310/357/334 311/360/336
+f 297/320/298 296/318/296 324/359/335 325/361/337
+f 284/321/299 283/319/297 311/360/336 312/362/338
+f 298/322/300 297/320/298 325/361/337 326/333/311
+f 285/323/301 284/321/299 312/362/338 313/335/313
+f 335/351/329 334/349/327 363/363/339 364/364/340
+f 322/352/330 321/350/328 350/365/341 351/366/342
+f 309/353/331 337/54/54 338/367/343
+f 336/354/332 335/351/329 364/364/340 365/368/344
+f 323/355/333 322/352/330 351/366/342 352/369/345
+f 310/357/334 309/356/331 338/370/343 339/371/346
+f 366/60/58 336/358/332 365/372/344
+f 324/359/335 323/355/333 352/369/345 353/373/347
+f 311/360/336 310/357/334 339/371/346 340/374/348
+f 325/361/337 324/359/335 353/373/347 354/375/349
+f 312/362/338 311/360/336 340/374/348 341/376/350
+f 326/333/311 325/361/337 354/375/349 355/377/351
+f 313/335/313 312/362/338 341/376/350 342/378/352
+f 327/334/312 326/333/311 355/377/351 356/379/353
+f 314/336/314 313/335/313 342/378/352 343/380/354
+f 328/337/315 327/334/312 356/379/353 357/381/355
+f 315/338/316 314/336/314 343/380/354 344/382/356
+f 329/339/317 328/337/315 357/381/355 358/383/357
+f 316/340/318 315/338/316 344/382/356 345/384/358
+f 330/341/319 329/339/317 358/383/357 359/385/359
+f 317/342/320 316/340/318 345/384/358 346/386/360
+f 331/343/321 330/341/319 359/385/359 360/387/361
+f 318/344/322 317/342/320 346/386/360 347/388/362
+f 332/345/323 331/343/321 360/387/361 361/389/363
+f 319/346/324 318/344/322 347/388/362 348/390/364
+f 333/347/325 332/345/323 361/389/363 362/391/365
+f 320/348/326 319/346/324 348/390/364 349/392/366
+f 334/349/327 333/347/325 362/391/365 363/363/339
+f 321/350/328 320/348/326 349/392/366 350/365/341
+f 345/384/358 344/382/356 373/393/367 374/394/368
+f 359/385/359 358/383/357 387/395/369 388/396/370
+f 346/386/360 345/384/358 374/394/368 375/397/371
+f 360/387/361 359/385/359 388/396/370 389/398/372
+f 347/388/362 346/386/360 375/397/371 376/399/373
+f 361/389/363 360/387/361 389/398/372 390/400/374
+f 348/390/364 347/388/362 376/399/373 377/401/375
+f 362/391/365 361/389/363 390/400/374 391/402/376
+f 349/392/366 348/390/364 377/401/375 378/403/377
+f 363/363/339 362/391/365 391/402/376 392/404/378
+f 350/365/341 349/392/366 378/403/377 379/405/379
+f 364/364/340 363/363/339 392/404/378 393/406/380
+f 351/366/342 350/365/341 379/405/379 380/407/381
+f 338/367/343 337/54/54 367/408/382
+f 365/368/344 364/364/340 393/406/380 394/409/383
+f 352/369/345 351/366/342 380/407/381 381/410/384
+f 339/371/346 338/370/343 367/411/382 368/412/385
+f 366/60/58 365/372/344 394/413/383
+f 353/373/347 352/369/345 381/410/384 382/414/386
+f 340/374/348 339/371/346 368/412/385 369/415/387
+f 354/375/349 353/373/347 382/414/386 383/416/388
+f 341/376/350 340/374/348 369/415/387 370/417/389
+f 355/377/351 354/375/349 383/416/388 384/418/390
+f 342/378/352 341/376/350 370/417/389 371/419/391
+f 356/379/353 355/377/351 384/418/390 385/420/392
+f 343/380/354 342/378/352 371/419/391 372/421/393
+f 357/381/355 356/379/353 385/420/392 386/422/394
+f 344/382/356 343/380/354 372/421/393 373/393/367
+f 358/383/357 357/381/355 386/422/394 387/395/369
+f 382/414/386 381/410/384 409/423/395 410/424/396
+f 369/415/387 368/412/385 396/425/397 397/426/398
+f 383/416/388 382/414/386 410/424/396 411/427/399
+f 370/417/389 369/415/387 397/426/398 398/428/400
+f 384/418/390 383/416/388 411/427/399 412/429/401
+f 371/419/391 370/417/389 398/428/400 399/430/402
+f 385/420/392 384/418/390 412/429/401 413/431/403
+f 372/421/393 371/419/391 399/430/402 400/432/404
+f 386/422/394 385/420/392 413/431/403 414/433/405
+f 373/393/367 372/421/393 400/432/404 401/434/406
+f 387/395/369 386/422/394 414/433/405 415/435/407
+f 374/394/368 373/393/367 401/434/406 402/436/408
+f 388/396/370 387/395/369 415/435/407 416/437/409
+f 375/397/371 374/394/368 402/436/408 403/438/410
+f 389/398/372 388/396/370 416/437/409 417/439/411
+f 376/399/373 375/397/371 403/438/410 404/440/412
+f 390/400/374 389/398/372 417/439/411 418/441/413
+f 377/401/375 376/399/373 404/440/412 405/442/414
+f 391/402/376 390/400/374 418/441/413 419/443/415
+f 378/403/377 377/401/375 405/442/414 406/444/416
+f 392/404/378 391/402/376 419/443/415 420/445/417
+f 379/405/379 378/403/377 406/444/416 407/446/418
+f 393/406/380 392/404/378 420/445/417 421/447/419
+f 380/407/381 379/405/379 407/446/418 408/448/420
+f 367/408/382 337/54/54 395/449/421
+f 394/409/383 393/406/380 421/447/419 422/450/422
+f 381/410/384 380/407/381 408/448/420 409/423/395
+f 368/412/385 367/411/382 395/451/421 396/425/397
+f 366/60/58 394/413/383 422/452/422
+f 418/441/413 417/439/411 445/453/423 446/454/424
+f 405/442/414 404/440/412 432/455/425 433/456/426
+f 419/443/415 418/441/413 446/454/424 447/457/427
+f 406/444/416 405/442/414 433/456/426 434/458/428
+f 420/445/417 419/443/415 447/457/427 448/459/429
+f 407/446/418 406/444/416 434/458/428 435/460/430
+f 421/447/419 420/445/417 448/459/429 449/461/431
+f 408/448/420 407/446/418 435/460/430 436/462/432
+f 395/449/421 337/54/54 423/463/433
+f 422/450/422 421/447/419 449/461/431 450/464/434
+f 409/423/395 408/448/420 436/462/432 437/465/435
+f 396/425/397 395/451/421 423/466/433 424/467/436
+f 366/60/58 422/452/422 450/468/434
+f 410/424/396 409/423/395 437/465/435 438/469/437
+f 397/426/398 396/425/397 424/467/436 425/470/438
+f 411/427/399 410/424/396 438/469/437 439/471/439
+f 398/428/400 397/426/398 425/470/438 426/472/440
+f 412/429/401 411/427/399 439/471/439 440/473/441
+f 399/430/402 398/428/400 426/472/440 427/474/442
+f 413/431/403 412/429/401 440/473/441 441/475/443
+f 400/432/404 399/430/402 427/474/442 428/476/444
+f 414/433/405 413/431/403 441/475/443 442/477/445
+f 401/434/406 400/432/404 428/476/444 429/478/446
+f 415/435/407 414/433/405 442/477/445 443/479/447
+f 402/436/408 401/434/406 429/478/446 430/480/448
+f 416/437/409 415/435/407 443/479/447 444/481/449
+f 403/438/410 402/436/408 430/480/448 431/482/450
+f 417/439/411 416/437/409 444/481/449 445/453/423
+f 404/440/412 403/438/410 431/482/450 432/455/425
+f 441/475/443 440/473/441 468/483/451 469/484/452
+f 428/476/444 427/474/442 455/485/453 456/486/454
+f 442/477/445 441/475/443 469/484/452 470/487/455
+f 429/478/446 428/476/444 456/486/454 457/488/456
+f 443/479/447 442/477/445 470/487/455 471/489/457
+f 430/480/448 429/478/446 457/488/456 458/490/458
+f 444/481/449 443/479/447 471/489/457 472/491/459
+f 431/482/450 430/480/448 458/490/458 459/492/460
+f 445/453/423 444/481/449 472/491/459 473/493/461
+f 432/455/425 431/482/450 459/492/460 460/494/462
+f 446/454/424 445/453/423 473/493/461 474/495/463
+f 433/456/426 432/455/425 460/494/462 461/496/464
+f 447/457/427 446/454/424 474/495/463 475/497/465
+f 434/458/428 433/456/426 461/496/464 462/498/466
+f 448/459/429 447/457/427 475/497/465 476/499/467
+f 435/460/430 434/458/428 462/498/466 463/500/468
+f 449/461/431 448/459/429 476/499/467 477/501/469
+f 436/462/432 435/460/430 463/500/468 464/502/470
+f 423/463/433 337/503/54 451/504/471
+f 450/464/434 449/461/431 477/501/469 478/505/472
+f 437/465/435 436/462/432 464/502/470 465/506/473
+f 424/467/436 423/466/433 451/507/471 452/508/474
+f 366/60/58 450/468/434 478/509/472
+f 438/469/437 437/465/435 465/506/473 466/510/475
+f 425/470/438 424/467/436 452/508/474 453/511/476
+f 439/471/439 438/469/437 466/510/475 467/512/477
+f 426/472/440 425/470/438 453/511/476 454/513/478
+f 440/473/441 439/471/439 467/512/477 468/483/451
+f 427/474/442 426/472/440 454/513/478 455/485/453
+f 477/501/469 476/499/467 504/514/479 505/515/480
+f 464/502/470 463/500/468 491/516/481 492/517/482
+f 451/504/471 337/503/54 479/518/483
+f 478/505/472 477/501/469 505/515/480 506/519/484
+f 465/506/473 464/502/470 492/517/482 493/520/485
+f 452/508/474 451/507/471 479/521/483 480/522/486
+f 366/523/58 478/509/472 506/524/484
+f 466/510/475 465/506/473 493/520/485 494/525/487
+f 453/511/476 452/508/474 480/522/486 481/526/488
+f 467/512/477 466/510/475 494/525/487 495/527/489
+f 454/513/478 453/511/476 481/526/488 482/528/490
+f 468/483/451 467/512/477 495/527/489 496/529/491
+f 455/485/453 454/513/478 482/528/490 483/530/492
+f 469/484/452 468/483/451 496/529/491 497/531/493
+f 456/486/454 455/485/453 483/530/492 484/532/494
+f 470/487/455 469/484/452 497/531/493 498/533/495
+f 457/488/456 456/486/454 484/532/494 485/534/496
+f 471/489/457 470/487/455 498/533/495 499/535/497
+f 458/490/458 457/488/456 485/534/496 486/536/498
+f 472/491/459 471/489/457 499/535/497 500/537/499
+f 459/492/460 458/490/458 486/536/498 487/538/500
+f 473/493/461 472/491/459 500/537/499 501/539/501
+f 460/494/462 459/492/460 487/538/500 488/540/502
+f 474/495/463 473/493/461 501/539/501 502/541/503
+f 461/496/464 460/494/462 488/540/502 489/542/504
+f 475/497/465 474/495/463 502/541/503 503/543/505
+f 462/498/466 461/496/464 489/542/504 490/544/506
+f 476/499/467 475/497/465 503/543/505 504/514/479
+f 463/500/468 462/498/466 490/544/506 491/516/481
+f 486/536/498 485/534/496 513/545/507 514/546/508
+f 500/537/499 499/535/497 527/547/509 528/548/510
+f 487/538/500 486/536/498 514/546/508 515/549/511
+f 501/539/501 500/537/499 528/548/510 529/550/512
+f 488/540/502 487/538/500 515/549/511 516/551/513
+f 502/541/503 501/539/501 529/550/512 530/552/514
+f 489/542/504 488/540/502 516/551/513 517/553/515
+f 503/543/505 502/541/503 530/552/514 531/554/516
+f 490/544/506 489/542/504 517/553/515 518/555/517
+f 504/514/479 503/543/505 531/554/516 532/556/518
+f 491/516/481 490/544/506 518/555/517 519/557/519
+f 505/515/480 504/514/479 532/556/518 533/558/520
+f 492/517/482 491/516/481 519/557/519 520/559/521
+f 479/518/483 337/503/54 507/560/522
+f 506/519/484 505/515/480 533/558/520 534/561/523
+f 493/520/485 492/517/482 520/559/521 521/562/524
+f 480/522/486 479/521/483 507/563/522 508/564/525
+f 366/523/58 506/524/484 534/565/523
+f 494/525/487 493/520/485 521/562/524 522/566/526
+f 481/526/488 480/522/486 508/564/525 509/567/527
+f 495/527/489 494/525/487 522/566/526 523/568/528
+f 482/528/490 481/526/488 509/567/527 510/569/529
+f 496/529/491 495/527/489 523/568/528 524/570/530
+f 483/530/492 482/528/490 510/569/529 511/571/531
+f 497/531/493 496/529/491 524/570/530 525/572/532
+f 484/532/494 483/530/492 511/571/531 512/573/533
+f 498/533/495 497/531/493 525/572/532 526/574/534
+f 485/534/496 484/532/494 512/573/533 513/545/507
+f 499/535/497 498/533/495 526/574/534 527/547/509
+f 522/566/526 521/562/524 549/575/535 550/576/536
+f 509/567/527 508/564/525 536/577/537 537/578/538
+f 523/568/528 522/566/526 550/576/536 551/579/539
+f 510/569/529 509/567/527 537/578/538 538/580/540
+f 524/570/530 523/568/528 551/579/539 552/581/541
+f 511/571/531 510/569/529 538/580/540 539/582/542
+f 525/572/532 524/570/530 552/581/541 553/583/543
+f 512/573/533 511/571/531 539/582/542 540/584/544
+f 526/574/534 525/572/532 553/583/543 554/585/545
+f 513/545/507 512/573/533 540/584/544 541/586/546
+f 527/547/509 526/574/534 554/585/545 555/587/547
+f 514/546/508 513/545/507 541/586/546 542/588/548
+f 528/548/510 527/547/509 555/587/547 556/589/549
+f 515/549/511 514/546/508 542/588/548 543/590/550
+f 529/550/512 528/548/510 556/589/549 557/591/551
+f 516/551/513 515/549/511 543/590/550 544/592/552
+f 530/552/514 529/550/512 557/591/551 558/593/553
+f 517/553/515 516/551/513 544/592/552 545/594/554
+f 531/554/516 530/552/514 558/593/553 559/595/555
+f 518/555/517 517/553/515 545/594/554 546/596/556
+f 532/556/518 531/554/516 559/595/555 560/597/557
+f 519/557/519 518/555/517 546/596/556 547/598/558
+f 533/558/520 532/556/518 560/597/557 561/599/559
+f 520/559/521 519/557/519 547/598/558 548/600/560
+f 507/560/522 337/503/54 535/601/561
+f 534/561/523 533/558/520 561/599/559 562/602/562
+f 521/562/524 520/559/521 548/600/560 549/575/535
+f 508/564/525 507/563/522 535/603/561 536/577/537
+f 366/523/58 534/565/523 562/604/562
+f 558/593/553 557/591/551 585/605/563 586/606/564
+f 545/594/554 544/592/552 572/607/565 573/608/566
+f 559/595/555 558/593/553 586/606/564 587/609/567
+f 546/596/556 545/594/554 573/608/566 574/610/568
+f 560/597/557 559/595/555 587/609/567 588/611/569
+f 547/598/558 546/596/556 574/610/568 575/612/570
+f 561/599/559 560/597/557 588/611/569 589/613/571
+f 548/600/560 547/598/558 575/612/570 576/614/572
+f 535/601/561 337/503/54 563/615/573
+f 562/602/562 561/599/559 589/613/571 590/616/574
+f 549/575/535 548/600/560 576/614/572 577/617/575
+f 536/577/537 535/603/561 563/618/573 564/619/576
+f 366/523/58 562/604/562 590/620/574
+f 550/576/536 549/575/535 577/617/575 578/621/577
+f 537/578/538 536/577/537 564/619/576 565/622/578
+f 551/579/539 550/576/536 578/621/577 579/623/579
+f 538/580/540 537/578/538 565/622/578 566/624/580
+f 552/581/541 551/579/539 579/623/579 580/625/581
+f 539/582/542 538/580/540 566/624/580 567/626/582
+f 553/583/543 552/581/541 580/625/581 581/627/583
+f 540/584/544 539/582/542 567/626/582 568/628/584
+f 554/585/545 553/583/543 581/627/583 582/629/585
+f 541/586/546 540/584/544 568/628/584 569/630/586
+f 555/587/547 554/585/545 582/629/585 583/631/587
+f 542/588/548 541/586/546 569/630/586 570/632/588
+f 556/589/549 555/587/547 583/631/587 584/633/589
+f 543/590/550 542/588/548 570/632/588 571/634/590
+f 557/591/551 556/589/549 584/633/589 585/605/563
+f 544/592/552 543/590/550 571/634/590 572/607/565
+f 567/626/582 566/624/580 594/635/591 595/636/592
+f 581/627/583 580/625/581 608/637/593 609/638/594
+f 568/628/584 567/626/582 595/636/592 596/639/595
+f 582/629/585 581/627/583 609/638/594 610/640/596
+f 569/630/586 568/628/584 596/639/595 597/641/597
+f 583/631/587 582/629/585 610/640/596 611/642/598
+f 570/632/588 569/630/586 597/641/597 598/643/599
+f 584/633/589 583/631/587 611/642/598 612/644/600
+f 571/634/590 570/632/588 598/643/599 599/645/601
+f 585/605/563 584/633/589 612/644/600 613/646/602
+f 572/607/565 571/634/590 599/645/601 600/647/603
+f 586/606/564 585/605/563 613/646/602 614/648/604
+f 573/608/566 572/607/565 600/647/603 601/649/605
+f 587/609/567 586/606/564 614/648/604 615/650/606
+f 574/610/568 573/608/566 601/649/605 602/651/607
+f 588/611/569 587/609/567 615/650/606 616/652/608
+f 575/612/570 574/610/568 602/651/607 603/653/609
+f 589/613/571 588/611/569 616/652/608 617/654/610
+f 576/614/572 575/612/570 603/653/609 604/655/611
+f 563/615/573 337/503/54 591/656/612
+f 590/616/574 589/613/571 617/654/610 618/657/613
+f 577/617/575 576/614/572 604/655/611 605/658/614
+f 564/619/576 563/618/573 591/659/612 592/660/615
+f 366/523/58 590/620/574 618/661/613
+f 578/621/577 577/617/575 605/658/614 606/662/616
+f 565/622/578 564/619/576 592/660/615 593/663/617
+f 579/623/579 578/621/577 606/662/616 607/664/618
+f 566/624/580 565/622/578 593/663/617 594/635/591
+f 580/625/581 579/623/579 607/664/618 608/637/593
+f 603/653/609 602/651/607 630/665/619 631/666/620
+f 617/654/610 616/652/608 644/667/621 645/668/622
+f 604/655/611 603/653/609 631/666/620 632/669/623
+f 591/656/612 337/503/54 619/670/624
+f 618/657/613 617/654/610 645/668/622 646/671/625
+f 605/658/614 604/655/611 632/669/623 633/672/626
+f 592/660/615 591/659/612 619/673/624 620/674/627
+f 366/523/58 618/661/613 646/675/625
+f 606/662/616 605/658/614 633/672/626 634/676/628
+f 593/663/617 592/660/615 620/674/627 621/677/629
+f 607/664/618 606/662/616 634/676/628 635/678/630
+f 594/635/591 593/663/617 621/677/629 622/679/631
+f 608/637/593 607/664/618 635/678/630 636/680/632
+f 595/636/592 594/635/591 622/679/631 623/681/633
+f 609/638/594 608/637/593 636/680/632 637/682/634
+f 596/639/595 595/636/592 623/681/633 624/683/635
+f 610/640/596 609/638/594 637/682/634 638/684/636
+f 597/641/597 596/639/595 624/683/635 625/685/637
+f 611/642/598 610/640/596 638/684/636 639/686/638
+f 598/643/599 597/641/597 625/685/637 626/687/639
+f 612/644/600 611/642/598 639/686/638 640/688/640
+f 599/645/601 598/643/599 626/687/639 627/689/641
+f 613/646/602 612/644/600 640/688/640 641/690/642
+f 600/647/603 599/645/601 627/689/641 628/691/643
+f 614/648/604 613/646/602 641/690/642 642/692/644
+f 601/649/605 600/647/603 628/691/643 629/693/645
+f 615/650/606 614/648/604 642/692/644 643/694/646
+f 602/651/607 601/649/605 629/693/645 630/665/619
+f 616/652/608 615/650/606 643/694/646 644/667/621
+f 639/686/638 638/684/636 666/695/647 667/696/648
+f 626/687/639 625/685/637 653/697/649 654/698/650
+f 640/688/640 639/686/638 667/696/648 668/699/651
+f 627/689/641 626/687/639 654/698/650 655/700/652
+f 641/690/642 640/688/640 668/699/651 669/701/653
+f 628/691/643 627/689/641 655/700/652 656/702/654
+f 642/692/644 641/690/642 669/701/653 670/703/655
+f 629/693/645 628/691/643 656/702/654 657/704/656
+f 643/694/646 642/692/644 670/703/655 671/705/657
+f 630/665/619 629/693/645 657/704/656 658/706/658
+f 644/667/621 643/694/646 671/705/657 672/707/659
+f 631/666/620 630/665/619 658/706/658 659/708/660
+f 645/668/622 644/667/621 672/707/659 673/709/661
+f 632/669/623 631/666/620 659/708/660 660/710/662
+f 619/670/624 337/503/54 647/711/663
+f 646/671/625 645/668/622 673/709/661 674/712/664
+f 633/672/626 632/669/623 660/710/662 661/713/665
+f 620/674/627 619/673/624 647/714/663 648/715/666
+f 366/523/58 646/675/625 674/716/664
+f 634/676/628 633/672/626 661/713/665 662/717/667
+f 621/677/629 620/674/627 648/715/666 649/718/668
+f 635/678/630 634/676/628 662/717/667 663/719/669
+f 622/679/631 621/677/629 649/718/668 650/720/670
+f 636/680/632 635/678/630 663/719/669 664/721/671
+f 623/681/633 622/679/631 650/720/670 651/722/672
+f 637/682/634 636/680/632 664/721/671 665/723/673
+f 624/683/635 623/681/633 651/722/672 652/724/674
+f 638/684/636 637/682/634 665/723/673 666/695/647
+f 625/685/637 624/683/635 652/724/674 653/697/649
+f 366/523/58 674/716/664 702/725/675
+f 662/717/667 661/713/665 689/726/676 690/727/677
+f 649/718/668 648/715/666 676/728/678 677/729/679
+f 663/719/669 662/717/667 690/727/677 691/730/680
+f 650/720/670 649/718/668 677/729/679 678/731/681
+f 664/721/671 663/719/669 691/730/680 692/732/682
+f 651/722/672 650/720/670 678/731/681 679/733/683
+f 665/723/673 664/721/671 692/732/682 693/734/684
+f 652/724/674 651/722/672 679/733/683 680/735/685
+f 666/695/647 665/723/673 693/734/684 694/736/686
+f 653/697/649 652/724/674 680/735/685 681/737/687
+f 667/696/648 666/695/647 694/736/686 695/738/688
+f 654/698/650 653/697/649 681/737/687 682/739/689
+f 668/699/651 667/696/648 695/738/688 696/740/690
+f 655/700/652 654/698/650 682/739/689 683/741/691
+f 669/701/653 668/699/651 696/740/690 697/742/692
+f 656/702/654 655/700/652 683/741/691 684/743/693
+f 670/703/655 669/701/653 697/742/692 698/744/694
+f 657/704/656 656/702/654 684/743/693 685/745/695
+f 671/705/657 670/703/655 698/744/694 699/746/696
+f 658/706/658 657/704/656 685/745/695 686/747/697
+f 672/707/659 671/705/657 699/746/696 700/748/698
+f 659/708/660 658/706/658 686/747/697 687/749/699
+f 673/709/661 672/707/659 700/748/698 701/750/700
+f 660/710/662 659/708/660 687/749/699 688/751/701
+f 647/711/663 337/503/54 675/752/702
+f 674/712/664 673/709/661 701/750/700 702/753/675
+f 661/713/665 660/710/662 688/751/701 689/726/676
+f 648/715/666 647/714/663 675/754/702 676/728/678
+f 698/744/694 697/742/692 725/755/703 726/756/704
+f 685/745/695 684/743/693 712/757/705 713/758/706
+f 699/746/696 698/744/694 726/756/704 727/759/707
+f 686/747/697 685/745/695 713/758/706 714/760/708
+f 700/748/698 699/746/696 727/759/707 728/761/709
+f 687/749/699 686/747/697 714/760/708 715/762/710
+f 701/750/700 700/748/698 728/761/709 729/763/711
+f 688/751/701 687/749/699 715/762/710 716/764/712
+f 675/752/702 337/503/54 703/765/713
+f 702/753/675 701/750/700 729/763/711 730/766/714
+f 689/726/676 688/751/701 716/764/712 717/767/715
+f 676/728/678 675/754/702 703/768/713 704/769/716
+f 366/523/58 702/725/675 730/770/714
+f 690/727/677 689/726/676 717/767/715 718/771/717
+f 677/729/679 676/728/678 704/769/716 705/772/718
+f 691/730/680 690/727/677 718/771/717 719/773/719
+f 678/731/681 677/729/679 705/772/718 706/774/720
+f 692/732/682 691/730/680 719/773/719 720/775/721
+f 679/733/683 678/731/681 706/774/720 707/776/722
+f 693/734/684 692/732/682 720/775/721 721/777/723
+f 680/735/685 679/733/683 707/776/722 708/778/724
+f 694/736/686 693/734/684 721/777/723 722/779/725
+f 681/737/687 680/735/685 708/778/724 709/780/726
+f 695/738/688 694/736/686 722/779/725 723/781/727
+f 682/739/689 681/737/687 709/780/726 710/782/728
+f 696/740/690 695/738/688 723/781/727 724/783/729
+f 683/741/691 682/739/689 710/782/728 711/784/730
+f 697/742/692 696/740/690 724/783/729 725/755/703
+f 684/743/693 683/741/691 711/784/730 712/757/705
+f 707/776/722 706/774/720 734/785/731 735/786/732
+f 721/777/723 720/775/721 748/787/733 749/788/734
+f 708/778/724 707/776/722 735/786/732 736/789/735
+f 722/779/725 721/777/723 749/788/734 750/790/736
+f 709/780/726 708/778/724 736/789/735 737/791/737
+f 723/781/727 722/779/725 750/790/736 751/792/738
+f 710/782/728 709/780/726 737/791/737 738/793/739
+f 724/783/729 723/781/727 751/792/738 752/794/740
+f 711/784/730 710/782/728 738/793/739 739/795/741
+f 725/755/703 724/783/729 752/794/740 753/796/742
+f 712/757/705 711/784/730 739/795/741 740/797/743
+f 726/756/704 725/755/703 753/796/742 754/798/744
+f 713/758/706 712/757/705 740/797/743 741/799/745
+f 727/759/707 726/756/704 754/798/744 755/800/746
+f 714/760/708 713/758/706 741/799/745 742/801/747
+f 728/761/709 727/759/707 755/800/746 756/802/748
+f 715/762/710 714/760/708 742/801/747 743/803/749
+f 729/763/711 728/761/709 756/802/748 757/804/750
+f 716/764/712 715/762/710 743/803/749 744/805/751
+f 703/765/713 337/503/54 731/806/752
+f 730/766/714 729/763/711 757/804/750 758/807/753
+f 717/767/715 716/764/712 744/805/751 745/808/754
+f 704/769/716 703/768/713 731/809/752 732/810/755
+f 366/523/58 730/770/714 758/811/753
+f 718/771/717 717/767/715 745/808/754 746/812/756
+f 705/772/718 704/769/716 732/810/755 733/813/757
+f 719/773/719 718/771/717 746/812/756 747/814/758
+f 706/774/720 705/772/718 733/813/757 734/785/731
+f 720/775/721 719/773/719 747/814/758 748/787/733
+f 743/803/749 742/801/747 770/815/759 771/816/760
+f 757/804/750 756/802/748 784/817/761 785/818/762
+f 744/805/751 743/803/749 771/816/760 772/819/763
+f 731/806/752 337/503/54 759/820/764
+f 758/807/753 757/804/750 785/818/762 786/821/765
+f 745/808/754 744/805/751 772/819/763 773/822/766
+f 732/810/755 731/809/752 759/823/764 760/824/767
+f 366/523/58 758/811/753 786/825/765
+f 746/812/756 745/808/754 773/822/766 774/826/768
+f 733/813/757 732/810/755 760/824/767 761/827/769
+f 747/814/758 746/812/756 774/826/768 775/828/770
+f 734/785/731 733/813/757 761/827/769 762/829/771
+f 748/787/733 747/814/758 775/828/770 776/830/772
+f 735/786/732 734/785/731 762/829/771 763/831/773
+f 749/788/734 748/787/733 776/830/772 777/832/774
+f 736/789/735 735/786/732 763/831/773 764/833/775
+f 750/790/736 749/788/734 777/832/774 778/834/776
+f 737/791/737 736/789/735 764/833/775 765/835/777
+f 751/792/738 750/790/736 778/834/776 779/836/778
+f 738/793/739 737/791/737 765/835/777 766/837/779
+f 752/794/740 751/792/738 779/836/778 780/838/780
+f 739/795/741 738/793/739 766/837/779 767/839/781
+f 753/796/742 752/794/740 780/838/780 781/840/782
+f 740/797/743 739/795/741 767/839/781 768/841/783
+f 754/798/744 753/796/742 781/840/782 782/842/784
+f 741/799/745 740/797/743 768/841/783 769/843/785
+f 755/800/746 754/798/744 782/842/784 783/844/786
+f 742/801/747 741/799/745 769/843/785 770/815/759
+f 756/802/748 755/800/746 783/844/786 784/817/761
+f 779/836/778 778/834/776 806/845/787 807/846/788
+f 766/837/779 765/835/777 793/847/789 794/848/790
+f 780/838/780 779/836/778 807/846/788 808/849/791
+f 767/839/781 766/837/779 794/848/790 795/850/792
+f 781/840/782 780/838/780 808/849/791 809/851/793
+f 768/841/783 767/839/781 795/850/792 796/852/794
+f 782/842/784 781/840/782 809/851/793 810/853/795
+f 769/843/785 768/841/783 796/852/794 797/854/796
+f 783/844/786 782/842/784 810/853/795 811/855/797
+f 770/815/759 769/843/785 797/854/796 798/856/798
+f 784/817/761 783/844/786 811/855/797 812/857/799
+f 771/816/760 770/815/759 798/856/798 799/858/800
+f 785/818/762 784/817/761 812/857/799 813/859/801
+f 772/819/763 771/816/760 799/858/800 800/860/802
+f 759/820/764 337/503/54 787/861/803
+f 786/821/765 785/818/762 813/859/801 814/862/804
+f 773/822/766 772/819/763 800/860/802 801/863/805
+f 760/824/767 759/823/764 787/864/803 788/865/806
+f 366/523/58 786/825/765 814/866/804
+f 774/826/768 773/822/766 801/863/805 802/867/807
+f 761/827/769 760/824/767 788/865/806 789/868/808
+f 775/828/770 774/826/768 802/867/807 803/869/809
+f 762/829/771 761/827/769 789/868/808 790/870/810
+f 776/830/772 775/828/770 803/869/809 804/871/811
+f 763/831/773 762/829/771 790/870/810 791/872/812
+f 777/832/774 776/830/772 804/871/811 805/873/813
+f 764/833/775 763/831/773 791/872/812 792/874/814
+f 778/834/776 777/832/774 805/873/813 806/845/787
+f 765/835/777 764/833/775 792/874/814 793/847/789
+f 788/865/806 787/864/803 815/875/815 816/876/816
+f 802/867/807 801/863/805 829/877/817 830/878/818
+f 814/862/804 813/859/801 841/879/819 842/880/820
+f 789/868/808 788/865/806 816/876/816 817/881/821
+f 366/523/58 814/866/804 842/882/820
+f 790/870/810 789/868/808 817/881/821 818/883/822
+f 791/872/812 790/870/810 818/883/822 819/884/823
+f 803/869/809 802/867/807 830/878/818 831/885/824
+f 792/874/814 791/872/812 819/884/823 820/886/825
+f 804/871/811 803/869/809 831/885/824 832/887/826
+f 793/847/789 792/874/814 820/886/825 821/888/827
+f 805/873/813 804/871/811 832/887/826 833/889/828
+f 794/848/790 793/847/789 821/888/827 822/890/829
+f 806/845/787 805/873/813 833/889/828 834/891/830
+f 795/850/792 794/848/790 822/890/829 823/892/831
+f 807/846/788 806/845/787 834/891/830 835/893/832
+f 796/852/794 795/850/792 823/892/831 824/894/833
+f 808/849/791 807/846/788 835/893/832 836/895/834
+f 797/854/796 796/852/794 824/894/833 825/896/835
+f 809/851/793 808/849/791 836/895/834 837/897/836
+f 798/856/798 797/854/796 825/896/835 826/898/837
+f 810/853/795 809/851/793 837/897/836 838/899/838
+f 799/858/800 798/856/798 826/898/837 827/900/839
+f 811/855/797 810/853/795 838/899/838 839/901/840
+f 800/860/802 799/858/800 827/900/839 828/902/841
+f 812/857/799 811/855/797 839/901/840 840/903/842
+f 787/861/803 337/503/54 815/904/815
+f 801/863/805 800/860/802 828/902/841 829/877/817
+f 813/859/801 812/857/799 840/903/842 841/879/819
+f 836/895/834 835/893/832 863/905/843 864/906/844
+f 823/892/831 822/890/829 850/907/845 851/908/846
+f 837/897/836 836/895/834 864/906/844 865/909/847
+f 824/894/833 823/892/831 851/908/846 852/910/848
+f 838/899/838 837/897/836 865/909/847 866/911/849
+f 825/896/835 824/894/833 852/910/848 853/912/850
+f 839/901/840 838/899/838 866/911/849 867/913/851
+f 826/898/837 825/896/835 853/912/850 854/914/852
+f 840/903/842 839/901/840 867/913/851 868/915/853
+f 827/900/839 826/898/837 854/914/852 855/916/854
+f 841/879/819 840/903/842 868/915/853 869/917/855
+f 828/902/841 827/900/839 855/916/854 856/918/856
+f 815/904/815 337/503/54 843/919/857
+f 842/880/820 841/879/819 869/917/855 870/920/858
+f 829/877/817 828/902/841 856/918/856 857/921/859
+f 816/876/816 815/875/815 843/922/857 844/923/860
+f 366/523/58 842/882/820 870/924/858
+f 830/878/818 829/877/817 857/921/859 858/925/861
+f 817/881/821 816/876/816 844/923/860 845/926/862
+f 831/885/824 830/878/818 858/925/861 859/927/863
+f 818/883/822 817/881/821 845/926/862 846/928/864
+f 832/887/826 831/885/824 859/927/863 860/929/865
+f 819/884/823 818/883/822 846/928/864 847/930/866
+f 833/889/828 832/887/826 860/929/865 861/931/867
+f 820/886/825 819/884/823 847/930/866 848/932/868
+f 834/891/830 833/889/828 861/931/867 862/933/869
+f 821/888/827 820/886/825 848/932/868 849/934/870
+f 835/893/832 834/891/830 862/933/869 863/905/843
+f 822/890/829 821/888/827 849/934/870 850/907/845
+f 859/927/863 858/925/861 886/935/871 887/936/872
+f 846/928/864 845/926/862 873/937/873 874/938/874
+f 860/929/865 859/927/863 887/936/872 888/939/875
+f 847/930/866 846/928/864 874/938/874 875/940/876
+f 861/931/867 860/929/865 888/939/875 889/941/877
+f 848/932/868 847/930/866 875/940/876 876/942/878
+f 862/933/869 861/931/867 889/941/877 890/943/879
+f 849/934/870 848/932/868 876/942/878 877/944/880
+f 863/905/843 862/933/869 890/943/879 891/945/881
+f 850/907/845 849/934/870 877/944/880 878/946/882
+f 864/906/844 863/905/843 891/945/881 892/947/883
+f 851/908/846 850/907/845 878/946/882 879/948/884
+f 865/909/847 864/906/844 892/947/883 893/949/885
+f 852/910/848 851/908/846 879/948/884 880/950/886
+f 866/911/849 865/909/847 893/949/885 894/951/887
+f 853/912/850 852/910/848 880/950/886 881/952/888
+f 867/913/851 866/911/849 894/951/887 895/953/889
+f 854/914/852 853/912/850 881/952/888 882/954/890
+f 868/915/853 867/913/851 895/953/889 896/955/891
+f 855/916/854 854/914/852 882/954/890 883/956/892
+f 869/917/855 868/915/853 896/955/891 897/957/893
+f 856/918/856 855/916/854 883/956/892 884/958/894
+f 843/919/857 337/503/54 871/959/895
+f 870/920/858 869/917/855 897/957/893 898/960/896
+f 857/921/859 856/918/856 884/958/894 885/961/897
+f 844/923/860 843/922/857 871/962/895 872/963/898
+f 366/523/58 870/924/858 898/964/896
+f 858/925/861 857/921/859 885/961/897 886/935/871
+f 845/926/862 844/923/860 872/963/898 873/937/873
+f 895/953/889 894/951/887 24/965/37 25/966/41
+f 882/954/890 881/952/888 11/967/39 12/968/43
+f 896/955/891 895/953/889 25/966/41 26/969/45
+f 883/956/892 882/954/890 12/968/43 13/970/47
+f 897/957/893 896/955/891 26/969/45 27/971/49
+f 884/958/894 883/956/892 13/970/47 14/972/51
+f 871/973/895 337/54/54 1/53/53
+f 898/960/896 897/957/893 27/971/49 28/974/56
+f 885/961/897 884/958/894 14/972/51 15/975/2
+f 872/976/898 871/977/895 1/58/53 2/6/6
+f 366/523/58 898/964/896 28/978/56
+f 886/935/871 885/961/897 15/975/2 16/979/1
+f 873/980/873 872/976/898 2/6/6 3/5/5
+f 887/936/872 886/935/871 16/979/1 17/981/9
+f 874/938/874 873/937/873 3/982/5 4/983/11
+f 888/939/875 887/936/872 17/981/9 18/984/13
+f 875/940/876 874/938/874 4/983/11 5/985/15
+f 889/941/877 888/939/875 18/984/13 19/986/17
+f 876/942/878 875/940/876 5/985/15 6/987/19
+f 890/943/879 889/941/877 19/986/17 20/988/21
+f 877/944/880 876/942/878 6/987/19 7/989/23
+f 891/945/881 890/943/879 20/988/21 21/990/25
+f 878/946/882 877/944/880 7/989/23 8/991/27
+f 892/947/883 891/945/881 21/990/25 22/992/29
+f 879/948/884 878/946/882 8/991/27 9/993/31
+f 893/949/885 892/947/883 22/992/29 23/994/33
+f 880/950/886 879/948/884 9/993/31 10/995/35
+f 894/951/887 893/949/885 23/994/33 24/965/37
+f 881/952/888 880/950/886 10/995/35 11/967/39
diff --git a/data/two_cubes.sdf b/data/two_cubes.sdf
new file mode 100644
index 0000000..e19b73b
--- /dev/null
+++ b/data/two_cubes.sdf
@@ -0,0 +1,240 @@
+<sdf version='1.6'>
+ <world name='default'>
+ <light name='sun' type='directional'>
+ <yunfei>99.2</yunfei>
+ <cast_shadows>1</cast_shadows>
+ <pose frame=''>0 0 10 0 -0 0</pose>
+ <diffuse>0.8 0.8 0.8 1</diffuse>
+ <specular>0.2 0.2 0.2 1</specular>
+ <attenuation>
+ <range>1000</range>
+ <constant>0.9</constant>
+ <linear>0.01</linear>
+ <quadratic>0.001</quadratic>
+ </attenuation>
+ <direction>-0.5 0.1 -0.9</direction>
+ </light>
+ <model name='ground_plane'>
+ <static>1</static>
+ <link name='link'>
+ <collision name='collision'>
+ <geometry>
+ <plane>
+ <normal>1 2 3</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>65535</collide_bitmask>
+ <ode/>
+ </contact>
+ <friction>
+ <ode>
+ <mu>100</mu>
+ <mu2>50</mu2>
+ </ode>
+ <torsional>
+ <ode/>
+ </torsional>
+ </friction>
+ <bounce/>
+ </surface>
+ <max_contacts>10</max_contacts>
+ </collision>
+ <visual name='visual'>
+ <cast_shadows>0</cast_shadows>
+ <geometry>
+ <plane>
+ <normal>4 5 6</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ </model>
+ <gravity>0 0 -9.8</gravity>
+ <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+ <atmosphere type='adiabatic'/>
+ <physics name='default_physics' default='0' type='ode'>
+ <max_step_size>0.001</max_step_size>
+ <real_time_factor>1</real_time_factor>
+ <real_time_update_rate>1000</real_time_update_rate>
+ </physics>
+ <scene>
+ <ambient>0.4 0.4 0.4 1</ambient>
+ <background>0.7 0.7 0.7 1</background>
+ <shadows>1</shadows>
+ </scene>
+ <audio>
+ <device>default</device>
+ </audio>
+ <spherical_coordinates>
+ <surface_model>EARTH_WGS84</surface_model>
+ <latitude_deg>0</latitude_deg>
+ <longitude_deg>0</longitude_deg>
+ <elevation>0</elevation>
+ <heading_deg>0</heading_deg>
+ </spherical_coordinates>
+ <model name='unit_box_0'>
+ <static>1</static>
+ <pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
+ <link name='unit_box_0::link'>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.166667</iyy>
+ <iyz>0</iyz>
+ <izz>0.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name='collision'>
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ <max_contacts>10</max_contacts>
+ <surface>
+ <contact>
+ <ode/>
+ </contact>
+ <bounce/>
+ <friction>
+ <torsional>
+ <ode/>
+ </torsional>
+ <ode/>
+ </friction>
+ </surface>
+ </collision>
+ <visual name='visual'>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>cube.obj</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <diffuse>1 1 1 1</diffuse>
+ <script>
+ <name>Gazebo/Grey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ </model>
+ <model name='unit_box_0_clone'>
+ <pose frame=''>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
+ <link name='unit_box_0::link'>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.166667</iyy>
+ <iyz>0</iyz>
+ <izz>0.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name='collision'>
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ <max_contacts>10</max_contacts>
+ <surface>
+ <contact>
+ <ode/>
+ </contact>
+ <bounce/>
+ <friction>
+ <torsional>
+ <ode/>
+ </torsional>
+ <ode/>
+ </friction>
+ </surface>
+ </collision>
+ <visual name='visual'>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>cube.obj</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <diffuse>1 1 1 1</diffuse>
+ </material>
+ </visual>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ </model>
+ <state world_name='default'>
+ <sim_time>0 0</sim_time>
+ <real_time>0 0</real_time>
+ <wall_time>1462824251 956472000</wall_time>
+ <iterations>0</iterations>
+ <model name='ground_plane'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <scale>1 1 1</scale>
+ <link name='link'>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <velocity>0 0 0 0 -0 0</velocity>
+ <acceleration>0 0 0 0 -0 0</acceleration>
+ <wrench>0 0 0 0 -0 0</wrench>
+ </link>
+ </model>
+ <model name='unit_box_0'>
+ <pose frame=''>0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159</pose>
+ <scale>1 1 1</scale>
+ <link name='unit_box_0::link'>
+ <pose frame=''>0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159</pose>
+ <velocity>0.004896 3e-06 -0.004891 -6e-06 0.009793 -0</velocity>
+ <acceleration>0.010615 0.006191 -9.78231 -0.012424 0.021225 -1.8e-05</acceleration>
+ <wrench>0.010615 0.006191 -9.78231 0 -0 0</wrench>
+ </link>
+ </model>
+ <model name='unit_box_0_clone'>
+ <pose frame=''>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
+ <scale>1 1 1</scale>
+ <link name='unit_box_0::link'>
+ <pose frame=''>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
+ <velocity>0 0 0 0 -0 0</velocity>
+ <acceleration>0 0 0 0 -0 0</acceleration>
+ <wrench>0 0 0 0 -0 0</wrench>
+ </link>
+ </model>
+ <light name='sun'>
+ <pose frame=''>0 0 10 0 -0 0</pose>
+ </light>
+ </state>
+ <gui fullscreen='0'>
+ <camera name='user_camera'>
+ <pose frame=''>8.0562 -8.87312 3.07529 0 0.205021 2.5208</pose>
+ <view_controller>orbit</view_controller>
+ <projection_type>perspective</projection_type>
+ </camera>
+ </gui>
+ </world>
+</sdf>
diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/BasicDemo/BasicExample.cpp
index d501b51..dac3008 100644
--- a/examples/BasicDemo/BasicExample.cpp
+++ b/examples/BasicDemo/BasicExample.cpp
@@ -14,7 +14,6 @@ subject to the following restrictions:
*/
-
#include "BasicExample.h"
#include "btBulletDynamicsCommon.h"
@@ -39,10 +38,10 @@ struct BasicExample : public CommonRigidBodyBase
virtual void renderScene();
void resetCamera()
{
- float dist = 41;
+ float dist = 4;
float pitch = 52;
float yaw = 35;
- float targetPos[3]={0,0.46,0};
+ float targetPos[3]={0,0,0};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
};
@@ -52,7 +51,7 @@ void BasicExample::initPhysics()
m_guiHelper->setUpAxis(1);
createEmptyDynamicsWorld();
-
+ //m_dynamicsWorld->setGravity(btVector3(0,0,0));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
if (m_dynamicsWorld->getDebugDrawer())
@@ -63,7 +62,7 @@ void BasicExample::initPhysics()
//groundShape->initializePolyhedralFeatures();
-// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
+ //btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
@@ -81,7 +80,7 @@ void BasicExample::initPhysics()
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
- btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
+ btBoxShape* colShape = createBoxShape(btVector3(.1,.1,.1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
@@ -108,9 +107,9 @@ void BasicExample::initPhysics()
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(btVector3(
- btScalar(2.0*i),
- btScalar(20+2.0*k),
- btScalar(2.0*j)));
+ btScalar(0.2*i),
+ btScalar(2+.2*k),
+ btScalar(0.2*j)));
createRigidBody(mass,startTransform,colShape);
@@ -121,7 +120,9 @@ void BasicExample::initPhysics()
}
}
+
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+
}
@@ -140,7 +141,11 @@ void BasicExample::renderScene()
CommonExampleInterface* BasicExampleCreateFunc(CommonExampleOptions& options)
{
return new BasicExample(options.m_guiHelper);
+
}
+B3_STANDALONE_EXAMPLE(BasicExampleCreateFunc)
+
+
diff --git a/examples/BasicDemo/CMakeLists.txt b/examples/BasicDemo/CMakeLists.txt
index 716c3a0..f01a03a 100644
--- a/examples/BasicDemo/CMakeLists.txt
+++ b/examples/BasicDemo/CMakeLists.txt
@@ -29,3 +29,74 @@ IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
SET_TARGET_PROPERTIES(App_BasicExample PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
SET_TARGET_PROPERTIES(App_BasicExample PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+
+
+
+
+
+#################
+# Standalone BasicExampleGui using OpenGL (but not the example browser)
+
+
+INCLUDE_DIRECTORIES(
+ ${BULLET_PHYSICS_SOURCE_DIR}/src
+ ${BULLET_PHYSICS_SOURCE_DIR}/btgui
+ ${BULLET_PHYSICS_SOURCE_DIR}/examples
+ ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew
+)
+
+
+SET(AppBasicExampleGui_SRCS
+ BasicExample.cpp
+ ${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc
+ ../StandaloneMain/main_opengl_single_example.cpp
+ ../ExampleBrowser/OpenGLGuiHelper.cpp
+ ../ExampleBrowser/GL_ShapeDrawer.cpp
+ ../ExampleBrowser/CollisionShape2TriangleMesh.cpp
+ ../Utils/b3Clock.cpp
+)
+
+#this define maps StandaloneExampleCreateFunc to the right 'CreateFunc'
+ADD_DEFINITIONS(-DB3_USE_STANDALONE_EXAMPLE)
+
+LINK_LIBRARIES(
+ BulletDynamics BulletCollision LinearMath OpenGLWindow Bullet3Common ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+)
+
+#some code to support OpenGL and Glew cross platform
+IF (WIN32)
+ INCLUDE_DIRECTORIES(
+ ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows
+ )
+ ADD_DEFINITIONS(-DGLEW_STATIC)
+ELSE(WIN32)
+ IF(APPLE)
+ find_library(COCOA NAMES Cocoa)
+ MESSAGE(${COCOA})
+ link_libraries(${COCOA})
+
+ ELSE(APPLE)
+ INCLUDE_DIRECTORIES(
+ ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows
+ )
+ ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1")
+ ADD_DEFINITIONS("-DGLEW_STATIC")
+ ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
+
+ LINK_LIBRARIES( X11 pthread dl Xext)
+ ENDIF(APPLE)
+ENDIF(WIN32)
+
+
+ADD_EXECUTABLE(AppBasicExampleGui
+ ${AppBasicExampleGui_SRCS}
+)
+
+
+IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+ SET_TARGET_PROPERTIES(AppBasicExampleGui PROPERTIES DEBUG_POSTFIX "_Debug")
+ SET_TARGET_PROPERTIES(AppBasicExampleGui PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
+ SET_TARGET_PROPERTIES(AppBasicExampleGui PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
+ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+
+
diff --git a/examples/BasicDemo/main.cpp b/examples/BasicDemo/main.cpp
index c6039fc..9ee7222 100644
--- a/examples/BasicDemo/main.cpp
+++ b/examples/BasicDemo/main.cpp
@@ -18,8 +18,13 @@ subject to the following restrictions:
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btHashMap.h"
int main(int argc, char* argv[])
@@ -39,3 +44,4 @@ int main(int argc, char* argv[])
return 0;
}
+
diff --git a/examples/BasicDemo/premake4.lua b/examples/BasicDemo/premake4.lua
index b4901e9..dd92424 100644
--- a/examples/BasicDemo/premake4.lua
+++ b/examples/BasicDemo/premake4.lua
@@ -20,3 +20,184 @@ files {
"**.h",
}
+
+project "App_BasicExampleGui"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+ "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
+}
+
+ initOpenGL()
+ initGlew()
+
+
+language "C++"
+
+files {
+ "BasicExample.cpp",
+ "*.h",
+ "../StandaloneMain/main_opengl_single_example.cpp",
+ "../ExampleBrowser/OpenGLGuiHelper.cpp",
+ "../ExampleBrowser/GL_ShapeDrawer.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../Utils/b3Clock.cpp",
+ "../Utils/b3Clock.h",
+}
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+end
+
+
+
+
+project "App_BasicExampleGuiWithSoftwareRenderer"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+ "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
+}
+
+ initOpenGL()
+ initGlew()
+
+
+language "C++"
+
+files {
+ "BasicExample.cpp",
+ "*.h",
+ "../StandaloneMain/main_sw_tinyrenderer_single_example.cpp",
+ "../ExampleBrowser/OpenGLGuiHelper.cpp",
+ "../ExampleBrowser/GL_ShapeDrawer.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../TinyRenderer/geometry.cpp",
+ "../TinyRenderer/model.cpp",
+ "../TinyRenderer/tgaimage.cpp",
+ "../TinyRenderer/our_gl.cpp",
+ "../TinyRenderer/TinyRenderer.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3Clock.cpp",
+ "../Utils/b3Clock.h",
+}
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+end
+
+
+project "App_BasicExampleTinyRenderer"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+ "BulletDynamics","BulletCollision", "LinearMath", "Bullet3Common"
+}
+
+
+language "C++"
+
+files {
+ "BasicExample.cpp",
+ "*.h",
+ "../StandaloneMain/main_tinyrenderer_single_example.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../OpenGLWindow/SimpleCamera.cpp",
+ "../TinyRenderer/geometry.cpp",
+ "../TinyRenderer/model.cpp",
+ "../TinyRenderer/tgaimage.cpp",
+ "../TinyRenderer/our_gl.cpp",
+ "../TinyRenderer/TinyRenderer.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3Clock.cpp",
+ "../Utils/b3Clock.h",
+}
+
+
+
+
+ if _OPTIONS["enable_openvr"] then
+
+project "App_BasicExampleVR"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE","BT_ENABLE_VR"}
+
+
+
+includedirs {"../../src",
+ "../ThirdPartyLibs/openvr/headers",
+ "../ThirdPartyLibs/openvr/samples/shared"}
+
+links {
+ "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common", "openvr_api"
+}
+
+ initOpenGL()
+ initGlew()
+
+
+language "C++"
+
+files {
+ "BasicExample.cpp",
+ "*.h",
+ "../StandaloneMain/hellovr_opengl_main.cpp",
+ "../ExampleBrowser/OpenGLGuiHelper.cpp",
+ "../ExampleBrowser/GL_ShapeDrawer.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/lodepng.h",
+ "../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/Matrices.h",
+ "../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/pathtools.h",
+ "../ThirdPartyLibs/openvr/samples/shared/Vectors.h",
+ "../Utils/b3Clock.cpp",
+ "../Utils/b3Clock.h",
+
+}
+
+if os.is("Windows") then
+ libdirs {"../ThirdPartyLibs/openvr/lib/win32"}
+end
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+end
+
+end
\ No newline at end of file
diff --git a/examples/Benchmarks/BenchmarkDemo.cpp b/examples/Benchmarks/BenchmarkDemo.cpp
index ca9f757..f659d01 100644
--- a/examples/Benchmarks/BenchmarkDemo.cpp
+++ b/examples/Benchmarks/BenchmarkDemo.cpp
@@ -1273,7 +1273,7 @@ void BenchmarkDemo::exitPhysics()
-struct CommonExampleInterface* BenchmarkCreateFunc(struct CommonExampleOptions& options)
+CommonExampleInterface* BenchmarkCreateFunc(struct CommonExampleOptions& options)
{
return new BenchmarkDemo(options.m_guiHelper,options.m_option);
}
\ No newline at end of file
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index 4d26967..47728c2 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -1,5 +1,7 @@
SUBDIRS( HelloWorld BasicDemo )
IF(BUILD_BULLET3)
- SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen OpenGLWindow)
+ SUBDIRS( ExampleBrowser ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK OpenGLWindow )
ENDIF()
-
+IF(BUILD_PYBULLET)
+ SUBDIRS(pybullet)
+ENDIF(BUILD_PYBULLET)
diff --git a/examples/Collision/CollisionTutorialBullet2.cpp b/examples/Collision/CollisionTutorialBullet2.cpp
index 9473c83..0479a73 100644
--- a/examples/Collision/CollisionTutorialBullet2.cpp
+++ b/examples/Collision/CollisionTutorialBullet2.cpp
@@ -226,7 +226,7 @@ public:
{
char relativeFileName[1024];
sprintf(relativeFileName,"%s%s",prefix[i],filename);
- image = stbi_load(relativeFileName, &width, &height, &n, 0);
+ image = stbi_load(relativeFileName, &width, &height, &n, 3);
}
b3Assert(image);
@@ -265,8 +265,9 @@ public:
virtual void stepSimulation(float deltaTime)
{
+#ifndef BT_NO_PROFILE
CProfileManager::Reset();
-
+#endif
@@ -314,7 +315,9 @@ public:
m_app->m_renderer->writeTransforms();
+#ifndef BT_NO_PROFILE
CProfileManager::Increment_Frame_Counter();
+#endif
}
virtual void renderScene()
{
diff --git a/examples/CommonInterfaces/CommonCameraInterface.h b/examples/CommonInterfaces/CommonCameraInterface.h
index 8f05659..1e9fa2f 100644
--- a/examples/CommonInterfaces/CommonCameraInterface.h
+++ b/examples/CommonInterfaces/CommonCameraInterface.h
@@ -6,6 +6,11 @@ struct CommonCameraInterface
virtual void getCameraProjectionMatrix(float m[16])const = 0;
virtual void getCameraViewMatrix(float m[16]) const = 0;
+ virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16])=0;
+ virtual void disableVRCamera()=0;
+ virtual bool isVRCamera() const =0;
+ virtual void setVRCameraOffsetTransform(const float offset[16])=0;
+
virtual void getCameraTargetPosition(float pos[3]) const = 0;
virtual void getCameraPosition(float pos[3]) const = 0;
diff --git a/examples/CommonInterfaces/CommonExampleInterface.h b/examples/CommonInterfaces/CommonExampleInterface.h
index 5d2c002..df5f181 100644
--- a/examples/CommonInterfaces/CommonExampleInterface.h
+++ b/examples/CommonInterfaces/CommonExampleInterface.h
@@ -10,12 +10,14 @@ struct CommonExampleOptions
//Those are optional, some examples will use them others don't. Each example should work with them being 0.
int m_option;
const char* m_fileName;
+ class SharedMemoryInterface* m_sharedMem;
CommonExampleOptions(struct GUIHelperInterface* helper, int option=0)
:m_guiHelper(helper),
m_option(option),
- m_fileName(0)
+ m_fileName(0),
+ m_sharedMem(0)
{
}
@@ -44,8 +46,48 @@ public:
virtual bool mouseButtonCallback(int button, int state, float x, float y)=0;
virtual bool keyboardCallback(int key, int state)=0;
+ virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis) {}
+ virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]){}
+
+ virtual void processCommandLineArgs(int argc, char* argv[]){};
+};
+
+class ExampleEntries
+{
+
+public:
+
+ virtual ~ExampleEntries() {}
+
+
+ virtual void initExampleEntries()=0;
+
+ virtual void initOpenCLExampleEntries()=0;
+
+ virtual int getNumRegisteredExamples()=0;
+
+ virtual CommonExampleInterface::CreateFunc* getExampleCreateFunc(int index)=0;
+
+ virtual const char* getExampleName(int index)=0;
+
+ virtual const char* getExampleDescription(int index)=0;
+
+ virtual int getExampleOption(int index)=0;
+
};
+CommonExampleInterface* StandaloneExampleCreateFunc(CommonExampleOptions& options);
+
+#ifdef B3_USE_STANDALONE_EXAMPLE
+ #define B3_STANDALONE_EXAMPLE(ExampleFunc) CommonExampleInterface* StandaloneExampleCreateFunc(CommonExampleOptions& options)\
+ {\
+ return ExampleFunc(options);\
+ }
+#else//B3_USE_STANDALONE_EXAMPLE
+ #define B3_STANDALONE_EXAMPLE(ExampleFunc)
+#endif //B3_USE_STANDALONE_EXAMPLE
+
+
#endif //COMMON_EXAMPLE_INTERFACE_H
diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h
index 0930399..baea8da 100644
--- a/examples/CommonInterfaces/CommonGUIHelperInterface.h
+++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h
@@ -29,10 +29,11 @@ struct GUIHelperInterface
virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld)=0;
- virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) =0;
-
+ virtual int registerTexture(const unsigned char* texels, int width, int height)=0;
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) = 0;
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) =0;
-
+ virtual void removeAllGraphicsInstances()=0;
+
virtual Common2dCanvasInterface* get2dCanvasInterface()=0;
virtual CommonParameterInterface* getParameterInterface()=0;
@@ -45,6 +46,24 @@ struct GUIHelperInterface
virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)=0;
+ virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
+ unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
+ float* depthBuffer, int depthBufferSizeInPixels,
+ int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)
+ {
+ copyCameraImageData(viewMatrix,projectionMatrix,pixelsRGBA,rgbaBufferSizeInPixels,
+ depthBuffer,depthBufferSizeInPixels,
+ 0,0,
+ startPixelIndex,destinationWidth,
+ destinationHeight,numPixelsCopied);
+ }
+
+ virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
+ unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
+ float* depthBuffer, int depthBufferSizeInPixels,
+ int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
+ int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)=0;
+
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0;
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0;
@@ -71,10 +90,11 @@ struct DummyGUIHelper : public GUIHelperInterface
virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
- virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) { return -1; }
-
- virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) { return -1;}
-
+ virtual int registerTexture(const unsigned char* texels, int width, int height){return -1;}
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId){return -1;}
+ virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) {return -1;}
+ virtual void removeAllGraphicsInstances(){}
+
virtual Common2dCanvasInterface* get2dCanvasInterface()
{
return 0;
@@ -103,6 +123,17 @@ struct DummyGUIHelper : public GUIHelperInterface
{
}
+ virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
+ unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
+ float* depthBuffer, int depthBufferSizeInPixels,
+ int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
+ int startPixelIndex, int width, int height, int* numPixelsCopied)
+
+ {
+ if (numPixelsCopied)
+ *numPixelsCopied = 0;
+ }
+
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
{
}
diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h
index 302148a..674807d 100644
--- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h
+++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h
@@ -15,10 +15,10 @@ struct DrawGridData
int upAxis;
float gridColor[4];
- DrawGridData()
+ DrawGridData(int upAxis=1)
:gridSize(10),
upOffset(0.001f),
- upAxis(1)
+ upAxis(upAxis)
{
gridColor[0] = 0.6f;
gridColor[1] = 0.6f;
@@ -66,9 +66,9 @@ struct CommonGraphicsApp
m_mouseYpos(0.f),
m_mouseInitialized(false)
{
- m_backgroundColorRGB[0] = 0.9;
- m_backgroundColorRGB[1] = 0.9;
- m_backgroundColorRGB[2] = 1;
+ m_backgroundColorRGB[0] = 0.7;
+ m_backgroundColorRGB[1] = 0.7;
+ m_backgroundColorRGB[2] = 0.8;
}
virtual ~CommonGraphicsApp()
{
@@ -76,6 +76,9 @@ struct CommonGraphicsApp
virtual void dumpNextFrameToPng(const char* pngFilename){}
virtual void dumpFramesToVideo(const char* mp4Filename){}
+
+ virtual void getScreenPixels(unsigned char* rgbaBuffer, int bufferSizeInBytes, float* depthBuffer, int depthBufferSizeInBytes){}
+
virtual void getBackgroundColor(float* red, float* green, float* blue) const
{
if (red)
@@ -119,8 +122,10 @@ struct CommonGraphicsApp
virtual void swapBuffer() = 0;
virtual void drawText( const char* txt, int posX, int posY) = 0;
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0;
+ virtual void drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)=0;
virtual int registerCubeShape(float halfExtentsX,float halfExtentsY, float halfExtentsZ, int textureIndex = -1, float textureScaling = 1)=0;
virtual int registerGraphicsUnitSphereShape(EnumSphereLevelOfDetail lod, int textureId=-1) = 0;
+
virtual void registerGrid(int xres, int yres, float color0[4], float color1[4])=0;
diff --git a/examples/CommonInterfaces/CommonMultiBodyBase.h b/examples/CommonInterfaces/CommonMultiBodyBase.h
index 3219f82..48130b2 100644
--- a/examples/CommonInterfaces/CommonMultiBodyBase.h
+++ b/examples/CommonInterfaces/CommonMultiBodyBase.h
@@ -63,7 +63,7 @@ struct CommonMultiBodyBase : public CommonExampleInterface
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
- m_broadphase = new btDbvtBroadphase();
+ m_broadphase = new btDbvtBroadphase();//btSimpleBroadphase();
m_solver = new btMultiBodyConstraintSolver;
@@ -97,6 +97,20 @@ struct CommonMultiBodyBase : public CommonExampleInterface
{
m_dynamicsWorld->removeConstraint(m_dynamicsWorld->getConstraint(i));
}
+
+ for (i = m_dynamicsWorld->getNumMultiBodyConstraints() - 1; i >= 0; i--)
+ {
+ btMultiBodyConstraint* mbc = m_dynamicsWorld->getMultiBodyConstraint(i);
+ m_dynamicsWorld->removeMultiBodyConstraint(mbc);
+ delete mbc;
+ }
+
+ for (i = m_dynamicsWorld->getNumMultibodies() - 1; i >= 0; i--)
+ {
+ btMultiBody* mb = m_dynamicsWorld->getMultiBody(i);
+ m_dynamicsWorld->removeMultiBody(mb);
+ delete mb;
+ }
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
diff --git a/examples/CommonInterfaces/CommonParameterInterface.h b/examples/CommonInterfaces/CommonParameterInterface.h
index dd8554d..3bb2f50 100644
--- a/examples/CommonInterfaces/CommonParameterInterface.h
+++ b/examples/CommonInterfaces/CommonParameterInterface.h
@@ -40,12 +40,14 @@ struct ButtonParams
const char* m_name;
int m_buttonId;
void* m_userPointer;
+ bool m_isTrigger;
ButtonParamChangedCallback m_callback;
ButtonParams(const char* name, int buttonId, bool isTrigger)
:m_name(name),
m_buttonId(buttonId),
m_userPointer(0),
+ m_isTrigger(isTrigger),
m_callback(0)
{
}
diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h
index a3d1d0f..888607d 100644
--- a/examples/CommonInterfaces/CommonRenderInterface.h
+++ b/examples/CommonInterfaces/CommonRenderInterface.h
@@ -22,13 +22,14 @@ struct CommonRenderInterface
virtual void init()=0;
virtual void updateCamera(int upAxis)=0;
virtual void removeAllInstances() = 0;
-
+
virtual const CommonCameraInterface* getActiveCamera() const =0;
virtual CommonCameraInterface* getActiveCamera()=0;
virtual void setActiveCamera(CommonCameraInterface* cam)=0;
- virtual void renderScene()=0;
+ virtual void renderScene()=0;
+ virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE){};
virtual int getScreenWidth() = 0;
virtual int getScreenHeight() = 0;
@@ -43,11 +44,19 @@ struct CommonRenderInterface
virtual void drawPoint(const double* position, const double color[4], double pointDrawSize)=0;
virtual int registerShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType=B3_GL_TRIANGLES, int textureIndex=-1)=0;
virtual void updateShape(int shapeIndex, const float* vertices)=0;
- virtual int registerTexture(const unsigned char* texels, int width, int height)=0;
+
+ virtual int registerTexture(const unsigned char* texels, int width, int height)=0;
+ virtual void updateTexture(int textureIndex, const unsigned char* texels)=0;
+ virtual void activateTexture(int textureIndex)=0;
+
virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)=0;
virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)=0;
virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex)=0;
virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex)=0;
+ virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex)=0;
+ virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex)=0;
+
+ virtual int getTotalNumInstances() const = 0;
virtual void writeTransforms()=0;
virtual void enableBlend(bool blend)=0;
diff --git a/examples/CommonInterfaces/CommonRigidBodyBase.h b/examples/CommonInterfaces/CommonRigidBodyBase.h
index c13f951..07175ca 100644
--- a/examples/CommonInterfaces/CommonRigidBodyBase.h
+++ b/examples/CommonInterfaces/CommonRigidBodyBase.h
@@ -446,9 +446,15 @@ struct CommonRigidBodyBase : public CommonExampleInterface
virtual void renderScene()
{
- m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
+ {
+
+ m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
+ }
- m_guiHelper->render(m_dynamicsWorld);
+ {
+
+ m_guiHelper->render(m_dynamicsWorld);
+ }
}
};
diff --git a/examples/CommonInterfaces/CommonWindowInterface.h b/examples/CommonInterfaces/CommonWindowInterface.h
index fb3b574..346fdac 100644
--- a/examples/CommonInterfaces/CommonWindowInterface.h
+++ b/examples/CommonInterfaces/CommonWindowInterface.h
@@ -119,7 +119,9 @@ class CommonWindowInterface
virtual float getRetinaScale() const =0;
virtual void setAllowRetina(bool allow) =0;
-
+
+ virtual int getWidth() const = 0;
+ virtual int getHeight() const = 0;
virtual int fileOpenDialog(char* fileName, int maxFileNameLength) = 0;
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 92c4ae0..39b8c04 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -1,20 +1,137 @@
-
INCLUDE_DIRECTORIES(
.
${BULLET_PHYSICS_SOURCE_DIR}/src
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
)
-FILE(GLOB ExampleBrowser_SRCS "*" "GwenGUISupport/*" )
-FILE(GLOB ExampleBrowser_HDRS "*" "GwenGUISupport/*" )
+FILE(GLOB GwenGUISupport_SRCS "GwenGUISupport/*" )
+FILE(GLOB GwenGUISupport_HDRS "GwenGUISupport/*" )
+
+
+IF (WIN32)
+ INCLUDE_DIRECTORIES(
+ ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew
+ )
+ ADD_DEFINITIONS(-DGLEW_STATIC)
+ELSE(WIN32)
+ IF(APPLE)
+ find_library(COCOA NAMES Cocoa)
+ ELSE(APPLE)
+ ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1")
+ ADD_DEFINITIONS("-DGLEW_STATIC")
+ ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
+ INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew )
+ ENDIF(APPLE)
+ENDIF(WIN32)
+
+
+ADD_LIBRARY(BulletExampleBrowserLib
+ OpenGLExampleBrowser.cpp
+ OpenGLGuiHelper.cpp
+ GL_ShapeDrawer.cpp
+ CollisionShape2TriangleMesh.cpp
+ CollisionShape2TriangleMesh.h
+ ../Utils/b3Clock.cpp
+ ../Utils/b3Clock.h
+ ../Utils/b3ResourcePath.cpp
+ ../Utils/b3ResourcePath.h
+ ${GwenGUISupport_SRCS}
+ ${GwenGUISupport_HDRS}
+
+)
+
+SET_TARGET_PROPERTIES(BulletExampleBrowserLib PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(BulletExampleBrowserLib PROPERTIES SOVERSION ${BULLET_VERSION})
+IF (BUILD_SHARED_LIBS)
+ IF (WIN32)
+ TARGET_LINK_LIBRARIES(
+ BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils
+ BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
+ ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+ )
+ ELSE(WIN32)
+ IF(APPLE)
+ TARGET_LINK_LIBRARIES(
+ BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils
+ BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
+ ${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+ )
+ ELSE(APPLE)
+ TARGET_LINK_LIBRARIES(
+ BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils
+ BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
+ pthread dl
+ )
+ ENDIF(APPLE)
+ ENDIF(WIN32)
+ENDIF(BUILD_SHARED_LIBS)
+
+####################
+#
+# Bullet Example Browser main app
+#
+####################
+
+INCLUDE_DIRECTORIES(
+ .
+ ${BULLET_PHYSICS_SOURCE_DIR}/src
+ ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
+)
+
+
+LINK_LIBRARIES(
+ BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
+)
+
+IF (WIN32)
+ INCLUDE_DIRECTORIES(
+ ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew
+ )
+ LINK_LIBRARIES(
+ ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+ )
+ ADD_DEFINITIONS(-DGLEW_STATIC)
+ELSE(WIN32)
+ IF(APPLE)
+ find_library(COCOA NAMES Cocoa)
+ MESSAGE(${COCOA})
+ link_libraries(${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY})
+ ELSE(APPLE)
+ ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1")
+ ADD_DEFINITIONS("-DGLEW_STATIC")
+ ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
+ INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew )
+ LINK_LIBRARIES( pthread dl)
+ ENDIF(APPLE)
+ENDIF(WIN32)
+SET(ExtendedTutorialsSources
+ ../ExtendedTutorials/SimpleBox.cpp
+ ../ExtendedTutorials/MultipleBoxes.cpp
+ ../ExtendedTutorials/SimpleJoint.cpp
+ ../ExtendedTutorials/SimpleCloth.cpp
+ ../ExtendedTutorials/Chain.cpp
+ ../ExtendedTutorials/Bridge.cpp
+ ../ExtendedTutorials/RigidBodyFromObj.cpp
+ ../ExtendedTutorials/InclinedPlane.cpp
+ ../ExtendedTutorials/InclinedPlane.h
+ ../ExtendedTutorials/NewtonsCradle.cpp
+)
-SET(App_ExampleBrowser_SRCS
- main.cpp
- ExampleEntries.cpp
- ExampleEntries.h
+SET(BulletExampleBrowser_SRCS
+
+ ../TinyRenderer/geometry.cpp
+ ../TinyRenderer/model.cpp
+ ../TinyRenderer/tgaimage.cpp
+ ../TinyRenderer/our_gl.cpp
+ ../TinyRenderer/TinyRenderer.cpp
+ ../SharedMemory/TinyRendererVisualShapeConverter.cpp
+ ../SharedMemory/TinyRendererVisualShapeConverter.h
+ ../SharedMemory/IKTrajectoryHelper.cpp
+ ../SharedMemory/IKTrajectoryHelper.h
+
../SharedMemory/PhysicsServer.cpp
../SharedMemory/PhysicsClientSharedMemory.cpp
../SharedMemory/PhysicsClient.cpp
@@ -23,11 +140,13 @@ SET(App_ExampleBrowser_SRCS
../SharedMemory/PhysicsClientExample.cpp
../SharedMemory/PosixSharedMemory.cpp
../SharedMemory/Win32SharedMemory.cpp
+ ../SharedMemory/InProcessMemory.cpp
../SharedMemory/PhysicsServerSharedMemory.cpp
../SharedMemory/PhysicsDirect.cpp
../SharedMemory/PhysicsDirect.h
../SharedMemory/PhysicsDirectC_API.cpp
../SharedMemory/PhysicsDirectC_API.h
+ ../SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
../SharedMemory/PhysicsLoopBack.cpp
../SharedMemory/PhysicsLoopBack.h
../SharedMemory/PhysicsLoopBackC_API.cpp
@@ -38,12 +157,30 @@ SET(App_ExampleBrowser_SRCS
../BasicDemo/BasicExample.h
../InverseDynamics/InverseDynamicsExample.cpp
../InverseDynamics/InverseDynamicsExample.h
+ ../InverseKinematics/InverseKinematicsExample.cpp
+ ../InverseKinematics/InverseKinematicsExample.h
../ForkLift/ForkLiftDemo.cpp
../ForkLift/ForkLiftDemo.h
../Tutorial/Tutorial.cpp
../Tutorial/Tutorial.h
../Tutorial/Dof6ConstraintTutorial.cpp
../Tutorial/Dof6ConstraintTutorial.h
+ ../ExtendedTutorials/SimpleBox.cpp
+ ../ExtendedTutorials/SimpleBox.h
+ ../ExtendedTutorials/MultipleBoxes.cpp
+ ../ExtendedTutorials/MultipleBoxes.h
+ ../ExtendedTutorials/SimpleCloth.cpp
+ ../ExtendedTutorials/SimpleCloth.h
+ ../ExtendedTutorials/SimpleJoint.cpp
+ ../ExtendedTutorials/SimpleJoint.h
+ ../ExtendedTutorials/NewtonsCradle.cpp
+ ../ExtendedTutorials/NewtonsCradle.h
+ ../ExtendedTutorials/NewtonsRopeCradle.cpp
+ ../ExtendedTutorials/NewtonsRopeCradle.h
+ ../ExtendedTutorials/InclinedPlane.cpp
+ ../ExtendedTutorials/InclinedPlane.h
+ ../ExtendedTutorials/MultiPendulum.cpp
+ ../ExtendedTutorials/MultiPendulum.h
../Collision/CollisionSdkC_Api.cpp
../Collision/CollisionSdkC_Api.h
../Collision/CollisionTutorialBullet2.cpp
@@ -71,11 +208,24 @@ SET(App_ExampleBrowser_SRCS
../MultiThreading/b3PosixThreadSupport.cpp
../MultiThreading/b3Win32ThreadSupport.cpp
../MultiThreading/b3ThreadSupportInterface.cpp
+ ../RenderingExamples/TinyRendererSetup.cpp
../RenderingExamples/TimeSeriesCanvas.cpp
../RenderingExamples/TimeSeriesCanvas.h
../RenderingExamples/TimeSeriesFontData.cpp
../RenderingExamples/TimeSeriesFontData.h
-
+ ../RenderingExamples/DynamicTexturedCubeDemo.cpp
+ ../RenderingExamples/DynamicTexturedCubeDemo.h
+ ../RenderingExamples/TinyVRGui.cpp
+ ../RenderingExamples/TinyVRGui.h
+
+ ../RoboticsLearning/GripperGraspExample.cpp
+ ../RoboticsLearning/GripperGraspExample.h
+ ../RoboticsLearning/b3RobotSimAPI.cpp
+ ../RoboticsLearning/b3RobotSimAPI.h
+ ../RoboticsLearning/R2D2GraspExample.cpp
+ ../RoboticsLearning/R2D2GraspExample.h
+ ../RoboticsLearning/KukaGraspExample.cpp
+ ../RoboticsLearning/KukaGraspExample.h
../RenderingExamples/CoordinateSystemDemo.cpp
../RenderingExamples/CoordinateSystemDemo.h
../RenderingExamples/RaytracerSetup.cpp
@@ -91,11 +241,14 @@ SET(App_ExampleBrowser_SRCS
../Importers/ImportBsp/BspConverter.h
../Importers/ImportBullet/SerializeSetup.cpp
../Importers/ImportBullet/SerializeSetup.h
-
+ ../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
+../Importers/ImportMeshUtility/b3ImportMeshUtility.h
../../Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp
../../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp
-../../Extras/Serialize/BulletFileLoader/bChunk.cpp ../../Extras/Serialize/BulletFileLoader/bFile.cpp
-../../Extras/Serialize/BulletFileLoader/bDNA.cpp ../../Extras/Serialize/BulletFileLoader/btBulletFile.cpp
+ ../../Extras/Serialize/BulletFileLoader/bChunk.cpp
+ ../../Extras/Serialize/BulletFileLoader/bFile.cpp
+ ../../Extras/Serialize/BulletFileLoader/bDNA.cpp
+ ../../Extras/Serialize/BulletFileLoader/btBulletFile.cpp
../Importers/ImportBsp/BspLoader.h
../Importers/ImportBsp/ImportBspExample.h
@@ -121,6 +274,7 @@ SET(App_ExampleBrowser_SRCS
../Importers/ImportObjDemo/LoadMeshFromObj.cpp
../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
../Importers/ImportSTLDemo/ImportSTLSetup.cpp
+ ../Importers/ImportSDFDemo/ImportSDFSetup.cpp
../Importers/ImportURDFDemo/ImportURDFSetup.cpp
../Importers/ImportURDFDemo/URDF2Bullet.cpp
../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
@@ -137,6 +291,7 @@ SET(App_ExampleBrowser_SRCS
../Vehicles/Hinge2Vehicle.cpp
../Vehicles/Hinge2Vehicle.h
../MultiBody/Pendulum.cpp
+ ../MultiBody/MultiBodySoftContact.cpp
../MultiBody/TestJointTorqueSetup.cpp
../MultiBody/TestJointTorqueSetup.h
../MultiBody/InvertedPendulumPDControl.cpp
@@ -144,6 +299,7 @@ SET(App_ExampleBrowser_SRCS
../MultiBody/MultiBodyConstraintFeedback.cpp
../MultiBody/MultiDofDemo.cpp
../MultiBody/MultiDofDemo.h
+ ../RigidBody/RigidBodySoftContact.cpp
../Constraints/TestHingeTorque.cpp
../Constraints/TestHingeTorque.h
../Constraints/ConstraintDemo.cpp
@@ -156,77 +312,39 @@ SET(App_ExampleBrowser_SRCS
../ThirdPartyLibs/stb_image/stb_image.cpp
../ThirdPartyLibs/stb_image/stb_image.h
-
+
../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
../ThirdPartyLibs/tinyxml/tinystr.cpp
../ThirdPartyLibs/tinyxml/tinyxml.cpp
../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
-
- ../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp
- ../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/model.cpp
- ../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp
- ../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/joint.cpp
- ../ThirdPartyLibs/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h
- ../ThirdPartyLibs/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h
- ../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h
- ../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h
- ../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h
- ../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h
../ThirdPartyLibs/tinyxml/tinystr.cpp
../ThirdPartyLibs/tinyxml/tinyxml.cpp
../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
- ../ThirdPartyLibs/urdf/boost_replacement/lexical_cast.h
- ../ThirdPartyLibs/urdf/boost_replacement/shared_ptr.h
- ../ThirdPartyLibs/urdf/boost_replacement/printf_console.cpp
- ../ThirdPartyLibs/urdf/boost_replacement/printf_console.h
- ../ThirdPartyLibs/urdf/boost_replacement/string_split.cpp
- ../ThirdPartyLibs/urdf/boost_replacement/string_split.h
- ../Utils/b3Clock.cpp
- ../Utils/b3Clock.h
- ../Utils/b3ResourcePath.cpp
- ../Utils/b3ResourcePath.h
- ${ExampleBrowser_SRCS}
- ${ExampleBrowser_HDRS}
+ InProcessExampleBrowser.cpp
${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc
)
-LINK_LIBRARIES(
- Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen
-)
-
-IF (WIN32)
- SET(App_ExampleBrowser_SRCS ${App_ExampleBrowser_SRCS} )
- INCLUDE_DIRECTORIES(
- ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew
- )
- LINK_LIBRARIES(
- ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
- )
- ADD_DEFINITIONS(-DGLEW_STATIC)
-ELSE(WIN32)
- IF(APPLE)
- find_library(COCOA NAMES Cocoa)
- MESSAGE(${COCOA})
- link_libraries(${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY})
- ELSE(APPLE)
- ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1")
- ADD_DEFINITIONS("-DGLEW_STATIC")
- ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
- INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/Glew )
- LINK_LIBRARIES( pthread dl)
- ENDIF(APPLE)
-ENDIF(WIN32)
-
-
ADD_EXECUTABLE(App_ExampleBrowser
- ${App_ExampleBrowser_SRCS}
+ main.cpp
+ ExampleEntries.cpp
+ ExampleEntries.h
+ ${ExtendedTutorialsSources}
+ ${BulletExampleBrowser_SRCS}
)
+FILE( MAKE_DIRECTORY "${PROJECT_BINARY_DIR}/data" )
+
+ADD_CUSTOM_COMMAND(
+ TARGET App_ExampleBrowser
+ POST_BUILD
+ COMMAND ${CMAKE_COMMAND} ARGS -E copy_directory ${BULLET_PHYSICS_SOURCE_DIR}/data ${PROJECT_BINARY_DIR}/data
+ )
+
IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
SET_TARGET_PROPERTIES(App_ExampleBrowser PROPERTIES DEBUG_POSTFIX "_Debug")
diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp
new file mode 100644
index 0000000..2bc8a0e
--- /dev/null
+++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp
@@ -0,0 +1,193 @@
+
+#include "CollisionShape2TriangleMesh.h"
+
+#include "btBulletCollisionCommon.h"
+#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
+
+void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTransform& parentTransform, btAlignedObjectArray<btVector3>& vertexPositions, btAlignedObjectArray<btVector3>& vertexNormals, btAlignedObjectArray<int>& indicesOut)
+
+{
+//todo: support all collision shape types
+ switch (collisionShape->getShapeType())
+ {
+ case SOFTBODY_SHAPE_PROXYTYPE:
+ {
+ //skip the soft body collision shape for now
+ break;
+ }
+ case STATIC_PLANE_PROXYTYPE:
+ {
+ //draw a box, oriented along the plane normal
+ const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(collisionShape);
+ btScalar planeConst = staticPlaneShape->getPlaneConstant();
+ const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
+ btVector3 planeOrigin = planeNormal * planeConst;
+ btVector3 vec0,vec1;
+ btPlaneSpace1(planeNormal,vec0,vec1);
+ btScalar vecLen = 100.f;
+ btVector3 verts[4];
+
+ verts[0] = planeOrigin + vec0*vecLen + vec1*vecLen;
+ verts[1] = planeOrigin - vec0*vecLen + vec1*vecLen;
+ verts[2] = planeOrigin - vec0*vecLen - vec1*vecLen;
+ verts[3] = planeOrigin + vec0*vecLen - vec1*vecLen;
+
+ int startIndex = vertexPositions.size();
+ indicesOut.push_back(startIndex+0);
+ indicesOut.push_back(startIndex+1);
+ indicesOut.push_back(startIndex+2);
+ indicesOut.push_back(startIndex+0);
+ indicesOut.push_back(startIndex+2);
+ indicesOut.push_back(startIndex+3);
+
+ btVector3 triNormal = parentTransform.getBasis()*planeNormal;
+
+
+ for (int i=0;i<4;i++)
+ {
+ btVector3 vtxPos;
+ btVector3 pos =parentTransform*verts[i];
+ vertexPositions.push_back(pos);
+ vertexNormals.push_back(triNormal);
+ }
+ break;
+ }
+ case TRIANGLE_MESH_SHAPE_PROXYTYPE:
+ {
+
+
+ btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) collisionShape;
+ btVector3 trimeshScaling = trimesh->getLocalScaling();
+ btStridingMeshInterface* meshInterface = trimesh->getMeshInterface();
+ btAlignedObjectArray<btVector3> vertices;
+ btAlignedObjectArray<int> indices;
+
+ for (int partId=0;partId<meshInterface->getNumSubParts();partId++)
+ {
+
+ const unsigned char *vertexbase = 0;
+ int numverts = 0;
+ PHY_ScalarType type = PHY_INTEGER;
+ int stride = 0;
+ const unsigned char *indexbase = 0;
+ int indexstride = 0;
+ int numfaces = 0;
+ PHY_ScalarType indicestype = PHY_INTEGER;
+ //PHY_ScalarType indexType=0;
+
+ btVector3 triangleVerts[3];
+ meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
+ btVector3 aabbMin,aabbMax;
+
+ for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
+ {
+ unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);
+
+ for (int j=2;j>=0;j--)
+ {
+
+ int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
+ if (type == PHY_FLOAT)
+ {
+ float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+ triangleVerts[j] = btVector3(
+ graphicsbase[0]*trimeshScaling.getX(),
+ graphicsbase[1]*trimeshScaling.getY(),
+ graphicsbase[2]*trimeshScaling.getZ());
+ }
+ else
+ {
+ double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
+ triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*trimeshScaling.getX()),
+ btScalar(graphicsbase[1]*trimeshScaling.getY()),
+ btScalar(graphicsbase[2]*trimeshScaling.getZ()));
+ }
+ }
+ indices.push_back(vertices.size());
+ vertices.push_back(triangleVerts[0]);
+ indices.push_back(vertices.size());
+ vertices.push_back(triangleVerts[1]);
+ indices.push_back(vertices.size());
+ vertices.push_back(triangleVerts[2]);
+
+ btVector3 triNormal = (triangleVerts[1]-triangleVerts[0]).cross(triangleVerts[2]-triangleVerts[0]);
+ triNormal.normalize();
+
+ for (int v=0;v<3;v++)
+ {
+
+ btVector3 pos =parentTransform*triangleVerts[v];
+ indicesOut.push_back(vertexPositions.size());
+ vertexPositions.push_back(pos);
+ vertexNormals.push_back(triNormal);
+
+ }
+
+
+ }
+ }
+
+ break;
+ }
+ default:
+ {
+ if (collisionShape->isConvex())
+ {
+ btConvexShape* convex = (btConvexShape*)collisionShape;
+ {
+ btShapeHull* hull = new btShapeHull(convex);
+ hull->buildHull(0.0);
+
+ {
+ //int strideInBytes = 9*sizeof(float);
+ //int numVertices = hull->numVertices();
+ //int numIndices =hull->numIndices();
+
+ for (int t=0;t<hull->numTriangles();t++)
+ {
+
+ btVector3 triNormal;
+
+ int index0 = hull->getIndexPointer()[t*3+0];
+ int index1 = hull->getIndexPointer()[t*3+1];
+ int index2 = hull->getIndexPointer()[t*3+2];
+ btVector3 pos0 =parentTransform*hull->getVertexPointer()[index0];
+ btVector3 pos1 =parentTransform*hull->getVertexPointer()[index1];
+ btVector3 pos2 =parentTransform*hull->getVertexPointer()[index2];
+ triNormal = (pos1-pos0).cross(pos2-pos0);
+ triNormal.normalize();
+
+ for (int v=0;v<3;v++)
+ {
+ int index = hull->getIndexPointer()[t*3+v];
+ btVector3 pos =parentTransform*hull->getVertexPointer()[index];
+ indicesOut.push_back(vertexPositions.size());
+ vertexPositions.push_back(pos);
+ vertexNormals.push_back(triNormal);
+ }
+ }
+ }
+ delete hull;
+ }
+ } else
+ {
+ if (collisionShape->isCompound())
+ {
+ btCompoundShape* compound = (btCompoundShape*) collisionShape;
+ for (int i=0;i<compound->getNumChildShapes();i++)
+ {
+
+ btTransform childWorldTrans = parentTransform * compound->getChildTransform(i);
+ CollisionShape2TriangleMesh(compound->getChildShape(i),childWorldTrans,vertexPositions,vertexNormals,indicesOut);
+ }
+ } else
+ {
+ btAssert(0);
+ }
+
+ }
+ }
+ };
+}
+
+
diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.h b/examples/ExampleBrowser/CollisionShape2TriangleMesh.h
new file mode 100644
index 0000000..6025d64
--- /dev/null
+++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.h
@@ -0,0 +1,10 @@
+#ifndef COLLISION_SHAPE_2_GRAPHICS_H
+#define COLLISION_SHAPE_2_GRAPHICS_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+class btCollisionShape;
+
+void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTransform& parentTransform, btAlignedObjectArray<btVector3>& vertexPositions, btAlignedObjectArray<btVector3>& vertexNormals, btAlignedObjectArray<int>& indicesOut);
+
+#endif //COLLISION_SHAPE_2_GRAPHICS_H
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index b252993..3310084 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -1,5 +1,3 @@
-
-
#include "ExampleEntries.h"
#include "LinearMath/btAlignedObjectArray.h"
@@ -7,6 +5,8 @@
#include "../RenderingExamples/RenderInstancingDemo.h"
#include "../RenderingExamples/CoordinateSystemDemo.h"
#include "../RenderingExamples/RaytracerSetup.h"
+#include "../RenderingExamples/TinyRendererSetup.h"
+#include "../RenderingExamples/DynamicTexturedCubeDemo.h"
#include "../ForkLift/ForkLiftDemo.h"
#include "../BasicDemo/BasicExample.h"
#include "../Planar2D/Planar2D.h"
@@ -16,16 +16,18 @@
#include "../Importers/ImportColladaDemo/ImportColladaSetup.h"
#include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
+#include "../Importers/ImportSDFDemo/ImportSDFSetup.h"
#include "../Collision/CollisionTutorialBullet2.h"
#include "../GyroscopicDemo/GyroscopicSetup.h"
#include "../Constraints/Dof6Spring2Setup.h"
#include "../Constraints/ConstraintPhysicsSetup.h"
#include "../MultiBody/TestJointTorqueSetup.h"
#include "../MultiBody/Pendulum.h"
-
+#include "../MultiBody/MultiBodySoftContact.h"
#include "../MultiBody/MultiBodyConstraintFeedback.h"
#include "../MultiBody/MultiDofDemo.h"
#include "../MultiBody/InvertedPendulumPDControl.h"
+#include "../RigidBody/RigidBodySoftContact.h"
#include "../VoronoiFracture/VoronoiFractureDemo.h"
#include "../SoftDemo/SoftDemo.h"
#include "../Constraints/ConstraintDemo.h"
@@ -43,6 +45,10 @@
#include "../Tutorial/Dof6ConstraintTutorial.h"
#include "../MultiThreading/MultiThreadingExample.h"
#include "../InverseDynamics/InverseDynamicsExample.h"
+#include "../RoboticsLearning/R2D2GraspExample.h"
+#include "../RoboticsLearning/KukaGraspExample.h"
+#include "../RoboticsLearning/GripperGraspExample.h"
+#include "../InverseKinematics/InverseKinematicsExample.h"
#ifdef ENABLE_LUA
#include "../LuaDemo/LuaPhysicsSetup.h"
@@ -55,7 +61,18 @@
#endif
#endif //B3_USE_CLEW
-
+//Extended Tutorial Includes Added by Mobeen and Benelot
+#include "../ExtendedTutorials/SimpleBox.h"
+#include "../ExtendedTutorials/MultipleBoxes.h"
+#include "../ExtendedTutorials/SimpleJoint.h"
+#include "../ExtendedTutorials/SimpleCloth.h"
+#include "../ExtendedTutorials/Chain.h"
+#include "../ExtendedTutorials/Bridge.h"
+#include "../ExtendedTutorials/RigidBodyFromObj.h"
+#include "../ExtendedTutorials/InclinedPlane.h"
+#include "../ExtendedTutorials/NewtonsCradle.h"
+#include "../ExtendedTutorials/NewtonsRopeCradle.h"
+#include "../ExtendedTutorials/MultiPendulum.h"
struct ExampleEntry
{
@@ -77,49 +94,58 @@ struct ExampleEntry
};
+
static ExampleEntry gDefaultExamples[]=
{
-
-
-
-
+
ExampleEntry(0,"API"),
+
ExampleEntry(1,"Basic Example","Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc),
ExampleEntry(1,"Rolling Friction", "Damping is often not good enough to keep rounded objects from rolling down a sloped surface. Instead, you can set the rolling friction of a rigid body. Generally it is best to leave the rolling friction to zero, to avoid artifacts.", RollingFrictionCreateFunc),
-
- ExampleEntry(1,"Constraints","Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.",
+
+ ExampleEntry(1,"Constraints","Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.",
AllConstraintCreateFunc),
ExampleEntry(1,"Motorized Hinge","Use of a btHingeConstraint. You can adjust the first slider to change the target velocity, and the second slider to adjust the maximum impulse applied to reach the target velocity. Note that the hinge angle can reach beyond -360 and 360 degrees.", ConstraintCreateFunc),
ExampleEntry(1,"TestHingeTorque", "Apply a torque in the hinge axis. This example uses a btHingeConstraint and btRigidBody. The setup is similar to the multi body example TestJointTorque.",
TestHingeTorqueCreateFunc),
// ExampleEntry(0,"What's new in 2.83"),
-
- ExampleEntry(1,"6DofSpring2","Show the use of the btGeneric6DofSpring2Constraint. This is a replacement of the btGeneric6DofSpringConstraint, it has various improvements. This includes improved spring implementation and better control over the restitution (bounce) when the constraint hits its limits.",
+
+ ExampleEntry(1,"6DofSpring2","Show the use of the btGeneric6DofSpring2Constraint. This is a replacement of the btGeneric6DofSpringConstraint, it has various improvements. This includes improved spring implementation and better control over the restitution (bounce) when the constraint hits its limits.",
Dof6Spring2CreateFunc),
ExampleEntry(1,"Motor Demo", "Dynamic control the target velocity of a motor of a btHingeConstraint. This demo makes use of the 'internal tick callback'. You can press W for wireframe, C and L to visualize constraint frame and limits.", MotorControlCreateFunc),
-
- ExampleEntry(1,"Gyroscopic", "Show the Dzhanibekov effect using various settings of the gyroscopic term. You can select the gyroscopic term computation using btRigidBody::setFlags, with arguments BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT (using explicit integration, which adds energy and can lead to explosions), BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD, BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY. If you don't set any of these flags, there is no gyroscopic term used.", GyroscopicCreateFunc),
+ ExampleEntry(1,"Gyroscopic", "Show the Dzhanibekov effect using various settings of the gyroscopic term. You can select the gyroscopic term computation using btRigidBody::setFlags, with arguments BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT (using explicit integration, which adds energy and can lead to explosions), BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD, BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY. If you don't set any of these flags, there is no gyroscopic term used.", GyroscopicCreateFunc),
-
+ ExampleEntry(1,"Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",RigidBodySoftContactCreateFunc),
ExampleEntry(0,"MultiBody"),
ExampleEntry(1,"MultiDofCreateFunc","Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),
ExampleEntry(1,"TestPendulum","Simulate a pendulum using btMultiBody with a constant joint torque applied. The same code is also used as a unit test comparing Bullet with the numerical solution of second-order non-linear differential equation stored in pendulum_gold.h", TestPendulumCreateFunc),
-
- ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
+
+ ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
ExampleEntry(1,"Inverted Pendulum PD","Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc),
+ ExampleEntry(1,"MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",MultiBodySoftContactCreateFunc,0),
+
+
+ ExampleEntry(0,"Inverse Dynamics"),
+ ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF),
+ ExampleEntry(1,"Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY),
+
+ ExampleEntry(0, "Inverse Kinematics"),
+ ExampleEntry(1, "SDLS", "Selectively Damped Least Squares by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_SDLS),
+ ExampleEntry(1, "DLS", "Damped Least Squares by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_DLS),
+ ExampleEntry(1, "DLS-SVD", "Damped Least Squares with Singular Value Decomposition by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_DLS_SVD),
- ExampleEntry(0,"Inverse Dynamics"),
- ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF),
- ExampleEntry(1,"Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY),
+
+
+ ExampleEntry(1, "Jacobi Transpose", "Jacobi Transpose by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_JACOB_TRANS),
+ ExampleEntry(1, "Jacobi Pseudo Inv", "Jacobi Pseudo Inverse Method by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_PURE_PSEUDO),
-
ExampleEntry(0,"Tutorial"),
ExampleEntry(1,"Constant Velocity","Free moving rigid body, without external or constraint forces", TutorialCreateFunc,TUT_VELOCITY),
ExampleEntry(1,"Gravity Acceleration","Motion of a free falling rigid body under constant gravitational acceleration", TutorialCreateFunc,TUT_ACCELERATION),
@@ -130,13 +156,13 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(0,"Collision"),
ExampleEntry(1, "Spheres & Plane C-API (Bullet2)", "Collision C-API using Bullet 2.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_BULLET2),
//ExampleEntry(1, "Spheres & Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3),
-
-
+
+
#ifdef INCLUDE_CLOTH_DEMOS
ExampleEntry(0,"Soft Body"),
ExampleEntry(1,"Cloth","Simulate a patch of cloth.", SoftDemoCreateFunc,0),
-
+
ExampleEntry(1,"Pressure","Simulate 3d soft body using a pressure constraint.",SoftDemoCreateFunc,1),
ExampleEntry(1,"Volume","Simulate 3d soft body using a volume constraint.",SoftDemoCreateFunc,2),
ExampleEntry(1,"Ropes","Simulate ropes", SoftDemoCreateFunc,3),
@@ -144,7 +170,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Cloth Attach","A rigid body attached to a cloth.", SoftDemoCreateFunc,5),
ExampleEntry(1,"Sticks","Show simulation of ropes fixed to the ground.", SoftDemoCreateFunc,6),
ExampleEntry(1,"Capsule Collision","Collision detection between a capsule shape and cloth.", SoftDemoCreateFunc,7),
-
+
ExampleEntry(1,"Collide","Soft body collision", SoftDemoCreateFunc,8),
ExampleEntry(1,"Collide 2","Soft body collision",SoftDemoCreateFunc,9),
ExampleEntry(1,"Collide 3","Soft body collision",SoftDemoCreateFunc,10),
@@ -170,11 +196,11 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Cluster Stack Mixed","Stacking of soft bodies and rigid bodies.",SoftDemoCreateFunc,29),
ExampleEntry(1,"Tetra Cube","Simulate a volumetric soft body cube defined by tetrahedra.", SoftDemoCreateFunc,30),
ExampleEntry(1,"Tetra Bunny","Simulate a volumetric soft body Stanford bunny defined by tetrahedra.", SoftDemoCreateFunc,31),
-
+
#endif //INCLUDE_CLOTH_DEMOS
///we disable the benchmarks in debug mode, they are way too slow and benchmarking in debug mode is not recommended
-#ifndef _DEBUG
+//#ifndef _DEBUG
ExampleEntry(0,"Benchmarks"),
ExampleEntry(1,"3000 boxes", "Benchmark a stack of 3000 boxes. It will stress the collision detection, a specialized box-box implementation based on the separating axis test, and the constraint solver. ", BenchmarkCreateFunc, 1),
ExampleEntry(1,"1000 stack", "Benchmark a stack of 3000 boxes. It will stress the collision detection, a specialized box-box implementation based on the separating axis test, and the constraint solver. ",
@@ -184,66 +210,77 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Prim vs Mesh", "Benchmark the performance and stability of rigid bodies using primitive collision shapes (btSphereShape, btBoxShape), resting on a triangle mesh, btBvhTriangleMeshShape.", BenchmarkCreateFunc, 5),
ExampleEntry(1,"Convex vs Mesh", "Benchmark the performance and stability of rigid bodies using convex hull collision shapes (btConvexHullShape), resting on a triangle mesh, btBvhTriangleMeshShape.", BenchmarkCreateFunc, 6),
ExampleEntry(1,"Raycast", "Benchmark the performance of the btCollisionWorld::rayTest. Note that currently the rays are not rendered.", BenchmarkCreateFunc, 7),
-#endif
+//#endif
+
-
ExampleEntry(0,"Importers"),
ExampleEntry(1,"Import .bullet", "Load a binary .bullet file. The serialization mechanism can deal with versioning, differences in endianess, 32 and 64bit, double/single precision. It is easy to save a .bullet file, see the examples/Importers/ImportBullet/SerializeDemo.cpp for a code example how to export a .bullet file.", SerializeBulletCreateFunc),
-
ExampleEntry(1,"Wavefront Obj", "Import a Wavefront .obj file", ImportObjCreateFunc, 0),
+ ExampleEntry(1,"Obj2RigidBody (Show Obj)", "Load a triangle mesh from Wavefront .obj and turn it in a convex hull collision shape, connected to a rigid body. We can use the original .obj mesh data to visualize the rigid body. In 'debug' wireframe mode (press 'w' to toggle) we still see the convex hull data.", ET_RigidBodyFromObjCreateFunc),
+ ExampleEntry(1,"Obj2RigidBody (Show Hull)", "Load a triangle mesh from Wavefront .obj and turn it in a convex hull collision shape, connected to a rigid body", ET_RigidBodyFromObjCreateFunc,ObjUseConvexHullForRendering),
+ ExampleEntry(1,"Obj2RigidBody Optimize", "Load a triangle mesh from Wavefront .obj, remove the vertices that are not on the convex hull", ET_RigidBodyFromObjCreateFunc,OptimizeConvexObj),
ExampleEntry(1,"Quake BSP", "Import a Quake .bsp file", ImportBspCreateFunc, 0),
- ExampleEntry(1,"COLLADA dae", "Import the geometric mesh data from a COLLADA file. This is used as part of the URDF importer. This loader can also be used to import collision geometry in general. ",
+ ExampleEntry(1,"COLLADA dae", "Import the geometric mesh data from a COLLADA file. This is used as part of the URDF importer. This loader can also be used to import collision geometry in general. ",
ImportColladaCreateFunc, 0),
ExampleEntry(1,"STL", "Import the geometric mesh data from a STL file. This is used as part of the URDF importer. This loader can also be used to import collision geometry in general. ",ImportSTLCreateFunc, 0),
ExampleEntry(1,"URDF (RigidBody)", "Import a URDF file, and create rigid bodies (btRigidBody) connected by constraints.", ImportURDFCreateFunc, 0),
- ExampleEntry(1,"URDF (MultiBody)", "Import a URDF file and create a single multibody (btMultiBody) with tree hierarchy of links (mobilizers).",
+ ExampleEntry(1,"URDF (MultiBody)", "Import a URDF file and create a single multibody (btMultiBody) with tree hierarchy of links (mobilizers).",
ImportURDFCreateFunc, 1),
+ ExampleEntry(1,"SDF (MultiBody)", "Import an SDF file, create multiple multibodies etc", ImportSDFCreateFunc),
ExampleEntry(0,"Vehicles"),
ExampleEntry(1,"Hinge2 Vehicle", "A rigid body chassis with 4 rigid body wheels attached by a btHinge2Constraint",Hinge2VehicleCreateFunc),
ExampleEntry(1,"ForkLift","Simulate a fork lift vehicle with a working fork lift that can be moved using the cursor keys. The wheels collision is simplified using ray tests."
"There are currently some issues with the wheel rendering, the wheels rotate when picking up the object."
- "The demo implementation allows to choose various MLCP constraint solvers.",
+ "The demo implementation allows to choose various MLCP constraint solvers.",
ForkLiftCreateFunc),
ExampleEntry(0,"Raycast"),
ExampleEntry(1,"Raytest", "Cast rays using the btCollisionWorld::rayTest method. The example shows how to receive the hit position and normal along the ray against the first object. Also it shows how to receive all the hits along a ray.", RaytestCreateFunc),
- ExampleEntry(1,"Raytracer","Implement an extremely simple ray tracer using the ray trace functionality in btCollisionWorld.",
+ ExampleEntry(1,"Raytracer","Implement an extremely simple ray tracer using the ray trace functionality in btCollisionWorld.",
RayTracerCreateFunc),
-
+
+
ExampleEntry(0,"Experiments"),
-
-// ExampleEntry(1,"Robot Control (Velocity)", "Perform some robot control tasks, using physics server and client that communicate over shared memory",
-// RobotControlExampleCreateFunc,ROBOT_VELOCITY_CONTROL),
-// ExampleEntry(1,"Robot Control (PD)", "Perform some robot control tasks, using physics server and client that communicate over shared memory",
-// RobotControlExampleCreateFunc,ROBOT_PD_CONTROL),
-// ExampleEntry(1,"Robot Joint Feedback", "Apply some small ping-pong target velocity jitter, and read the joint reaction forces, using physics server and client that communicate over shared memory.",
- // RobotControlExampleCreateFunc,ROBOT_PING_PONG_JOINT_FEEDBACK),
-
+ ExampleEntry(1,"Robot Control", "Create a physics client and server to create and control robots.",
+ PhysicsClientCreateFunc, eCLIENTEXAMPLE_SERVER),
ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory",
PhysicsServerCreateFunc),
+ ExampleEntry(1,"Physics Server (RTC)", "Create a physics server that communicates with a physics client over shared memory. At each update, the Physics Server will continue calling 'stepSimulation' based on the real-time clock (RTC).",
+ PhysicsServerCreateFunc,PHYSICS_SERVER_USE_RTC_CLOCK),
+
ExampleEntry(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.",
PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING),
ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
+ ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
+ ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
+
+ ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP),
+ ExampleEntry(1,"Kuka IK","Control a Kuka IIWA robot to follow a target using IK. This IK is not setup properly yet.", KukaGraspExampleCreateFunc,0),
+ ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),
+ ExampleEntry(1,"Rolling friction","Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_ROLLING_FRICTION),
+ ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP),
+ ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
+ ExampleEntry(1,"One Motor Gripper Grasp","Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP),
+
- ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc),
#ifdef ENABLE_LUA
ExampleEntry(1,"Lua Script", "Create the dynamics world, collision shapes and rigid bodies using Lua scripting",
LuaDemoCreateFunc),
#endif
ExampleEntry(1,"MultiThreading (submitJob)", "Simple example of executing jobs across multiple threads.",
MultiThreadingExampleCreateFunc,SINGLE_SIM_THREAD),
-
+
ExampleEntry(1,"Voronoi Fracture", "Automatically create a compound rigid body using voronoi tesselation. Individual parts are modeled as rigid bodies using a btConvexHullShape.",
VoronoiFractureCreateFunc),
ExampleEntry(1,"Fracture demo", "Create a basic custom implementation to model fracturing objects, based on a btCompoundShape. It explicitly propagates the collision impulses and breaks the rigid body into multiple rigid bodies. Press F to toggle fracture and glue mode.", FractureDemoCreateFunc),
-
+
ExampleEntry(1,"Planar 2D","Show the use of 2D collision shapes and rigid body simulation. The collision shape is wrapped into a btConvex2dShape. The rigid bodies are restricted in a plane using the 'setAngularFactor' and 'setLinearFactor' API call.",Planar2DCreateFunc),
@@ -251,8 +288,31 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(0,"Rendering"),
ExampleEntry(1,"Instanced Rendering", "Simple example of fast instanced rendering, only active when using OpenGL3+.",RenderInstancingCreateFunc),
ExampleEntry(1,"CoordinateSystemDemo","Show the axis and positive rotation direction around the axis.", CoordinateSystemCreateFunc),
- ExampleEntry(1,"Time Series", "Render some value(s) in a 2D graph window, shifting to the left", TimeSeriesCreateFunc)
-
+ ExampleEntry(1,"Time Series", "Render some value(s) in a 2D graph window, shifting to the left", TimeSeriesCreateFunc),
+ ExampleEntry(1,"TinyRenderer", "Very small software renderer.", TinyRendererCreateFunc),
+ ExampleEntry(1,"Dynamic Texture", "Dynamic updated textured applied to a cube.", DynamicTexturedCubeDemoCreateFunc),
+
+
+
+ //Extended Tutorials Added by Mobeen
+ ExampleEntry(0,"Extended Tutorials"),
+ ExampleEntry(1,"Simple Box", "Simplest possible demo creating a single box rigid body that falls under gravity", ET_SimpleBoxCreateFunc),
+ ExampleEntry(1,"Multiple Boxes", "Add multiple box rigid bodies that fall under gravity", ET_MultipleBoxesCreateFunc),
+ ExampleEntry(1,"Simple Joint", "Create a single distance constraint between two box rigid bodies", ET_SimpleJointCreateFunc),
+ ExampleEntry(1,"Simple Cloth", "Create a simple piece of cloth", ET_SimpleClothCreateFunc),
+ ExampleEntry(1,"Simple Chain", "Create a simple chain using a pair of point2point/distance constraints. You may click and drag any box to see the chain respond.", ET_ChainCreateFunc),
+ ExampleEntry(1,"Simple Bridge", "Create a simple bridge using a pair of point2point/distance constraints. You may click and drag any plank to see the bridge respond.", ET_BridgeCreateFunc),
+ ExampleEntry(1,"Inclined Plane", "Create an inclined plane to show restitution and different types of friction. Use the sliders to vary restitution and friction and press space to reset the scene.", ET_InclinedPlaneCreateFunc),
+ ExampleEntry(1,"Newton's Cradle", "Create a Newton's Cradle using a pair of point2point/slider constraints. Press 1/2 to lengthen/shorten the pendula, press 3 to displace pendula. Use the sliders to select the number (reset simulation), length and restitution of pendula, the number of displaced pendula and apply the displacement force.", ET_NewtonsCradleCreateFunc),
+ ExampleEntry(1,"Newton's Rope Cradle", "Create a Newton's Cradle using ropes. Press 3 to displace pendula. Use the sliders to select the number (reset simulation), length and restitution of pendula and the number of displaced pendula and apply the displacement force.",ET_NewtonsRopeCradleCreateFunc),
+ ExampleEntry(1,"Multi-Pendulum", "Create a Multi-Pendulum using point2point/slider constraints. Press 1/2 to lengthen/shorten the pendula, press 3 to displace pendula. Use the sliders to select the number (reset simulation), length and restitution of pendula, the number of displaced pendula and apply the displacement force.",ET_MultiPendulumCreateFunc),
+
+
+ //todo: create a category/tutorial about advanced topics, such as optimizations, using different collision detection algorithm, different constraint solvers etc.
+ //ExampleEntry(0,"Advanced"),
+ //ExampleEntry(1,"Obj2RigidBody Add Features", "Load a triangle mesh from Wavefront .obj and create polyhedral features to perform the separating axis test (instead of GJK/MPR). It is best to combine optimization and polyhedral feature generation.", ET_RigidBodyFromObjCreateFunc,OptimizeConvexObj+ComputePolyhedralFeatures),
+
+
};
#ifdef B3_USE_CLEW
@@ -275,17 +335,17 @@ struct ExampleEntriesInternalData
btAlignedObjectArray<ExampleEntry> m_allExamples;
};
-ExampleEntries::ExampleEntries()
+ExampleEntriesAll::ExampleEntriesAll()
{
m_data = new ExampleEntriesInternalData;
}
-ExampleEntries::~ExampleEntries()
+ExampleEntriesAll::~ExampleEntriesAll()
{
delete m_data;
}
-void ExampleEntries::initOpenCLExampleEntries()
+void ExampleEntriesAll::initOpenCLExampleEntries()
{
#ifdef B3_USE_CLEW
#ifndef NO_OPENGL3
@@ -298,7 +358,7 @@ void ExampleEntries::initOpenCLExampleEntries()
#endif //B3_USE_CLEW
}
-void ExampleEntries::initExampleEntries()
+void ExampleEntriesAll::initExampleEntries()
{
m_data->m_allExamples.clear();
@@ -306,8 +366,8 @@ void ExampleEntries::initExampleEntries()
{
m_data->m_allExamples.push_back(gAdditionalRegisteredExamples[i]);
}
-
-
+
+
int numDefaultEntries = sizeof(gDefaultExamples)/sizeof(ExampleEntry);
for (int i=0;i<numDefaultEntries;i++)
@@ -331,33 +391,33 @@ void ExampleEntries::initExampleEntries()
}
-void ExampleEntries::registerExampleEntry(int menuLevel, const char* name,const char* description, CommonExampleInterface::CreateFunc* createFunc, int option)
+void ExampleEntriesAll::registerExampleEntry(int menuLevel, const char* name,const char* description, CommonExampleInterface::CreateFunc* createFunc, int option)
{
ExampleEntry e( menuLevel,name,description, createFunc, option);
gAdditionalRegisteredExamples.push_back(e);
}
-int ExampleEntries::getNumRegisteredExamples()
+int ExampleEntriesAll::getNumRegisteredExamples()
{
return m_data->m_allExamples.size();
}
-CommonExampleInterface::CreateFunc* ExampleEntries::getExampleCreateFunc(int index)
+CommonExampleInterface::CreateFunc* ExampleEntriesAll::getExampleCreateFunc(int index)
{
return m_data->m_allExamples[index].m_createFunc;
}
-int ExampleEntries::getExampleOption(int index)
+int ExampleEntriesAll::getExampleOption(int index)
{
return m_data->m_allExamples[index].m_option;
}
-const char* ExampleEntries::getExampleName(int index)
+const char* ExampleEntriesAll::getExampleName(int index)
{
return m_data->m_allExamples[index].m_name;
}
-const char* ExampleEntries::getExampleDescription(int index)
+const char* ExampleEntriesAll::getExampleDescription(int index)
{
return m_data->m_allExamples[index].m_description;
}
diff --git a/examples/ExampleBrowser/ExampleEntries.h b/examples/ExampleBrowser/ExampleEntries.h
index f6d661a..0158863 100644
--- a/examples/ExampleBrowser/ExampleEntries.h
+++ b/examples/ExampleBrowser/ExampleEntries.h
@@ -6,32 +6,31 @@
-
-class ExampleEntries
+class ExampleEntriesAll : public ExampleEntries
{
struct ExampleEntriesInternalData* m_data;
public:
- ExampleEntries();
- virtual ~ExampleEntries();
+ ExampleEntriesAll();
+ virtual ~ExampleEntriesAll();
static void registerExampleEntry(int menuLevel, const char* name,const char* description, CommonExampleInterface::CreateFunc* createFunc, int option=0);
- void initExampleEntries();
+ virtual void initExampleEntries();
- void initOpenCLExampleEntries();
+ virtual void initOpenCLExampleEntries();
- int getNumRegisteredExamples();
+ virtual int getNumRegisteredExamples();
- CommonExampleInterface::CreateFunc* getExampleCreateFunc(int index);
+ virtual CommonExampleInterface::CreateFunc* getExampleCreateFunc(int index);
- const char* getExampleName(int index);
+ virtual const char* getExampleName(int index);
- const char* getExampleDescription(int index);
+ virtual const char* getExampleDescription(int index);
- int getExampleOption(int index);
+ virtual int getExampleOption(int index);
};
diff --git a/examples/ExampleBrowser/GwenGUISupport/GraphingTexture.cpp b/examples/ExampleBrowser/GwenGUISupport/GraphingTexture.cpp
index 4cfcc8f..7a87be8 100644
--- a/examples/ExampleBrowser/GwenGUISupport/GraphingTexture.cpp
+++ b/examples/ExampleBrowser/GwenGUISupport/GraphingTexture.cpp
@@ -21,6 +21,7 @@ void GraphingTexture::destroy()
m_height=0;
glDeleteTextures(1,(GLuint*)&m_textureId);
m_textureId=0;
+
}
bool GraphingTexture::create(int texWidth, int texHeight)
diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
index 5032a1c..449f27e 100644
--- a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
+++ b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
@@ -27,17 +27,19 @@ struct MyButtonEventHandler : public Gwen::Event::Handler
template<typename T>
struct MySliderEventHandler : public Gwen::Event::Handler
{
+ SliderParamChangedCallback m_callback;
Gwen::Controls::TextBox* m_label;
Gwen::Controls::Slider* m_pSlider;
char m_variableName[1024];
T* m_targetValue;
bool m_showValue;
- MySliderEventHandler(const char* varName, Gwen::Controls::TextBox* label, Gwen::Controls::Slider* pSlider,T* target)
+ MySliderEventHandler(const char* varName, Gwen::Controls::TextBox* label, Gwen::Controls::Slider* pSlider,T* target,SliderParamChangedCallback callback)
:m_label(label),
m_pSlider(pSlider),
m_targetValue(target),
- m_showValue(true)
+ m_showValue(true),
+ m_callback(callback)
{
memcpy(m_variableName,varName,strlen(varName)+1);
}
@@ -51,6 +53,11 @@ struct MySliderEventHandler : public Gwen::Event::Handler
T v = T(bla);
SetValue(v);
+ if (m_callback)
+ {
+ (*m_callback)(v);
+ }
+
}
void SetValue(T v)
@@ -134,7 +141,8 @@ void GwenParameterInterface::registerButtonParameter(ButtonParams& params)
MyButtonEventHandler* handler = new MyButtonEventHandler(params.m_callback,params.m_buttonId,params.m_userPointer);
button->SetText(params.m_name);
button->onPress.Add( handler, &MyButtonEventHandler::onButtonPress );
-
+ button->SetIsToggle(params.m_isTrigger);
+
m_paramInternalData->m_buttons.push_back(button);
m_paramInternalData->m_buttonEventHandlers.push_back(handler);
@@ -183,8 +191,8 @@ void GwenParameterInterface::registerComboBox(ComboBoxParams& params)
combobox->onSelection.Add(handler,&MyComboBoxHander2::onSelect);
int ypos = m_gwenInternalData->m_curYposition;
m_gwenInternalData->m_curYposition+=22;
- combobox->SetPos(10, ypos );
- combobox->SetWidth( 100 );
+ combobox->SetPos(5, ypos );
+ combobox->SetWidth( 220 );
//box->SetPos(120,130);
for (int i=0;i<params.m_numItems;i++)
{
@@ -193,6 +201,7 @@ void GwenParameterInterface::registerComboBox(ComboBoxParams& params)
combobox->OnItemSelected(item);
}
+
}
@@ -219,7 +228,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
pSlider->SetValue( *params.m_paramValuePointer);//dimensions[i] );
char labelName[1024];
sprintf(labelName,"%s",params.m_name);//axisNames[0]);
- MySliderEventHandler<btScalar>* handler = new MySliderEventHandler<btScalar>(labelName,label,pSlider,params.m_paramValuePointer);
+ MySliderEventHandler<btScalar>* handler = new MySliderEventHandler<btScalar>(labelName,label,pSlider,params.m_paramValuePointer,params.m_callback);
handler->m_showValue = params.m_showValues;
m_paramInternalData->m_sliderEventHandlers.push_back(handler);
diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp b/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp
index b10db3e..4991ee0 100644
--- a/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp
+++ b/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp
@@ -4,7 +4,7 @@
#include "LinearMath/btQuickprof.h"
-
+#ifndef BT_NO_PROFILE
class MyProfileWindow : public Gwen::Controls::WindowControl
@@ -42,9 +42,10 @@ protected:
}
public:
-
+
CProfileIterator* profIter;
+ class MyMenuItems* m_menuItems;
MyProfileWindow ( Gwen::Controls::Base* pParent)
: Gwen::Controls::WindowControl( pParent ),
profIter(0)
@@ -83,6 +84,12 @@ public:
}
+ virtual ~MyProfileWindow()
+ {
+
+ delete m_node;
+ delete m_ctrl;
+ }
float dumpRecursive(CProfileIterator* profileIterator, Gwen::Controls::TreeNode* parentNode)
{
@@ -93,7 +100,9 @@ public:
float accumulated_time=0,parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time();
int i;
int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset();
-
+ if (0==frames_since_reset)
+ return 0.f;
+
//printf("Profiling: %s (total running time: %.3f ms) ---\n", profileIterator->Get_Current_Parent_Name(), parent_time );
float totalTime = 0.f;
@@ -264,11 +273,16 @@ public:
MyProfileWindow* setupProfileWindow(GwenInternalData* data)
{
MyMenuItems* menuItems = new MyMenuItems;
+
MyProfileWindow* profWindow = new MyProfileWindow(data->pCanvas);
//profWindow->SetHidden(true);
- profWindow->profIter = CProfileManager::Get_Iterator();
+
+ profWindow->m_menuItems = menuItems;
+ //profWindow->profIter = CProfileManager::Get_Iterator();
data->m_viewMenu->GetMenu()->AddItem( L"Profiler", menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::MenuItemSelect);
+
menuItems->m_profWindow = profWindow;
+
return profWindow;
}
@@ -288,5 +302,8 @@ void profileWindowSetVisible(MyProfileWindow* window, bool visible)
}
void destroyProfileWindow(MyProfileWindow* window)
{
+ CProfileManager::Release_Iterator(window->profIter);
delete window;
}
+
+#endif //BT_NO_PROFILE
\ No newline at end of file
diff --git a/examples/ExampleBrowser/GwenGUISupport/gwenInternalData.h b/examples/ExampleBrowser/GwenGUISupport/gwenInternalData.h
index ec09825..3fc3bb4 100644
--- a/examples/ExampleBrowser/GwenGUISupport/gwenInternalData.h
+++ b/examples/ExampleBrowser/GwenGUISupport/gwenInternalData.h
@@ -45,7 +45,10 @@ struct GwenInternalData
Gwen::Controls::ListBox* m_TextOutput;
Gwen::Controls::Label* m_exampleInfoGroupBox;
Gwen::Controls::ListBox* m_exampleInfoTextOutput;
-
+ struct MyTestMenuBar* m_menubar;
+ Gwen::Controls::StatusBar* m_bar;
+ Gwen::Controls::ScrollControl* m_windowRight;
+ Gwen::Controls::TabControl* m_tab;
int m_curYposition;
diff --git a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
index a70d926..fcff139 100644
--- a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
+++ b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
@@ -17,82 +17,102 @@ GwenUserInterface::GwenUserInterface()
}
-GwenUserInterface::~GwenUserInterface()
+class MyMenuItems : public Gwen::Controls::Base
{
- for (int i=0;i<m_data->m_handlers.size();i++)
- {
- delete m_data->m_handlers[i];
- }
-
- m_data->m_handlers.clear();
-
-
- delete m_data->pCanvas;
-
-
- delete m_data;
-
-
-
-}
+public:
+ b3FileOpenCallback m_fileOpenCallback;
+ b3QuitCallback m_quitCallback;
+ MyMenuItems() :Gwen::Controls::Base(0), m_fileOpenCallback(0)
+ {
+ }
+ void myQuitApp(Gwen::Controls::Base* pControl)
+ {
+ if (m_quitCallback)
+ {
+ (*m_quitCallback)();
+ }
+ }
+ void fileOpen(Gwen::Controls::Base* pControl)
+ {
+ if (m_fileOpenCallback)
+ {
+ (*m_fileOpenCallback)();
+ }
+ }
+};
-class MyMenuItems : public Gwen::Controls::Base
-{
-public:
- b3FileOpenCallback m_fileOpenCallback;
- b3QuitCallback m_quitCallback;
-
- MyMenuItems() :Gwen::Controls::Base(0),m_fileOpenCallback(0)
- {
- }
- void myQuitApp( Gwen::Controls::Base* pControl )
- {
- if (m_quitCallback)
- {
- (*m_quitCallback)();
- }
- }
- void fileOpen( Gwen::Controls::Base* pControl )
- {
- if (m_fileOpenCallback)
- {
- (*m_fileOpenCallback)();
- }
- }
-
-};
struct MyTestMenuBar : public Gwen::Controls::MenuStrip
{
Gwen::Controls::MenuItem* m_fileMenu;
Gwen::Controls::MenuItem* m_viewMenu;
- MyMenuItems* m_menuItems;
+ MyMenuItems* m_menuItems;
MyTestMenuBar(Gwen::Controls::Base* pParent)
:Gwen::Controls::MenuStrip(pParent)
{
-// Gwen::Controls::MenuStrip* menu = new Gwen::Controls::MenuStrip( pParent );
+ // Gwen::Controls::MenuStrip* menu = new Gwen::Controls::MenuStrip( pParent );
{
m_menuItems = new MyMenuItems();
- m_menuItems->m_fileOpenCallback = 0;
- m_menuItems->m_quitCallback = 0;
-
- m_fileMenu = AddItem( L"File" );
-
- m_fileMenu->GetMenu()->AddItem(L"Open",m_menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::fileOpen);
- m_fileMenu->GetMenu()->AddItem(L"Quit",m_menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::myQuitApp);
- m_viewMenu = AddItem( L"View" );
-
+ m_menuItems->m_fileOpenCallback = 0;
+ m_menuItems->m_quitCallback = 0;
+
+ m_fileMenu = AddItem(L"File");
+
+ m_fileMenu->GetMenu()->AddItem(L"Open", m_menuItems, (Gwen::Event::Handler::Function)&MyMenuItems::fileOpen);
+ m_fileMenu->GetMenu()->AddItem(L"Quit", m_menuItems, (Gwen::Event::Handler::Function)&MyMenuItems::myQuitApp);
+ m_viewMenu = AddItem(L"View");
+
}
}
+ virtual ~MyTestMenuBar()
+ {
+ delete m_menuItems;
+ }
};
+
+void GwenUserInterface::exit()
+{
+ //m_data->m_menubar->RemoveAllChildren();
+ delete m_data->m_tab;
+ delete m_data->m_windowRight;
+ delete m_data->m_leftStatusBar;
+ delete m_data->m_TextOutput;
+ delete m_data->m_rightStatusBar;
+ delete m_data->m_bar;
+ delete m_data->m_menubar;
+
+ m_data->m_menubar = 0;
+ delete m_data->pCanvas;
+ m_data->pCanvas = 0;
+}
+
+GwenUserInterface::~GwenUserInterface()
+{
+ for (int i=0;i<m_data->m_handlers.size();i++)
+ {
+ delete m_data->m_handlers[i];
+ }
+
+ m_data->m_handlers.clear();
+
+
+ delete m_data;
+
+}
+
+
+
+
+
+
void GwenUserInterface::resize(int width, int height)
{
m_data->pCanvas->SetSize(width,height);
@@ -232,6 +252,7 @@ void GwenUserInterface::setStatusBarMessage(const char* message, bool isLeft)
}
}
+
void GwenUserInterface::registerFileOpenCallback(b3FileOpenCallback callback)
{
m_data->m_menuItems->m_fileOpenCallback = callback;
@@ -249,35 +270,43 @@ void GwenUserInterface::init(int width, int height,Gwen::Renderer::Base* rendere
m_data->pRenderer = renderer;//new GwenOpenGL3CoreRenderer(m_data->m_primRenderer,stash,width,height,retinaScale);
m_data->skin.SetRender( m_data->pRenderer );
+
m_data->pCanvas= new Gwen::Controls::Canvas( &m_data->skin );
m_data->pCanvas->SetSize( width,height);
m_data->pCanvas->SetDrawBackground( false);
m_data->pCanvas->SetBackgroundColor( Gwen::Color( 150, 170, 170, 255 ) );
-
-
-
+
+
+
MyTestMenuBar* menubar = new MyTestMenuBar(m_data->pCanvas);
m_data->m_viewMenu = menubar->m_viewMenu;
m_data->m_menuItems = menubar->m_menuItems;
-
+ m_data->m_menubar = menubar;
+
-
Gwen::Controls::StatusBar* bar = new Gwen::Controls::StatusBar(m_data->pCanvas);
+ m_data->m_bar = bar;
+
+
m_data->m_rightStatusBar = new Gwen::Controls::Label( bar );
+
m_data->m_rightStatusBar->SetWidth(width/2);
//m_data->m_rightStatusBar->SetText( L"Label Added to Right" );
bar->AddControl( m_data->m_rightStatusBar, true );
-
+
m_data->m_TextOutput = new Gwen::Controls::ListBox( m_data->pCanvas );
+
m_data->m_TextOutput->Dock( Gwen::Pos::Bottom );
m_data->m_TextOutput->SetHeight( 100 );
-
+
m_data->m_leftStatusBar = new Gwen::Controls::Label( bar );
+
//m_data->m_leftStatusBar->SetText( L"Label Added to Left" );
m_data->m_leftStatusBar->SetWidth(width/2);
bar->AddControl( m_data->m_leftStatusBar,false);
+
//Gwen::KeyboardFocus
/*Gwen::Controls::GroupBox* box = new Gwen::Controls::GroupBox(m_data->pCanvas);
box->SetText("text");
@@ -289,11 +318,14 @@ void GwenUserInterface::init(int width, int height,Gwen::Renderer::Base* rendere
windowRight->SetWidth(250);
windowRight->SetHeight(250);
windowRight->SetScroll(false,true);
-
+ m_data->m_windowRight = windowRight;
//windowLeft->SetSkin(
Gwen::Controls::TabControl* tab = new Gwen::Controls::TabControl(windowRight);
+ m_data->m_tab = tab;
+
+
//tab->SetHeight(300);
tab->SetWidth(240);
@@ -304,7 +336,8 @@ void GwenUserInterface::init(int width, int height,Gwen::Renderer::Base* rendere
Gwen::UnicodeString str1(L"Params");
m_data->m_demoPage = tab->AddPage(str1);
-
+
+
@@ -348,7 +381,7 @@ void GwenUserInterface::init(int width, int height,Gwen::Renderer::Base* rendere
//windowLeft->SetClosable(false);
// windowLeft->SetShouldDrawBackground(true);
windowLeft->SetTabable(true);
-
+
Gwen::Controls::TabControl* explorerTab = new Gwen::Controls::TabControl(windowLeft);
//tab->SetHeight(300);
@@ -388,7 +421,6 @@ void GwenUserInterface::init(int width, int height,Gwen::Renderer::Base* rendere
m_data->m_exampleInfoGroupBox->SetText("Example Description");
m_data->m_exampleInfoTextOutput = new Gwen::Controls::ListBox(m_data->m_explorerPage->GetPage());
-
//m_data->m_exampleInfoTextOutput->Dock( Gwen::Pos::Bottom );
m_data->m_exampleInfoTextOutput->SetPos(2, 332);
diff --git a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.h b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.h
index c448e9f..d66b598 100644
--- a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.h
+++ b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.h
@@ -26,6 +26,7 @@ class GwenUserInterface
virtual ~GwenUserInterface();
void init(int width, int height,Gwen::Renderer::Base* gwenRenderer,float retinaScale);
+ void exit();
void setFocus();
void forceUpdateScrollBars();
diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp
new file mode 100644
index 0000000..4924a5d
--- /dev/null
+++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp
@@ -0,0 +1,422 @@
+#include "InProcessExampleBrowser.h"
+
+//#define EXAMPLE_CONSOLE_ONLY
+#ifdef EXAMPLE_CONSOLE_ONLY
+ #include "EmptyBrowser.h"
+ typedef EmptyBrowser DefaultBrowser;
+#else
+ #include "OpenGLExampleBrowser.h"
+ typedef OpenGLExampleBrowser DefaultBrowser;
+#endif //EXAMPLE_CONSOLE_ONLY
+
+#include "Bullet3Common/b3CommandLineArgs.h"
+#include "../Utils/b3Clock.h"
+
+#include "ExampleEntries.h"
+#include "Bullet3Common/b3Scalar.h"
+#include "../SharedMemory/InProcessMemory.h"
+
+void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory);
+void* ExampleBrowserMemoryFunc();
+
+#include <stdio.h>
+//#include "BulletMultiThreaded/PlatformDefinitions.h"
+
+#include "Bullet3Common/b3Logging.h"
+#include "ExampleEntries.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "EmptyExample.h"
+
+#include "../SharedMemory/PhysicsServerExample.h"
+#include "../SharedMemory/PhysicsClientExample.h"
+
+#ifndef _WIN32
+#include "../MultiThreading/b3PosixThreadSupport.h"
+
+
+
+static b3ThreadSupportInterface* createExampleBrowserThreadSupport(int numThreads)
+{
+ b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("testThreads",
+ ExampleBrowserThreadFunc,
+ ExampleBrowserMemoryFunc,
+ numThreads);
+ b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
+
+ return threadSupport;
+
+}
+
+
+
+#elif defined( _WIN32)
+#include "../MultiThreading/b3Win32ThreadSupport.h"
+
+b3ThreadSupportInterface* createExampleBrowserThreadSupport(int numThreads)
+{
+ b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("testThreads",ExampleBrowserThreadFunc,ExampleBrowserMemoryFunc,numThreads);
+ b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
+ return threadSupport;
+
+}
+#endif
+
+
+
+
+
+class ExampleEntriesPhysicsServer : public ExampleEntries
+{
+
+ struct ExampleEntriesInternalData2* m_data;
+
+public:
+
+ ExampleEntriesPhysicsServer();
+ virtual ~ExampleEntriesPhysicsServer();
+
+ static void registerExampleEntry(int menuLevel, const char* name,const char* description, CommonExampleInterface::CreateFunc* createFunc, int option=0);
+
+ virtual void initExampleEntries();
+
+ virtual void initOpenCLExampleEntries();
+
+ virtual int getNumRegisteredExamples();
+
+ virtual CommonExampleInterface::CreateFunc* getExampleCreateFunc(int index);
+
+ virtual const char* getExampleName(int index);
+
+ virtual const char* getExampleDescription(int index);
+
+ virtual int getExampleOption(int index);
+
+};
+
+
+struct ExampleEntryPhysicsServer
+{
+ int m_menuLevel;
+ const char* m_name;
+ const char* m_description;
+ CommonExampleInterface::CreateFunc* m_createFunc;
+ int m_option;
+
+ ExampleEntryPhysicsServer(int menuLevel, const char* name)
+ :m_menuLevel(menuLevel), m_name(name), m_description(0), m_createFunc(0), m_option(0)
+ {
+ }
+
+ ExampleEntryPhysicsServer(int menuLevel, const char* name,const char* description, CommonExampleInterface::CreateFunc* createFunc, int option=0)
+ :m_menuLevel(menuLevel), m_name(name), m_description(description), m_createFunc(createFunc), m_option(option)
+ {
+ }
+};
+
+struct ExampleEntriesInternalData2
+{
+ btAlignedObjectArray<ExampleEntryPhysicsServer> m_allExamples;
+};
+
+static ExampleEntryPhysicsServer gDefaultExamplesPhysicsServer[]=
+{
+
+ ExampleEntryPhysicsServer(0,"Robotics Control"),
+
+ ExampleEntryPhysicsServer(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory",
+ PhysicsServerCreateFunc),
+ ExampleEntryPhysicsServer(1,"Physics Server (RTC)", "Create a physics server that communicates with a physics client over shared memory. At each update, the Physics Server will continue calling 'stepSimulation' based on the real-time clock (RTC).",
+ PhysicsServerCreateFunc,PHYSICS_SERVER_USE_RTC_CLOCK),
+
+ ExampleEntryPhysicsServer(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.",
+ PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING),
+ ExampleEntryPhysicsServer(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
+ PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
+
+
+};
+
+
+ExampleEntriesPhysicsServer::ExampleEntriesPhysicsServer()
+{
+ m_data = new ExampleEntriesInternalData2;
+}
+
+ExampleEntriesPhysicsServer::~ExampleEntriesPhysicsServer()
+{
+ delete m_data;
+}
+
+void ExampleEntriesPhysicsServer::initOpenCLExampleEntries()
+{
+}
+
+void ExampleEntriesPhysicsServer::initExampleEntries()
+{
+ m_data->m_allExamples.clear();
+
+
+
+ int numDefaultEntries = sizeof(gDefaultExamplesPhysicsServer)/sizeof(ExampleEntryPhysicsServer);
+ for (int i=0;i<numDefaultEntries;i++)
+ {
+ m_data->m_allExamples.push_back(gDefaultExamplesPhysicsServer[i]);
+ }
+
+}
+
+void ExampleEntriesPhysicsServer::registerExampleEntry(int menuLevel, const char* name,const char* description, CommonExampleInterface::CreateFunc* createFunc, int option)
+{
+}
+
+int ExampleEntriesPhysicsServer::getNumRegisteredExamples()
+{
+ return m_data->m_allExamples.size();
+}
+
+CommonExampleInterface::CreateFunc* ExampleEntriesPhysicsServer::getExampleCreateFunc(int index)
+{
+ return m_data->m_allExamples[index].m_createFunc;
+}
+
+int ExampleEntriesPhysicsServer::getExampleOption(int index)
+{
+ return m_data->m_allExamples[index].m_option;
+}
+
+const char* ExampleEntriesPhysicsServer::getExampleName(int index)
+{
+ return m_data->m_allExamples[index].m_name;
+}
+
+const char* ExampleEntriesPhysicsServer::getExampleDescription(int index)
+{
+ return m_data->m_allExamples[index].m_description;
+}
+
+
+
+
+struct ExampleBrowserArgs
+{
+ ExampleBrowserArgs()
+ :m_fakeWork(1),m_argc(0)
+ {
+ }
+ b3CriticalSection* m_cs;
+ float m_fakeWork;
+ int m_argc;
+ char** m_argv;
+};
+
+struct ExampleBrowserThreadLocalStorage
+{
+ SharedMemoryInterface* m_sharedMem;
+ int threadId;
+};
+
+enum TestExampleBrowserCommunicationEnums
+{
+ eRequestTerminateExampleBrowser = 13,
+ eExampleBrowserIsUnInitialized,
+ eExampleBrowserIsInitialized,
+ eExampleBrowserInitializationFailed,
+ eExampleBrowserHasTerminated
+};
+
+void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
+{
+ printf("ExampleBrowserThreadFunc started\n");
+
+ ExampleBrowserThreadLocalStorage* localStorage = (ExampleBrowserThreadLocalStorage*) lsMemory;
+
+ ExampleBrowserArgs* args = (ExampleBrowserArgs*) userPtr;
+ int workLeft = true;
+ b3CommandLineArgs args2(args->m_argc,args->m_argv);
+ b3Clock clock;
+
+
+ ExampleEntriesPhysicsServer examples;
+ examples.initExampleEntries();
+
+ DefaultBrowser* exampleBrowser = new DefaultBrowser(&examples);
+ exampleBrowser->setSharedMemoryInterface(localStorage->m_sharedMem);
+
+ bool init = exampleBrowser->init(args->m_argc,args->m_argv);
+ clock.reset();
+ if (init)
+ {
+
+ args->m_cs->lock();
+ args->m_cs->setSharedParam(0,eExampleBrowserIsInitialized);
+ args->m_cs->unlock();
+
+ do
+ {
+ float deltaTimeInSeconds = clock.getTimeMicroseconds()/1000000.f;
+ clock.reset();
+ exampleBrowser->update(deltaTimeInSeconds);
+
+ } while (!exampleBrowser->requestedExit() && (args->m_cs->getSharedParam(0)!=eRequestTerminateExampleBrowser));
+ } else
+ {
+ args->m_cs->lock();
+ args->m_cs->setSharedParam(0,eExampleBrowserInitializationFailed);
+ args->m_cs->unlock();
+ }
+
+ delete exampleBrowser;
+ args->m_cs->lock();
+ args->m_cs->setSharedParam(0,eExampleBrowserHasTerminated);
+ args->m_cs->unlock();
+ printf("finished\n");
+ //do nothing
+}
+
+
+void* ExampleBrowserMemoryFunc()
+{
+ //don't create local store memory, just return 0
+ return new ExampleBrowserThreadLocalStorage;
+}
+
+
+
+
+
+struct btInProcessExampleBrowserInternalData
+{
+ ExampleBrowserArgs m_args;
+ b3ThreadSupportInterface* m_threadSupport;
+ SharedMemoryInterface* m_sharedMem;
+};
+
+
+
+btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc,char** argv2)
+{
+
+ btInProcessExampleBrowserInternalData* data = new btInProcessExampleBrowserInternalData;
+ data->m_sharedMem = new InProcessMemory;
+
+ int numThreads = 1;
+ int i;
+
+ data->m_threadSupport = createExampleBrowserThreadSupport(numThreads);
+
+ printf("argc=%d\n", argc);
+ for (i=0;i<argc;i++)
+ {
+ printf("argv[%d] = %s\n",i,argv2[i]);
+ }
+
+ for (i=0;i<data->m_threadSupport->getNumTasks();i++)
+ {
+ ExampleBrowserThreadLocalStorage* storage = (ExampleBrowserThreadLocalStorage*) data->m_threadSupport->getThreadLocalMemory(i);
+ b3Assert(storage);
+ storage->threadId = i;
+ storage->m_sharedMem = data->m_sharedMem;
+ }
+
+
+ data->m_args.m_cs = data->m_threadSupport->createCriticalSection();
+ data->m_args.m_cs->setSharedParam(0,eExampleBrowserIsUnInitialized);
+ data->m_args.m_argc = argc;
+ data->m_args.m_argv = argv2;
+
+
+ for (i=0;i<numThreads;i++)
+ {
+ data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &data->m_args, i);
+ }
+
+ while (data->m_args.m_cs->getSharedParam(0)==eExampleBrowserIsUnInitialized)
+ {
+ b3Clock::usleep(1000);
+ }
+
+ return data;
+}
+
+bool btIsExampleBrowserTerminated(btInProcessExampleBrowserInternalData* data)
+{
+ return (data->m_args.m_cs->getSharedParam(0)==eExampleBrowserHasTerminated);
+}
+
+SharedMemoryInterface* btGetSharedMemoryInterface(btInProcessExampleBrowserInternalData* data)
+{
+ return data->m_sharedMem;
+}
+
+void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data)
+{
+ int numActiveThreads = 1;
+
+ data->m_args.m_cs->lock();
+ data->m_args.m_cs->setSharedParam(0,eRequestTerminateExampleBrowser);
+ data->m_args.m_cs->unlock();
+
+ while (numActiveThreads)
+ {
+ int arg0,arg1;
+ if (data->m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
+ {
+ numActiveThreads--;
+ printf("numActiveThreads = %d\n",numActiveThreads);
+
+ } else
+ {
+// printf("polling..");
+ b3Clock::usleep(1000);
+ }
+ };
+
+ printf("btShutDownExampleBrowser stopping threads\n");
+ delete data->m_threadSupport;
+ delete data->m_sharedMem;
+ delete data;
+}
+
+struct btInProcessExampleBrowserMainThreadInternalData
+{
+ ExampleEntriesPhysicsServer m_examples;
+ DefaultBrowser* m_exampleBrowser;
+ SharedMemoryInterface* m_sharedMem;
+ b3Clock m_clock;
+};
+
+btInProcessExampleBrowserMainThreadInternalData* btCreateInProcessExampleBrowserMainThread(int argc,char** argv)
+{
+ btInProcessExampleBrowserMainThreadInternalData* data = new btInProcessExampleBrowserMainThreadInternalData;
+ data->m_examples.initExampleEntries();
+ data->m_exampleBrowser = new DefaultBrowser(&data->m_examples);
+ data->m_sharedMem = new InProcessMemory;
+ data->m_exampleBrowser->setSharedMemoryInterface(data->m_sharedMem );
+ bool init = data->m_exampleBrowser->init(argc,argv);
+ data->m_clock.reset();
+ return data;
+}
+
+bool btIsExampleBrowserMainThreadTerminated(btInProcessExampleBrowserMainThreadInternalData* data)
+{
+ return data->m_exampleBrowser->requestedExit();
+}
+
+void btUpdateInProcessExampleBrowserMainThread(btInProcessExampleBrowserMainThreadInternalData* data)
+{
+ float deltaTimeInSeconds = data->m_clock.getTimeMicroseconds()/1000000.f;
+ data->m_clock.reset();
+ data->m_exampleBrowser->update(deltaTimeInSeconds);
+}
+void btShutDownExampleBrowserMainThread(btInProcessExampleBrowserMainThreadInternalData* data)
+{
+
+ data->m_exampleBrowser->setSharedMemoryInterface(0);
+ delete data->m_exampleBrowser;
+ delete data;
+}
+
+class SharedMemoryInterface* btGetSharedMemoryInterfaceMainThread(btInProcessExampleBrowserMainThreadInternalData* data)
+{
+ return data->m_sharedMem;
+}
diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.h b/examples/ExampleBrowser/InProcessExampleBrowser.h
new file mode 100644
index 0000000..e01d3fd
--- /dev/null
+++ b/examples/ExampleBrowser/InProcessExampleBrowser.h
@@ -0,0 +1,33 @@
+#ifndef IN_PROCESS_EXAMPLE_BROWSER_H
+#define IN_PROCESS_EXAMPLE_BROWSER_H
+
+struct btInProcessExampleBrowserInternalData;
+
+btInProcessExampleBrowserInternalData* btCreateInProcessExampleBrowser(int argc,char** argv2);
+
+bool btIsExampleBrowserTerminated(btInProcessExampleBrowserInternalData* data);
+
+void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data);
+
+class SharedMemoryInterface* btGetSharedMemoryInterface(btInProcessExampleBrowserInternalData* data);
+
+
+///////////////////////
+
+
+struct btInProcessExampleBrowserMainThreadInternalData;
+
+btInProcessExampleBrowserMainThreadInternalData* btCreateInProcessExampleBrowserMainThread(int argc,char** argv2);
+
+bool btIsExampleBrowserMainThreadTerminated(btInProcessExampleBrowserMainThreadInternalData* data);
+
+void btUpdateInProcessExampleBrowserMainThread(btInProcessExampleBrowserMainThreadInternalData* data);
+
+void btShutDownExampleBrowserMainThread(btInProcessExampleBrowserMainThreadInternalData* data);
+
+class SharedMemoryInterface* btGetSharedMemoryInterfaceMainThread(btInProcessExampleBrowserMainThreadInternalData* data);
+
+
+//////////////////////
+
+#endif //IN_PROCESS_EXAMPLE_BROWSER_H
diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
index bc33563..e9f09b6 100644
--- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
+++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
@@ -42,25 +42,72 @@
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
#include "../Importers/ImportBullet/SerializeSetup.h"
+#include "Bullet3Common/b3HashMap.h"
+
+struct GL3TexLoader : public MyTextureLoader
+{
+ b3HashMap<b3HashString, GLint> m_hashMap;
+
+ virtual void LoadTexture(Gwen::Texture* pTexture)
+ {
+ Gwen::String namestr = pTexture->name.Get();
+ const char* n = namestr.c_str();
+ GLint* texIdPtr = m_hashMap[n];
+ if (texIdPtr)
+ {
+ pTexture->m_intData = *texIdPtr;
+ }
+ }
+ virtual void FreeTexture(Gwen::Texture* pTexture)
+ {
+ }
+};
+
+
+struct OpenGLExampleBrowserInternalData
+{
+ Gwen::Renderer::Base* m_gwenRenderer;
+ CommonGraphicsApp* m_app;
+// MyProfileWindow* m_profWindow;
+ btAlignedObjectArray<Gwen::Controls::TreeNode*> m_nodes;
+ GwenUserInterface* m_gui;
+ GL3TexLoader* m_myTexLoader;
+ struct MyMenuItemHander* m_handler2;
+ btAlignedObjectArray<MyMenuItemHander*> m_handlers;
+
+ OpenGLExampleBrowserInternalData()
+ : m_gwenRenderer(0),
+ m_app(0),
+// m_profWindow(0),
+ m_gui(0),
+ m_myTexLoader(0),
+ m_handler2(0)
+ {
+
+ }
+};
+
static CommonGraphicsApp* s_app=0;
static CommonWindowInterface* s_window = 0;
static CommonParameterInterface* s_parameterInterface=0;
static CommonRenderInterface* s_instancingRenderer=0;
static OpenGLGuiHelper* s_guiHelper=0;
-static MyProfileWindow* s_profWindow =0;
+//static MyProfileWindow* s_profWindow =0;
+static SharedMemoryInterface* sSharedMem = 0;
#define DEMO_SELECTION_COMBOBOX 13
const char* startFileName = "0_Bullet3Demo.txt";
char staticPngFileName[1024];
-static GwenUserInterface* gui = 0;
+//static GwenUserInterface* gui = 0;
+static GwenUserInterface* gui2 = 0;
static int sCurrentDemoIndex = -1;
static int sCurrentHightlighted = 0;
static CommonExampleInterface* sCurrentDemo = 0;
static b3AlignedObjectArray<const char*> allNames;
static float gFixedTimeStep = 0;
bool gAllowRetina = true;
-
+bool gDisableDemoSelection = false;
static class ExampleEntries* gAllExamples=0;
bool sUseOpenGL2 = false;
bool drawGUI=true;
@@ -76,6 +123,7 @@ static bool enable_experimental_opencl = false;
int gDebugDrawFlags = 0;
static bool pauseSimulation=false;
+static bool singleStepSimulation = false;
int midiBaseIndex = 176;
extern bool gDisableDeactivation;
@@ -110,6 +158,7 @@ void deleteDemo()
}
const char* gPngFileName = 0;
+int gPngSkipFrames = 0;
@@ -122,9 +171,9 @@ void MyKeyboardCallback(int key, int state)
//b3Printf("key=%d, state=%d", key, state);
bool handled = false;
- if (gui && !handled )
+ if (gui2 && !handled )
{
- handled = gui->keyboardCallback(key, state);
+ handled = gui2->keyboardCallback(key, state);
}
if (!handled && sCurrentDemo)
@@ -145,7 +194,6 @@ void MyKeyboardCallback(int key, int state)
}
if (key=='c' && state)
{
- gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraints;
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawContactPoints;
}
if (key == 'd' && state)
@@ -153,6 +201,11 @@ void MyKeyboardCallback(int key, int state)
gDebugDrawFlags ^= btIDebugDraw::DBG_NoDeactivation;
gDisableDeactivation = ((gDebugDrawFlags & btIDebugDraw::DBG_NoDeactivation) != 0);
}
+ if (key == 'k' && state)
+ {
+ gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraints;
+ }
+
if (key=='l' && state)
{
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraintLimits;
@@ -179,6 +232,12 @@ void MyKeyboardCallback(int key, int state)
{
pauseSimulation = !pauseSimulation;
}
+ if (key == 'o' && state)
+ {
+ singleStepSimulation = true;
+ }
+
+
#ifndef NO_OPENGL3
if (key=='s' && state)
{
@@ -222,11 +281,11 @@ void MyKeyboardCallback(int key, int state)
b3MouseMoveCallback prevMouseMoveCallback = 0;
static void MyMouseMoveCallback( float x, float y)
{
- bool handled = false;
+ bool handled = false;
if (sCurrentDemo)
handled = sCurrentDemo->mouseMoveCallback(x,y);
- if (!handled && gui)
- handled = gui->mouseMoveCallback(x,y);
+ if (!handled && gui2)
+ handled = gui2->mouseMoveCallback(x,y);
if (!handled)
{
if (prevMouseMoveCallback)
@@ -243,8 +302,8 @@ static void MyMouseButtonCallback(int button, int state, float x, float y)
if (sCurrentDemo)
handled = sCurrentDemo->mouseButtonCallback(button,state,x,y);
- if (!handled && gui)
- handled = gui->mouseButtonCallback(button,state,x,y);
+ if (!handled && gui2)
+ handled = gui2->mouseButtonCallback(button,state,x,y);
if (!handled)
{
@@ -255,6 +314,21 @@ static void MyMouseButtonCallback(int button, int state, float x, float y)
}
#include <string.h>
+struct FileImporterByExtension
+{
+ std::string m_extension;
+ CommonExampleInterface::CreateFunc* m_createFunc;
+};
+
+static btAlignedObjectArray<FileImporterByExtension> gFileImporterByExtension;
+
+void OpenGLExampleBrowser::registerFileImporter(const char* extension, CommonExampleInterface::CreateFunc* createFunc)
+{
+ FileImporterByExtension fi;
+ fi.m_extension = extension;
+ fi.m_createFunc = createFunc;
+ gFileImporterByExtension.push_back(fi);
+}
void openFileDemo(const char* filename)
{
@@ -278,20 +352,15 @@ void openFileDemo(const char* filename)
char fullPath[1024];
sprintf(fullPath, "%s", filename);
b3FileUtils::toLower(fullPath);
- if (strstr(fullPath, ".urdf"))
- {
- sCurrentDemo = ImportURDFCreateFunc(options);
- } else
- {
- if (strstr(fullPath, ".bullet"))
- {
- sCurrentDemo = SerializeBulletCreateFunc(options);
- }
- }
+
+ for (int i=0;i<gFileImporterByExtension.size();i++)
+ {
+ if (strstr(fullPath, gFileImporterByExtension[i].m_extension.c_str()))
+ {
+ sCurrentDemo = gFileImporterByExtension[i].m_createFunc(options);
+ }
+ }
-
- //physicsSetup->setFileName(filename);
-
if (sCurrentDemo)
{
@@ -322,19 +391,26 @@ void selectDemo(int demoIndex)
CommonExampleInterface::CreateFunc* func = gAllExamples->getExampleCreateFunc(demoIndex);
if (func)
{
- s_parameterInterface->removeAllParameters();
+ if (s_parameterInterface)
+ {
+ s_parameterInterface->removeAllParameters();
+ }
int option = gAllExamples->getExampleOption(demoIndex);
s_guiHelper= new OpenGLGuiHelper(s_app, sUseOpenGL2);
CommonExampleOptions options(s_guiHelper, option);
+ options.m_sharedMem = sSharedMem;
sCurrentDemo = (*func)(options);
if (sCurrentDemo)
{
- if (gui)
+ if (gui2)
{
- gui->setStatusBarMessage("Status: OK", false);
+ gui2->setStatusBarMessage("Status: OK", false);
}
b3Printf("Selected demo: %s",gAllExamples->getExampleName(demoIndex));
- gui->setExampleDescription(gAllExamples->getExampleDescription(demoIndex));
+ if (gui2)
+ {
+ gui2->setExampleDescription(gAllExamples->getExampleDescription(demoIndex));
+ }
sCurrentDemo->initPhysics();
if(resetCamera)
@@ -387,7 +463,6 @@ static void loadCurrentSettings(const char* startFileName, b3CommandLineArgs& ar
FILE* f = fopen(startFileName,"r");
if (f)
{
- int result;
char oneline[1024];
char* argv[] = {0,&oneline[0]};
@@ -426,10 +501,10 @@ void MyComboBoxCallback(int comboId, const char* item)
void MyGuiPrintf(const char* msg)
{
printf("b3Printf: %s\n",msg);
- if (gui)
+ if (!gDisableDemoSelection)
{
- gui->textOutput(msg);
- gui->forceUpdateScrollBars();
+ gui2->textOutput(msg);
+ gui2->forceUpdateScrollBars();
}
}
@@ -438,10 +513,10 @@ void MyGuiPrintf(const char* msg)
void MyStatusBarPrintf(const char* msg)
{
printf("b3Printf: %s\n", msg);
- if (gui)
+ if (!gDisableDemoSelection)
{
bool isLeft = true;
- gui->setStatusBarMessage(msg,isLeft);
+ gui2->setStatusBarMessage(msg,isLeft);
}
}
@@ -449,13 +524,15 @@ void MyStatusBarPrintf(const char* msg)
void MyStatusBarError(const char* msg)
{
printf("Warning: %s\n", msg);
- if (gui)
+ if (!gDisableDemoSelection)
{
bool isLeft = false;
- gui->setStatusBarMessage(msg,isLeft);
- gui->textOutput(msg);
- gui->forceUpdateScrollBars();
+ gui2->setStatusBarMessage(msg,isLeft);
+ gui2->textOutput(msg);
+ gui2->forceUpdateScrollBars();
}
+ btAssert(0);
+
}
struct MyMenuItemHander :public Gwen::Event::Handler
@@ -490,9 +567,11 @@ struct MyMenuItemHander :public Gwen::Event::Handler
Gwen::String laa = Gwen::Utility::UnicodeToString(la);
//const char* ha = laa.c_str();
-
- selectDemo(sCurrentHightlighted);
- saveCurrentSettings(sCurrentDemoIndex, startFileName);
+ if (!gDisableDemoSelection )
+ {
+ selectDemo(sCurrentHightlighted);
+ saveCurrentSettings(sCurrentDemoIndex, startFileName);
+ }
}
void onButtonC(Gwen::Controls::Base* pControl)
{
@@ -514,8 +593,11 @@ struct MyMenuItemHander :public Gwen::Event::Handler
*/
// printf("onKeyReturn ! \n");
- selectDemo(sCurrentHightlighted);
- saveCurrentSettings(sCurrentDemoIndex, startFileName);
+ if (!gDisableDemoSelection )
+ {
+ selectDemo(sCurrentHightlighted);
+ saveCurrentSettings(sCurrentDemoIndex, startFileName);
+ }
}
@@ -523,7 +605,7 @@ struct MyMenuItemHander :public Gwen::Event::Handler
{
// printf("select %d\n",m_buttonId);
sCurrentHightlighted = m_buttonId;
- gui->setExampleDescription(gAllExamples->getExampleDescription(sCurrentHightlighted));
+ gui2->setExampleDescription(gAllExamples->getExampleDescription(sCurrentHightlighted));
}
void onButtonF(Gwen::Controls::Base* pControl)
@@ -539,26 +621,6 @@ struct MyMenuItemHander :public Gwen::Event::Handler
};
-#include "Bullet3Common/b3HashMap.h"
-
-struct GL3TexLoader : public MyTextureLoader
-{
- b3HashMap<b3HashString,GLint> m_hashMap;
-
- virtual void LoadTexture( Gwen::Texture* pTexture )
- {
- Gwen::String namestr = pTexture->name.Get();
- const char* n = namestr.c_str();
- GLint* texIdPtr = m_hashMap[n];
- if (texIdPtr)
- {
- pTexture->m_intData = *texIdPtr;
- }
- }
- virtual void FreeTexture( Gwen::Texture* pTexture )
- {
- }
-};
void quitCallback()
{
@@ -588,10 +650,12 @@ struct QuickCanvas : public Common2dCanvasInterface
MyGraphWindow* m_gw[MAX_GRAPH_WINDOWS];
GraphingTexture* m_gt[MAX_GRAPH_WINDOWS];
int m_curNumGraphWindows;
+ int m_curXpos;
QuickCanvas(GL3TexLoader* myTexLoader)
:m_myTexLoader(myTexLoader),
- m_curNumGraphWindows(0)
+ m_curNumGraphWindows(0),
+ m_curXpos(0)
{
for (int i=0;i<MAX_GRAPH_WINDOWS;i++)
{
@@ -612,10 +676,11 @@ struct QuickCanvas : public Common2dCanvasInterface
m_curNumGraphWindows++;
- MyGraphInput input(gui->getInternalData());
+ MyGraphInput input(gui2->getInternalData());
input.m_width=width;
input.m_height=height;
- input.m_xPos = 10000;//GUI will clamp it to the right//300;
+ input.m_xPos = m_curXpos;//GUI will clamp it to the right//300;
+ m_curXpos+=width+20;
input.m_yPos = 10000;//GUI will clamp it to bottom
input.m_name=canvasName;
input.m_texName = canvasName;
@@ -631,8 +696,12 @@ struct QuickCanvas : public Common2dCanvasInterface
}
virtual void destroyCanvas(int canvasId)
{
+ m_curXpos = 0;
btAssert(canvasId>=0);
+ delete m_gt[canvasId];
+ m_gt[canvasId] = 0;
destroyTextureWindow(m_gw[canvasId]);
+ m_gw[canvasId] = 0;
m_curNumGraphWindows--;
}
virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha)
@@ -658,12 +727,51 @@ struct QuickCanvas : public Common2dCanvasInterface
OpenGLExampleBrowser::OpenGLExampleBrowser(class ExampleEntries* examples)
{
+ m_internalData = new OpenGLExampleBrowserInternalData;
+
gAllExamples = examples;
}
OpenGLExampleBrowser::~OpenGLExampleBrowser()
{
- deleteDemo();
+ deleteDemo();
+ for (int i = 0; i < m_internalData->m_nodes.size(); i++)
+ {
+ delete m_internalData->m_nodes[i];
+ }
+ delete m_internalData->m_handler2;
+ for (int i = 0; i < m_internalData->m_handlers.size(); i++)
+ {
+ delete m_internalData->m_handlers[i];
+ }
+ m_internalData->m_handlers.clear();
+ m_internalData->m_nodes.clear();
+ delete s_parameterInterface;
+ s_parameterInterface = 0;
+ delete s_app->m_2dCanvasInterface;
+ s_app->m_2dCanvasInterface = 0;
+
+ m_internalData->m_gui->exit();
+
+
+ delete m_internalData->m_gui;
+ delete m_internalData->m_gwenRenderer;
+ delete m_internalData->m_myTexLoader;
+
+
+
+
+
+ delete m_internalData->m_app;
+ s_app = 0;
+
+
+
+// delete m_internalData->m_profWindow;
+
+ delete m_internalData;
+
+ gFileImporterByExtension.clear();
gAllExamples = 0;
}
@@ -676,7 +784,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
loadCurrentSettings(startFileName, args);
args.GetCmdLineArgument("fixed_timestep",gFixedTimeStep);
-
+ args.GetCmdLineArgument("png_skip_frames", gPngSkipFrames);
///The OpenCL rigid body pipeline is experimental and
///most OpenCL drivers and OpenCL compilers have issues with our kernels.
///If you have a high-end desktop GPU such as AMD 7970 or better, or NVIDIA GTX 680 with up-to-date drivers
@@ -715,6 +823,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
s_app = new SimpleOpenGL2App(title,width,height);
s_app->m_renderer = new SimpleOpenGL2Renderer(width,height);
}
+
#ifndef NO_OPENGL3
else
{
@@ -724,6 +833,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
s_app = simpleApp;
}
#endif
+ m_internalData->m_app = s_app;
char* gVideoFileName = 0;
args.GetCmdLineArgument("mp4",gVideoFileName);
#ifndef NO_OPENGL3
@@ -732,7 +842,11 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
#endif
s_instancingRenderer = s_app->m_renderer;
- s_window = s_app->m_window;
+ s_window = s_app->m_window;
+
+ width = s_window->getWidth();
+ height = s_window->getHeight();
+
prevMouseMoveCallback = s_window->getMouseMoveCallback();
s_window->setMouseMoveCallback(MyMouseMoveCallback);
@@ -775,56 +889,77 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
assert(glGetError()==GL_NO_ERROR);
+ {
+ GL3TexLoader* myTexLoader = new GL3TexLoader;
+ m_internalData->m_myTexLoader = myTexLoader;
- gui = new GwenUserInterface;
- GL3TexLoader* myTexLoader = new GL3TexLoader;
-
- Gwen::Renderer::Base* gwenRenderer = 0;
- if (sUseOpenGL2 )
- {
- gwenRenderer = new Gwen::Renderer::OpenGL_DebugFont();
- }
+
+ if (sUseOpenGL2)
+ {
+ m_internalData->m_gwenRenderer = new Gwen::Renderer::OpenGL_DebugFont();
+ }
#ifndef NO_OPENGL3
- else
- {
- sth_stash* fontstash=simpleApp->getFontStash();
- gwenRenderer = new GwenOpenGL3CoreRenderer(simpleApp->m_primRenderer,fontstash,width,height,s_window->getRetinaScale(),myTexLoader);
- }
+ else
+ {
+ sth_stash* fontstash = simpleApp->getFontStash();
+ m_internalData->m_gwenRenderer = new GwenOpenGL3CoreRenderer(simpleApp->m_primRenderer, fontstash, width, height, s_window->getRetinaScale(), myTexLoader);
+ }
#endif
- //
- gui->init(width,height,gwenRenderer,s_window->getRetinaScale());
-
-
-
-
-// gui->getInternalData()->m_explorerPage
- Gwen::Controls::TreeControl* tree = gui->getInternalData()->m_explorerTreeCtrl;
+ gui2 = new GwenUserInterface;
+
+ m_internalData->m_gui = gui2;
+
+ m_internalData->m_myTexLoader = myTexLoader;
-
- //gui->getInternalData()->pRenderer->setTextureLoader(myTexLoader);
+
+ gui2->init(width, height, m_internalData->m_gwenRenderer, s_window->getRetinaScale());
+
+
+ }
+ //gui = 0;// new GwenUserInterface;
- s_profWindow= setupProfileWindow(gui->getInternalData());
- profileWindowSetVisible(s_profWindow,false);
- gui->setFocus();
+ GL3TexLoader* myTexLoader = m_internalData->m_myTexLoader;
+ // = myTexLoader;
+
+
+
+ //
+
+ if (gui2)
+ {
+
- s_parameterInterface = s_app->m_parameterInterface = new GwenParameterInterface(gui->getInternalData());
- s_app->m_2dCanvasInterface = new QuickCanvas(myTexLoader);
- ///add some demos to the gAllExamples
-
+ // gui->getInternalData()->m_explorerPage
+ Gwen::Controls::TreeControl* tree = gui2->getInternalData()->m_explorerTreeCtrl;
+
+
+ //gui->getInternalData()->pRenderer->setTextureLoader(myTexLoader);
+
+
+// s_profWindow= setupProfileWindow(gui2->getInternalData());
+ //m_internalData->m_profWindow = s_profWindow;
+ // profileWindowSetVisible(s_profWindow,false);
+ gui2->setFocus();
+
+ s_parameterInterface = s_app->m_parameterInterface = new GwenParameterInterface(gui2->getInternalData());
+ s_app->m_2dCanvasInterface = new QuickCanvas(myTexLoader);
+ ///add some demos to the gAllExamples
+
+
int numDemos = gAllExamples->getNumRegisteredExamples();
//char nodeText[1024];
//int curDemo = 0;
int selectedDemo = 0;
Gwen::Controls::TreeNode* curNode = tree;
- MyMenuItemHander* handler2 = new MyMenuItemHander(-1);
+ m_internalData->m_handler2 = new MyMenuItemHander(-1);
char* demoNameFromCommandOption = 0;
args.GetCmdLineArgument("start_demo_name", demoNameFromCommandOption);
@@ -832,7 +967,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
selectedDemo = -1;
}
- tree->onReturnKeyDown.Add(handler2, &MyMenuItemHander::onButtonD);
+ tree->onReturnKeyDown.Add(m_internalData->m_handler2, &MyMenuItemHander::onButtonD);
int firstAvailableDemoIndex=-1;
Gwen::Controls::TreeNode* firstNode=0;
@@ -879,13 +1014,18 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
}
}
+#if 1
MyMenuItemHander* handler = new MyMenuItemHander(d);
+ m_internalData->m_handlers.push_back(handler);
+
pNode->onNamePress.Add(handler, &MyMenuItemHander::onButtonA);
pNode->GetButton()->onDoubleClick.Add(handler, &MyMenuItemHander::onButtonB);
pNode->GetButton()->onDown.Add(handler, &MyMenuItemHander::onButtonC);
pNode->onSelect.Add(handler, &MyMenuItemHander::onButtonE);
pNode->onReturnKeyDown.Add(handler, &MyMenuItemHander::onButtonG);
pNode->onSelectChange.Add(handler, &MyMenuItemHander::onButtonF);
+
+#endif
// pNode->onKeyReturn.Add(handler, &MyMenuItemHander::onButtonD);
// pNode->GetButton()->onKeyboardReturn.Add(handler, &MyMenuItemHander::onButtonD);
// pNode->onNamePress.Add(handler, &MyMenuItemHander::onButtonD);
@@ -895,6 +1035,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
else
{
curNode = tree->AddNode(nodeUText);
+ m_internalData->m_nodes.push_back(curNode);
}
}
@@ -913,6 +1054,9 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
}
}
+ free(demoNameFromCommandOption);
+ demoNameFromCommandOption = 0;
+
btAssert(sCurrentDemo!=0);
if (sCurrentDemo==0)
{
@@ -920,9 +1064,11 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
exit(0);
}
- gui->registerFileOpenCallback(fileOpenCallback);
- gui->registerQuitCallback(quitCallback);
-
+ gui2->registerFileOpenCallback(fileOpenCallback);
+ gui2->registerQuitCallback(quitCallback);
+ }
+
+
return true;
}
@@ -941,7 +1087,7 @@ bool OpenGLExampleBrowser::requestedExit()
void OpenGLExampleBrowser::update(float deltaTime)
{
-
+ B3_PROFILE("OpenGLExampleBrowser::update");
assert(glGetError()==GL_NO_ERROR);
s_instancingRenderer->init();
DrawGridData dg;
@@ -955,14 +1101,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
s_instancingRenderer->updateCamera(dg.upAxis);
}
- if (renderGrid)
- {
- BT_PROFILE("Draw Grid");
- glPolygonOffset(3.0, 3);
- glEnable(GL_POLYGON_OFFSET_FILL);
- s_app->drawGrid(dg);
-
- }
+
static int frameCount = 0;
frameCount++;
@@ -974,35 +1113,37 @@ void OpenGLExampleBrowser::update(float deltaTime)
s_app->drawText(bla,10,10);
}
+ if (gPngFileName)
+ {
+
+ static int skip = 0;
+ skip--;
+ if (skip<0)
+ {
+ skip=gPngSkipFrames;
+ //printf("gPngFileName=%s\n",gPngFileName);
+ static int s_frameCount = 100;
+
+ sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
+ //b3Printf("Made screenshot %s",staticPngFileName);
+ s_app->dumpNextFrameToPng(staticPngFileName);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+ }
+ }
+
if (sCurrentDemo)
{
- if (!pauseSimulation)
+ if (!pauseSimulation || singleStepSimulation)
{
+ singleStepSimulation = false;
//printf("---------------------------------------------------\n");
//printf("Framecount = %d\n",frameCount);
-
- if (gPngFileName)
- {
-
- static int skip = 0;
- skip++;
- if (skip>4)
- {
- skip=0;
- //printf("gPngFileName=%s\n",gPngFileName);
- static int s_frameCount = 100;
-
- sprintf(staticPngFileName,"%s%d.png",gPngFileName,s_frameCount++);
- //b3Printf("Made screenshot %s",staticPngFileName);
- s_app->dumpNextFrameToPng(staticPngFileName);
- glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
- }
- }
-
+ B3_PROFILE("sCurrentDemo->stepSimulation");
if (gFixedTimeStep>0)
{
+
sCurrentDemo->stepSimulation(gFixedTimeStep);
} else
{
@@ -1010,6 +1151,14 @@ void OpenGLExampleBrowser::update(float deltaTime)
}
}
+ if (renderGrid)
+ {
+ BT_PROFILE("Draw Grid");
+ glPolygonOffset(3.0, 3);
+ glEnable(GL_POLYGON_OFFSET_FILL);
+ s_app->drawGrid(dg);
+
+ }
if (renderVisualGeometry && ((gDebugDrawFlags&btIDebugDraw::DBG_DrawWireframe)==0))
{
if (visualWireframe)
@@ -1020,16 +1169,19 @@ void OpenGLExampleBrowser::update(float deltaTime)
sCurrentDemo->renderScene();
}
{
-
+ B3_PROFILE("physicsDebugDraw");
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
sCurrentDemo->physicsDebugDraw(gDebugDrawFlags);
}
}
+
+
{
- if (s_guiHelper && s_guiHelper->getRenderInterface() && s_guiHelper->getRenderInterface()->getActiveCamera())
+ if (gui2 && s_guiHelper && s_guiHelper->getRenderInterface() && s_guiHelper->getRenderInterface()->getActiveCamera())
{
+ B3_PROFILE("setStatusBarMessage");
char msg[1024];
float camDist = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraDistance();
float pitch = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPitch();
@@ -1037,7 +1189,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
float camTarget[3];
s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraTargetPosition(camTarget);
sprintf(msg,"dist=%f, pitch=%f, yaw=%f,target=%f,%f,%f", camDist,pitch,yaw,camTarget[0],camTarget[1],camTarget[2]);
- gui->setStatusBarMessage(msg, true);
+ gui2->setStatusBarMessage(msg, true);
}
}
@@ -1045,16 +1197,25 @@ void OpenGLExampleBrowser::update(float deltaTime)
static int toggle = 1;
if (renderGui)
{
- if (!pauseSimulation)
- processProfileData(s_profWindow,false);
+ B3_PROFILE("renderGui");
+ // if (!pauseSimulation)
+ // processProfileData(s_profWindow,false);
- if (sUseOpenGL2)
+ if (sUseOpenGL2)
{
-
- saveOpenGLState(s_instancingRenderer->getScreenWidth(),s_instancingRenderer->getScreenHeight());
+
+ saveOpenGLState(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight());
}
- BT_PROFILE("Draw Gwen GUI");
- gui->draw(s_instancingRenderer->getScreenWidth(),s_instancingRenderer->getScreenHeight());
+
+ if (m_internalData->m_gui)
+ {
+ m_internalData->m_gui->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight());
+ }
+ if (gui2)
+ {
+ gui2->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight());
+ }
+
if (sUseOpenGL2)
{
restoreOpenGLState();
@@ -1068,13 +1229,26 @@ void OpenGLExampleBrowser::update(float deltaTime)
toggle=1-toggle;
{
BT_PROFILE("Sync Parameters");
- s_parameterInterface->syncParameters();
+ if (s_parameterInterface)
+ {
+ s_parameterInterface->syncParameters();
+ }
}
{
BT_PROFILE("Swap Buffers");
s_app->swapBuffer();
}
- gui->forceUpdateScrollBars();
+ if (gui2)
+ {
+ B3_PROFILE("forceUpdateScrollBars");
+ gui2->forceUpdateScrollBars();
+ }
}
+
+void OpenGLExampleBrowser::setSharedMemoryInterface(class SharedMemoryInterface* sharedMem)
+{
+ gDisableDemoSelection = true;
+ sSharedMem = sharedMem;
+}
diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.h b/examples/ExampleBrowser/OpenGLExampleBrowser.h
index 7d181e0..1f68abe 100644
--- a/examples/ExampleBrowser/OpenGLExampleBrowser.h
+++ b/examples/ExampleBrowser/OpenGLExampleBrowser.h
@@ -5,6 +5,9 @@
class OpenGLExampleBrowser : public ExampleBrowserInterface
{
+
+ struct OpenGLExampleBrowserInternalData* m_internalData;
+
public:
OpenGLExampleBrowser(class ExampleEntries* examples);
@@ -17,7 +20,10 @@ public:
virtual void update(float deltaTime);
virtual bool requestedExit();
+
+ virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
+ static void registerFileImporter(const char* extension, CommonExampleInterface::CreateFunc* createFunc);
};
#endif //OPENGL_BROWSER_GUI_H
diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp
index 19d3927..4a4277b 100644
--- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp
+++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp
@@ -5,9 +5,10 @@
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "Bullet3Common/b3Scalar.h"
+#include "CollisionShape2TriangleMesh.h"
-#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
+#include "../OpenGLWindow/SimpleCamera.h"
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
//backwards compatibility
#include "GL_ShapeDrawer.h"
@@ -83,7 +84,10 @@ public:
virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)
{
- drawLine(PointOnB,PointOnB+normalOnB,color);
+ drawLine(PointOnB,PointOnB+normalOnB*distance,color);
+ btVector3 ncolor(0, 0, 0);
+ drawLine(PointOnB, PointOnB + normalOnB*0.01, ncolor);
+
}
@@ -142,8 +146,26 @@ struct OpenGLGuiHelperInternalData
struct CommonGraphicsApp* m_glApp;
class MyDebugDrawer* m_debugDraw;
GL_ShapeDrawer* m_gl2ShapeDrawer;
+ bool m_vrMode;
+ int m_vrSkipShadowPass;
+
+ btAlignedObjectArray<unsigned char> m_rgbaPixelBuffer1;
+ btAlignedObjectArray<float> m_depthBuffer1;
+
+ OpenGLGuiHelperInternalData()
+ :m_vrMode(false),
+ m_vrSkipShadowPass(0)
+ {
+ }
+
};
+void OpenGLGuiHelper::setVRMode(bool vrMode)
+{
+ m_data->m_vrMode = vrMode;
+ m_data->m_vrSkipShadowPass = 0;
+}
+
OpenGLGuiHelper::OpenGLGuiHelper(CommonGraphicsApp* glApp, bool useOpenGL2)
@@ -163,6 +185,7 @@ OpenGLGuiHelper::OpenGLGuiHelper(CommonGraphicsApp* glApp, bool useOpenGL2)
OpenGLGuiHelper::~OpenGLGuiHelper()
{
+ delete m_data->m_debugDraw;
delete m_data->m_gl2ShapeDrawer;
delete m_data;
}
@@ -195,9 +218,16 @@ void OpenGLGuiHelper::createCollisionObjectGraphicsObject(btCollisionObject* bod
}
}
-int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices)
+int OpenGLGuiHelper::registerTexture(const unsigned char* texels, int width, int height)
{
- int shapeId = m_data->m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices);
+ int textureId = m_data->m_glApp->m_renderer->registerTexture(texels,width,height);
+ return textureId;
+}
+
+
+int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
+{
+ int shapeId = m_data->m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices,primitiveType, textureId);
return shapeId;
}
@@ -206,221 +236,9 @@ int OpenGLGuiHelper::registerGraphicsInstance(int shapeIndex, const float* posit
return m_data->m_glApp->m_renderer->registerGraphicsInstance(shapeIndex,position,quaternion,color,scaling);
}
-static void createCollisionShapeGraphicsObjectInternal(btCollisionShape* collisionShape, const btTransform& parentTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
+void OpenGLGuiHelper::removeAllGraphicsInstances()
{
-//todo: support all collision shape types
- switch (collisionShape->getShapeType())
- {
- case SOFTBODY_SHAPE_PROXYTYPE:
- {
- //skip the soft body collision shape for now
- break;
- }
- case STATIC_PLANE_PROXYTYPE:
- {
- //draw a box, oriented along the plane normal
- const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(collisionShape);
- btScalar planeConst = staticPlaneShape->getPlaneConstant();
- const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
- btVector3 planeOrigin = planeNormal * planeConst;
- btVector3 vec0,vec1;
- btPlaneSpace1(planeNormal,vec0,vec1);
- btScalar vecLen = 100.f;
- btVector3 verts[4];
-
- verts[0] = planeOrigin + vec0*vecLen + vec1*vecLen;
- verts[1] = planeOrigin - vec0*vecLen + vec1*vecLen;
- verts[2] = planeOrigin - vec0*vecLen - vec1*vecLen;
- verts[3] = planeOrigin + vec0*vecLen - vec1*vecLen;
-
- int startIndex = verticesOut.size();
- indicesOut.push_back(startIndex+0);
- indicesOut.push_back(startIndex+1);
- indicesOut.push_back(startIndex+2);
- indicesOut.push_back(startIndex+0);
- indicesOut.push_back(startIndex+2);
- indicesOut.push_back(startIndex+3);
-
- btVector3 triNormal = parentTransform.getBasis()*planeNormal;
-
-
- for (int i=0;i<4;i++)
- {
- GLInstanceVertex vtx;
- btVector3 pos =parentTransform*verts[i];
- vtx.xyzw[0] = pos.x();
- vtx.xyzw[1] = pos.y();
- vtx.xyzw[2] = pos.z();
- vtx.xyzw[3] = 0.f;
-
- vtx.normal[0] =triNormal.x();
- vtx.normal[1] =triNormal.y();
- vtx.normal[2] =triNormal.z();
-
- vtx.uv[0] = 0.5f;
- vtx.uv[1] = 0.5f;
- verticesOut.push_back(vtx);
- }
- break;
- }
- case TRIANGLE_MESH_SHAPE_PROXYTYPE:
- {
-
-
- btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) collisionShape;
- btVector3 trimeshScaling = trimesh->getLocalScaling();
- btStridingMeshInterface* meshInterface = trimesh->getMeshInterface();
- btAlignedObjectArray<btVector3> vertices;
- btAlignedObjectArray<int> indices;
-
- for (int partId=0;partId<meshInterface->getNumSubParts();partId++)
- {
-
- const unsigned char *vertexbase = 0;
- int numverts = 0;
- PHY_ScalarType type = PHY_INTEGER;
- int stride = 0;
- const unsigned char *indexbase = 0;
- int indexstride = 0;
- int numfaces = 0;
- PHY_ScalarType indicestype = PHY_INTEGER;
- //PHY_ScalarType indexType=0;
-
- btVector3 triangleVerts[3];
- meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
- btVector3 aabbMin,aabbMax;
-
- for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
- {
- unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);
-
- for (int j=2;j>=0;j--)
- {
-
- int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
- if (type == PHY_FLOAT)
- {
- float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
- triangleVerts[j] = btVector3(
- graphicsbase[0]*trimeshScaling.getX(),
- graphicsbase[1]*trimeshScaling.getY(),
- graphicsbase[2]*trimeshScaling.getZ());
- }
- else
- {
- double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
- triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*trimeshScaling.getX()),
- btScalar(graphicsbase[1]*trimeshScaling.getY()),
- btScalar(graphicsbase[2]*trimeshScaling.getZ()));
- }
- }
- indices.push_back(vertices.size());
- vertices.push_back(triangleVerts[0]);
- indices.push_back(vertices.size());
- vertices.push_back(triangleVerts[1]);
- indices.push_back(vertices.size());
- vertices.push_back(triangleVerts[2]);
-
- btVector3 triNormal = (triangleVerts[1]-triangleVerts[0]).cross(triangleVerts[2]-triangleVerts[0]);
- triNormal.normalize();
-
- for (int v=0;v<3;v++)
- {
- GLInstanceVertex vtx;
- btVector3 pos =parentTransform*triangleVerts[v];
- vtx.xyzw[0] = pos.x();
- vtx.xyzw[1] = pos.y();
- vtx.xyzw[2] = pos.z();
- vtx.xyzw[3] = 0.f;
-
-
- vtx.normal[0] =triNormal.x();
- vtx.normal[1] =triNormal.y();
- vtx.normal[2] =triNormal.z();
-
- vtx.uv[0] = 0.5f;
- vtx.uv[1] = 0.5f;
-
- indicesOut.push_back(verticesOut.size());
- verticesOut.push_back(vtx);
- }
-
-
- }
- }
-
- break;
- }
- default:
- {
- if (collisionShape->isConvex())
- {
- btConvexShape* convex = (btConvexShape*)collisionShape;
- {
- btShapeHull* hull = new btShapeHull(convex);
- hull->buildHull(0.0);
-
- {
- //int strideInBytes = 9*sizeof(float);
- //int numVertices = hull->numVertices();
- //int numIndices =hull->numIndices();
-
- for (int t=0;t<hull->numTriangles();t++)
- {
-
- btVector3 triNormal;
-
- int index0 = hull->getIndexPointer()[t*3+0];
- int index1 = hull->getIndexPointer()[t*3+1];
- int index2 = hull->getIndexPointer()[t*3+2];
- btVector3 pos0 =parentTransform*hull->getVertexPointer()[index0];
- btVector3 pos1 =parentTransform*hull->getVertexPointer()[index1];
- btVector3 pos2 =parentTransform*hull->getVertexPointer()[index2];
- triNormal = (pos1-pos0).cross(pos2-pos0);
- triNormal.normalize();
-
- for (int v=0;v<3;v++)
- {
- int index = hull->getIndexPointer()[t*3+v];
- GLInstanceVertex vtx;
- btVector3 pos =parentTransform*hull->getVertexPointer()[index];
- vtx.xyzw[0] = pos.x();
- vtx.xyzw[1] = pos.y();
- vtx.xyzw[2] = pos.z();
- vtx.xyzw[3] = 0.f;
-
- vtx.normal[0] =triNormal.x();
- vtx.normal[1] =triNormal.y();
- vtx.normal[2] =triNormal.z();
-
- vtx.uv[0] = 0.5f;
- vtx.uv[1] = 0.5f;
-
- indicesOut.push_back(verticesOut.size());
- verticesOut.push_back(vtx);
- }
- }
- }
- }
- } else
- {
- if (collisionShape->isCompound())
- {
- btCompoundShape* compound = (btCompoundShape*) collisionShape;
- for (int i=0;i<compound->getNumChildShapes();i++)
- {
-
- btTransform childWorldTrans = parentTransform * compound->getChildTransform(i);
- createCollisionShapeGraphicsObjectInternal(compound->getChildShape(i),childWorldTrans,verticesOut,indicesOut);
- }
- } else
- {
- btAssert(0);
- }
-
- }
- }
- };
+ m_data->m_glApp->m_renderer->removeAllInstances();
}
void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
@@ -429,42 +247,92 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli
if (collisionShape->getUserIndex()>=0)
return;
- btAlignedObjectArray<GLInstanceVertex> vertices;
+ btAlignedObjectArray<GLInstanceVertex> gfxVertices;
+
btAlignedObjectArray<int> indices;
btTransform startTrans;startTrans.setIdentity();
- createCollisionShapeGraphicsObjectInternal(collisionShape,startTrans,vertices,indices);
+ {
+ btAlignedObjectArray<btVector3> vertexPositions;
+ btAlignedObjectArray<btVector3> vertexNormals;
+ CollisionShape2TriangleMesh(collisionShape,startTrans,vertexPositions,vertexNormals,indices);
+ gfxVertices.resize(vertexPositions.size());
+ for (int i=0;i<vertexPositions.size();i++)
+ {
+ for (int j=0;j<4;j++)
+ {
+ gfxVertices[i].xyzw[j] = vertexPositions[i][j];
+ }
+ for (int j=0;j<3;j++)
+ {
+ gfxVertices[i].normal[j] = vertexNormals[i][j];
+ }
+ for (int j=0;j<2;j++)
+ {
+ gfxVertices[i].uv[j] = 0.5;//we don't have UV info...
+ }
+ }
+ }
+
- if (vertices.size() && indices.size())
+ if (gfxVertices.size() && indices.size())
{
- int shapeId = m_data->m_glApp->m_renderer->registerShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size());
+ int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size(),B3_GL_TRIANGLES,-1);
collisionShape->setUserIndex(shapeId);
}
}
void OpenGLGuiHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
{
+ //in VR mode, we skip the synchronization for the second eye
+ if (m_data->m_vrMode && m_data->m_vrSkipShadowPass==1)
+ return;
+
int numCollisionObjects = rbWorld->getNumCollisionObjects();
- for (int i = 0; i<numCollisionObjects; i++)
{
- btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
- btVector3 pos = colObj->getWorldTransform().getOrigin();
- btQuaternion orn = colObj->getWorldTransform().getRotation();
- int index = colObj->getUserIndex();
- if (index >= 0)
+ B3_PROFILE("write all InstanceTransformToCPU");
+ for (int i = 0; i<numCollisionObjects; i++)
{
- m_data->m_glApp->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, index);
+ B3_PROFILE("writeSingleInstanceTransformToCPU");
+ btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
+ btVector3 pos = colObj->getWorldTransform().getOrigin();
+ btQuaternion orn = colObj->getWorldTransform().getRotation();
+ int index = colObj->getUserIndex();
+ if (index >= 0)
+ {
+ m_data->m_glApp->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, index);
+ }
}
}
- m_data->m_glApp->m_renderer->writeTransforms();
+ {
+ B3_PROFILE("writeTransforms");
+ m_data->m_glApp->m_renderer->writeTransforms();
+ }
}
void OpenGLGuiHelper::render(const btDiscreteDynamicsWorld* rbWorld)
{
-
- m_data->m_glApp->m_renderer->renderScene();
+ if (m_data->m_vrMode)
+ {
+ //in VR, we skip the shadow generation for the second eye
+
+ if (m_data->m_vrSkipShadowPass>=1)
+ {
+ m_data->m_glApp->m_renderer->renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE);
+ m_data->m_vrSkipShadowPass=0;
+
+ } else
+ {
+ m_data->m_glApp->m_renderer->renderScene();
+ m_data->m_vrSkipShadowPass++;
+ }
+ } else
+ {
+ m_data->m_glApp->m_renderer->renderScene();
+ }
+
//backwards compatible OpenGL2 rendering
if (m_data->m_gl2ShapeDrawer && rbWorld)
@@ -501,6 +369,7 @@ CommonParameterInterface* OpenGLGuiHelper::getParameterInterface()
void OpenGLGuiHelper::setUpAxis(int axis)
{
m_data->m_glApp->setUpAxis(axis);
+
}
void OpenGLGuiHelper::resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
@@ -515,6 +384,88 @@ void OpenGLGuiHelper::resetCamera(float camDist, float pitch, float yaw, float c
}
+void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
+ unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
+ float* depthBuffer, int depthBufferSizeInPixels,
+ int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
+ int startPixelIndex, int destinationWidth,
+ int destinationHeight, int* numPixelsCopied)
+{
+ int sourceWidth = m_data->m_glApp->m_window->getWidth()*m_data->m_glApp->m_window->getRetinaScale();
+ int sourceHeight = m_data->m_glApp->m_window->getHeight()*m_data->m_glApp->m_window->getRetinaScale();
+
+ if (numPixelsCopied)
+ *numPixelsCopied = 0;
+
+ int numTotalPixels = destinationWidth*destinationHeight;
+ int numRemainingPixels = numTotalPixels - startPixelIndex;
+ int numBytesPerPixel = 4;//RGBA
+ int numRequestedPixels = btMin(rgbaBufferSizeInPixels,numRemainingPixels);
+ if (numRequestedPixels)
+ {
+ if (startPixelIndex==0)
+ {
+ CommonCameraInterface* oldCam = getRenderInterface()->getActiveCamera();
+ SimpleCamera tempCam;
+ getRenderInterface()->setActiveCamera(&tempCam);
+ getRenderInterface()->getActiveCamera()->setVRCamera(viewMatrix,projectionMatrix);
+ getRenderInterface()->renderScene();
+ getRenderInterface()->setActiveCamera(oldCam);
+
+ {
+ btAlignedObjectArray<unsigned char> sourceRgbaPixelBuffer;
+ btAlignedObjectArray<float> sourceDepthBuffer;
+ //copy the image into our local cache
+ sourceRgbaPixelBuffer.resize(sourceWidth*sourceHeight*numBytesPerPixel);
+ sourceDepthBuffer.resize(sourceWidth*sourceHeight);
+ m_data->m_glApp->getScreenPixels(&(sourceRgbaPixelBuffer[0]),sourceRgbaPixelBuffer.size(), &sourceDepthBuffer[0],sizeof(float)*sourceDepthBuffer.size());
+
+ m_data->m_rgbaPixelBuffer1.resize(destinationWidth*destinationHeight*numBytesPerPixel);
+ m_data->m_depthBuffer1.resize(destinationWidth*destinationHeight);
+ //rescale and flip
+
+ for (int i=0;i<destinationWidth;i++)
+ {
+ for (int j=0;j<destinationHeight;j++)
+ {
+ int xIndex = int(float(i)*(float(sourceWidth)/float(destinationWidth)));
+ int yIndex = int(float(destinationHeight-1-j)*(float(sourceHeight)/float(destinationHeight)));
+ btClamp(xIndex,0,sourceWidth);
+ btClamp(yIndex,0,sourceHeight);
+ int bytesPerPixel = 4; //RGBA
+
+ int sourcePixelIndex = (xIndex+yIndex*sourceWidth)*bytesPerPixel;
+ m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0] = sourceRgbaPixelBuffer[sourcePixelIndex+0];
+ m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+1] = sourceRgbaPixelBuffer[sourcePixelIndex+1];
+ m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+2] = sourceRgbaPixelBuffer[sourcePixelIndex+2];
+ m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+3] = 255;
+ }
+ }
+ }
+ }
+ if (pixelsRGBA)
+ {
+ for (int i=0;i<numRequestedPixels*numBytesPerPixel;i++)
+ {
+ pixelsRGBA[i] = m_data->m_rgbaPixelBuffer1[i+startPixelIndex*numBytesPerPixel];
+ }
+ }
+ if (depthBuffer)
+ {
+ for (int i=0;i<numRequestedPixels;i++)
+ {
+ depthBuffer[i] = m_data->m_depthBuffer1[i];
+ }
+ }
+ if (numPixelsCopied)
+ *numPixelsCopied = numRequestedPixels;
+
+
+ }
+
+
+}
+
struct MyConvertPointerSizeT
@@ -561,6 +512,8 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor
void OpenGLGuiHelper::drawText3D( const char* txt, float posX, float posY, float posZ, float size)
{
+ B3_PROFILE("OpenGLGuiHelper::drawText3D");
+
btAssert(m_data->m_glApp);
m_data->m_glApp->drawText3D(txt,posX,posY,posZ,size);
}
diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h
index 631218d..eaa72a7 100644
--- a/examples/ExampleBrowser/OpenGLGuiHelper.h
+++ b/examples/ExampleBrowser/OpenGLGuiHelper.h
@@ -20,12 +20,11 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual void createCollisionObjectGraphicsObject(btCollisionObject* body, const btVector3& color);
- virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices);
-
+ virtual int registerTexture(const unsigned char* texels, int width, int height);
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId);
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
-
+ virtual void removeAllGraphicsInstances();
-
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);
virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld);
@@ -44,12 +43,21 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual void setUpAxis(int axis);
virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ);
+
+ virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
+ unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
+ float* depthBuffer, int depthBufferSizeInPixels,
+ int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
+ int startPixelIndex, int destinationWidth,
+ int destinationHeight, int* numPixelsCopied);
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) ;
virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size);
void renderInternalGl2(int pass, const btDiscreteDynamicsWorld* dynamicsWorld);
+
+ void setVRMode(bool vrMode);
};
#endif //OPENGL_GUI_HELPER_H
diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp
index c3442de..d0f132c 100644
--- a/examples/ExampleBrowser/main.cpp
+++ b/examples/ExampleBrowser/main.cpp
@@ -1,12 +1,6 @@
-//#define EXAMPLE_CONSOLE_ONLY
-#ifdef EXAMPLE_CONSOLE_ONLY
- #include "EmptyBrowser.h"
- typedef EmptyBrowser DefaultBrowser;
-#else
- #include "OpenGLExampleBrowser.h"
- typedef OpenGLExampleBrowser DefaultBrowser;
-#endif //EXAMPLE_CONSOLE_ONLY
+
+#include "OpenGLExampleBrowser.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include "../Utils/b3Clock.h"
@@ -14,32 +8,52 @@
#include "ExampleEntries.h"
#include "Bullet3Common/b3Logging.h"
+#include "../Importers/ImportObjDemo/ImportObjExample.h"
+#include "../Importers/ImportBsp/ImportBspExample.h"
+#include "../Importers/ImportColladaDemo/ImportColladaSetup.h"
+#include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
+#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
+#include "../Importers/ImportSDFDemo/ImportSDFSetup.h"
+#include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
+#include "LinearMath/btAlignedAllocator.h"
int main(int argc, char* argv[])
{
- b3CommandLineArgs args(argc,argv);
- b3Clock clock;
-
-
- ExampleEntries examples;
- examples.initExampleEntries();
-
- ExampleBrowserInterface* exampleBrowser = new DefaultBrowser(&examples);
- bool init = exampleBrowser->init(argc,argv);
- clock.reset();
- if (init)
{
- do
+ b3CommandLineArgs args(argc, argv);
+ b3Clock clock;
+
+
+ ExampleEntriesAll examples;
+ examples.initExampleEntries();
+
+ OpenGLExampleBrowser* exampleBrowser = new OpenGLExampleBrowser(&examples);
+ bool init = exampleBrowser->init(argc, argv);
+ exampleBrowser->registerFileImporter(".urdf", ImportURDFCreateFunc);
+ exampleBrowser->registerFileImporter(".sdf", ImportSDFCreateFunc);
+ exampleBrowser->registerFileImporter(".obj", ImportObjCreateFunc);
+ exampleBrowser->registerFileImporter(".stl", ImportSTLCreateFunc);
+
+ clock.reset();
+ if (init)
{
- float deltaTimeInSeconds = clock.getTimeMicroseconds()/1000000.f;
- clock.reset();
- exampleBrowser->update(deltaTimeInSeconds);
+ do
+ {
+ float deltaTimeInSeconds = clock.getTimeMicroseconds() / 1000000.f;
+ clock.reset();
+ exampleBrowser->update(deltaTimeInSeconds);
+
+ } while (!exampleBrowser->requestedExit());
+ }
+ delete exampleBrowser;
- } while (!exampleBrowser->requestedExit());
}
- delete exampleBrowser;
+#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
+ int numBytesLeaked = btDumpMemoryLeaks();
+ btAssert(numBytesLeaked==0);
+#endif//BT_DEBUG_MEMORY_ALLOCATIONS
return 0;
}
diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua
index 4b464e4..e5cc37d 100644
--- a/examples/ExampleBrowser/premake4.lua
+++ b/examples/ExampleBrowser/premake4.lua
@@ -1,57 +1,61 @@
-
- project "App_ExampleBrowser"
+project "App_BulletExampleBrowser"
- hasCL = findOpenCL("clew")
-
- if (hasCL) then
+ language "C++"
- -- project ("App_Bullet3_OpenCL_Demos_" .. vendor)
+ kind "ConsoleApp"
- initOpenCL("clew")
+ hasCL = findOpenCL("clew")
- end
+ if (hasCL) then
+ initOpenCL("clew")
+ end
- language "C++"
-
- kind "ConsoleApp"
+ links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"}
+ initOpenGL()
+ initGlew()
- includedirs {
+ includedirs {
".",
"../../src",
"../ThirdPartyLibs",
}
- if _OPTIONS["lua"] then
- includedirs{"../ThirdPartyLibs/lua-5.2.3/src"}
- links {"lua-5.2.3"}
- defines {"ENABLE_LUA"}
- files {"../LuaDemo/LuaPhysicsSetup.cpp"}
- end
-
- links{"gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
- initOpenGL()
- initGlew()
+ if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+ end
- if (hasCL) then
- links {
- "Bullet3OpenCL_clew",
- "Bullet3Dynamics",
- "Bullet3Collision",
- "Bullet3Geometry",
- "Bullet3Common",
- }
- end
-
- defines {"INCLUDE_CLOTH_DEMOS"}
-
+ if (hasCL) then
+ links {
+ "Bullet3OpenCL_clew",
+ "Bullet3Dynamics",
+ "Bullet3Collision",
+ "Bullet3Geometry",
+ "Bullet3Common",
+ }
+ end
+ if _OPTIONS["lua"] then
+ includedirs{"../ThirdPartyLibs/lua-5.2.3/src"}
+ links {"lua-5.2.3"}
+ defines {"ENABLE_LUA"}
+ files {"../LuaDemo/LuaPhysicsSetup.cpp"}
+ end
- files {
- "*.cpp",
- "*.h",
- "GwenGUISupport/*.cpp",
- "GwenGUISupport/*.h",
+ defines {"INCLUDE_CLOTH_DEMOS"}
+
+ files {
+
+ "main.cpp",
+ "ExampleEntries.cpp",
+ "../InverseKinematics/*",
+ "../TinyRenderer/geometry.cpp",
+ "../TinyRenderer/model.cpp",
+ "../TinyRenderer/tgaimage.cpp",
+ "../TinyRenderer/our_gl.cpp",
+ "../TinyRenderer/TinyRenderer.cpp",
+ "../SharedMemory/IKTrajectoryHelper.cpp",
+ "../SharedMemory/IKTrajectoryHelper.h",
"../SharedMemory/PhysicsClientC_API.cpp",
"../SharedMemory/PhysicsClientC_API.h",
"../SharedMemory/PhysicsServerExample.cpp",
@@ -59,9 +63,11 @@
"../SharedMemory/PhysicsServer.cpp",
"../SharedMemory/PhysicsServerSharedMemory.cpp",
"../SharedMemory/PhysicsClientSharedMemory.cpp",
+ "../SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
"../SharedMemory/PhysicsClient.cpp",
"../SharedMemory/PosixSharedMemory.cpp",
"../SharedMemory/Win32SharedMemory.cpp",
+ "../SharedMemory/InProcessMemory.cpp",
"../SharedMemory/PhysicsDirect.cpp",
"../SharedMemory/PhysicsDirect.h",
"../SharedMemory/PhysicsDirectC_API.cpp",
@@ -72,6 +78,8 @@
"../SharedMemory/PhysicsLoopBackC_API.h",
"../SharedMemory/PhysicsServerCommandProcessor.cpp",
"../SharedMemory/PhysicsServerCommandProcessor.h",
+ "../SharedMemory/TinyRendererVisualShapeConverter.cpp",
+ "../SharedMemory/TinyRendererVisualShapeConverter.h",
"../MultiThreading/MultiThreadingExample.cpp",
"../MultiThreading/b3PosixThreadSupport.cpp",
"../MultiThreading/b3Win32ThreadSupport.cpp",
@@ -80,7 +88,9 @@
"../InverseDynamics/InverseDynamicsExample.h",
"../BasicDemo/BasicExample.*",
"../Tutorial/*",
+ "../ExtendedTutorials/*",
"../Collision/*",
+ "../RoboticsLearning/*",
"../Collision/Internal/*",
"../Benchmarks/*",
"../CommonInterfaces/*",
@@ -101,39 +111,22 @@
"../MultiBody/MultiDofDemo.cpp",
"../MultiBody/TestJointTorqueSetup.cpp",
"../MultiBody/Pendulum.cpp",
+ "../MultiBody/MultiBodySoftContact.cpp",
"../MultiBody/MultiBodyConstraintFeedback.cpp",
"../MultiBody/InvertedPendulumPDControl.cpp",
+ "../RigidBody/RigidBodySoftContact.cpp",
"../ThirdPartyLibs/stb_image/*",
"../ThirdPartyLibs/Wavefront/tiny_obj_loader.*",
"../ThirdPartyLibs/tinyxml/*",
- "../Utils/b3Clock.*",
- "../Utils/b3ResourcePath.*",
+ "../ThirdPartyLibs/BussIK/*",
"../GyroscopicDemo/GyroscopicSetup.cpp",
"../GyroscopicDemo/GyroscopicSetup.h",
- "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp",
- "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/model.cpp",
- "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp",
- "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/joint.cpp",
- "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h",
- "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h",
- "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h",
- "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h",
- "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h",
- "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h",
- "../ThirdPartyLibs/tinyxml/tinystr.cpp",
- "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
- "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
- "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
- "../ThirdPartyLibs/urdf/boost_replacement/lexical_cast.h",
- "../ThirdPartyLibs/urdf/boost_replacement/shared_ptr.h",
- "../ThirdPartyLibs/urdf/boost_replacement/printf_console.cpp",
- "../ThirdPartyLibs/urdf/boost_replacement/printf_console.h",
- "../ThirdPartyLibs/urdf/boost_replacement/string_split.cpp",
- "../ThirdPartyLibs/urdf/boost_replacement/string_split.h",
-
- }
-
- if (hasCL and findOpenGL3()) then
+ "../ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ }
+if (hasCL and findOpenGL3()) then
files {
"../OpenCL/broadphase/*",
"../OpenCL/CommonOpenCL/*",
@@ -141,13 +134,72 @@
"../OpenCL/rigidbody/GpuRigidBodyDemo.cpp",
}
end
+if os.is("Linux") then
+ initX11()
+end
+
+
+
+project "BulletExampleBrowserLib"
+
+ hasCL = findOpenCL("clew")
+
+ if (hasCL) then
+
+ -- project ("App_Bullet3_OpenCL_Demos_" .. vendor)
+
+ initOpenCL("clew")
+
+ end
+
+ language "C++"
+
+ kind "StaticLib"
+
+ includedirs {
+ ".",
+ "../../src",
+ "../ThirdPartyLibs",
+ }
+
+ if _OPTIONS["lua"] then
+ includedirs{"../ThirdPartyLibs/lua-5.2.3/src"}
+ links {"lua-5.2.3"}
+ defines {"ENABLE_LUA"}
+ files {"../LuaDemo/LuaPhysicsSetup.cpp"}
+ end
+
+
+ initOpenGL()
+ initGlew()
+
+ defines {"INCLUDE_CLOTH_DEMOS"}
+
+
+
+ files {
+ "OpenGLExampleBrowser.cpp",
+ "OpenGLGuiHelper.cpp",
+ "OpenGLExampleBrowser.cpp",
+ "../Utils/b3Clock.cpp",
+ "*.h",
+ "GwenGUISupport/*.cpp",
+ "GwenGUISupport/*.h",
+ "CollisionShape2TriangleMesh.cpp",
+ "CollisionShape2TriangleMesh.h",
+ "../Utils/b3ResourcePath.*",
+ "GL_ShapeDrawer.cpp",
+ "InProcessExampleBrowser.cpp",
+
+
+
+ }
+
+
if os.is("Linux") then
initX11()
end
-if os.is("MacOSX") then
- links{"Cocoa.framework"}
-end
-
+
diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/ExtendedTutorials/Bridge.cpp
similarity index 56%
copy from examples/BasicDemo/BasicExample.cpp
copy to examples/ExtendedTutorials/Bridge.cpp
index d501b51..269b5c4 100644
--- a/examples/BasicDemo/BasicExample.cpp
+++ b/examples/ExtendedTutorials/Bridge.cpp
@@ -15,26 +15,21 @@ subject to the following restrictions:
-#include "BasicExample.h"
+#include "Bridge.h"
#include "btBulletDynamicsCommon.h"
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Z 5
-
#include "LinearMath/btVector3.h"
-#include "LinearMath/btAlignedObjectArray.h"
-
+#include "LinearMath/btAlignedObjectArray.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
-
-struct BasicExample : public CommonRigidBodyBase
+const int TOTAL_PLANKS = 10;
+struct BridgeExample : public CommonRigidBodyBase
{
- BasicExample(struct GUIHelperInterface* helper)
+ BridgeExample(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
{
}
- virtual ~BasicExample(){}
+ virtual ~BridgeExample(){}
virtual void initPhysics();
virtual void renderScene();
void resetCamera()
@@ -47,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase
}
};
-void BasicExample::initPhysics()
+void BridgeExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
@@ -60,31 +55,33 @@ void BasicExample::initPhysics()
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
-
-
- //groundShape->initializePolyhedralFeatures();
-// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
-
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
- groundTransform.setOrigin(btVector3(0,-50,0));
-
+ groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
}
+ //create two fixed boxes to hold the planks
+
+
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
-
- btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
-
-
- //btCollisionShape* colShape = new btSphereShape(btScalar(1.));
+ btScalar plankWidth = 0.4;
+ btScalar plankHeight = 0.2;
+ btScalar plankBreadth = 1;
+ btScalar plankOffset = plankWidth; //distance between two planks
+ btScalar bridgeWidth = plankWidth*TOTAL_PLANKS + plankOffset*(TOTAL_PLANKS-1);
+ btScalar bridgeHeight = 5;
+ btScalar halfBridgeWidth = bridgeWidth*0.5f;
+
+ btBoxShape* colShape = createBoxShape(btVector3(plankWidth,plankHeight,plankBreadth));
+
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
@@ -100,24 +97,32 @@ void BasicExample::initPhysics()
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
-
- for (int k=0;k<ARRAY_SIZE_Y;k++)
- {
- for (int i=0;i<ARRAY_SIZE_X;i++)
- {
- for(int j = 0;j<ARRAY_SIZE_Z;j++)
- {
- startTransform.setOrigin(btVector3(
- btScalar(2.0*i),
- btScalar(20+2.0*k),
- btScalar(2.0*j)));
-
-
- createRigidBody(mass,startTransform,colShape);
-
-
- }
- }
+ //create a set of boxes to represent bridge
+ btAlignedObjectArray<btRigidBody*> boxes;
+ int lastBoxIndex = TOTAL_PLANKS-1;
+ for(int i=0;i<TOTAL_PLANKS;++i) {
+ float t = float(i)/lastBoxIndex;
+ t = -(t*2-1.0f) * halfBridgeWidth;
+ startTransform.setOrigin(btVector3(
+ btScalar(t),
+ bridgeHeight,
+ btScalar(0)
+ )
+ );
+ boxes.push_back(createRigidBody((i==0 || i==lastBoxIndex)?0:mass,startTransform,colShape));
+ }
+
+
+ //add N-1 spring constraints
+ for(int i=0;i<TOTAL_PLANKS-1;++i) {
+ btRigidBody* b1 = boxes[i];
+ btRigidBody* b2 = boxes[i+1];
+
+ btPoint2PointConstraint* leftSpring = new btPoint2PointConstraint(*b1, *b2, btVector3(-0.5,0,-0.5), btVector3(0.5,0,-0.5));
+ m_dynamicsWorld->addConstraint(leftSpring);
+
+ btPoint2PointConstraint* rightSpring = new btPoint2PointConstraint(*b1, *b2, btVector3(-0.5,0,0.5), btVector3(0.5,0,0.5));
+ m_dynamicsWorld->addConstraint(rightSpring);
}
}
@@ -125,10 +130,9 @@ void BasicExample::initPhysics()
}
-void BasicExample::renderScene()
+void BridgeExample::renderScene()
{
- CommonRigidBodyBase::renderScene();
-
+ CommonRigidBodyBase::renderScene();
}
@@ -137,9 +141,9 @@ void BasicExample::renderScene()
-CommonExampleInterface* BasicExampleCreateFunc(CommonExampleOptions& options)
+CommonExampleInterface* ET_BridgeCreateFunc(CommonExampleOptions& options)
{
- return new BasicExample(options.m_guiHelper);
+ return new BridgeExample(options.m_guiHelper);
}
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/ExtendedTutorials/Bridge.h
similarity index 75%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/ExtendedTutorials/Bridge.h
index 13c37a4..5f83311 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/ExtendedTutorials/Bridge.h
@@ -13,16 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
+#ifndef ET_BRIDGE_EXAMPLE_H
+#define ET_BRIDGE_EXAMPLE_H
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
+class CommonExampleInterface* ET_BridgeCreateFunc(struct CommonExampleOptions& options);
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+#endif //ET_BRIDGE_EXAMPLE_H
diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/ExtendedTutorials/Chain.cpp
similarity index 64%
copy from examples/BasicDemo/BasicExample.cpp
copy to examples/ExtendedTutorials/Chain.cpp
index d501b51..e78952d 100644
--- a/examples/BasicDemo/BasicExample.cpp
+++ b/examples/ExtendedTutorials/Chain.cpp
@@ -15,26 +15,21 @@ subject to the following restrictions:
-#include "BasicExample.h"
+#include "Chain.h"
#include "btBulletDynamicsCommon.h"
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Z 5
-
#include "LinearMath/btVector3.h"
-#include "LinearMath/btAlignedObjectArray.h"
-
+#include "LinearMath/btAlignedObjectArray.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
-
-struct BasicExample : public CommonRigidBodyBase
+const int TOTAL_BOXES = 10;
+struct ChainExample : public CommonRigidBodyBase
{
- BasicExample(struct GUIHelperInterface* helper)
+ ChainExample(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
{
}
- virtual ~BasicExample(){}
+ virtual ~ChainExample(){}
virtual void initPhysics();
virtual void renderScene();
void resetCamera()
@@ -47,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase
}
};
-void BasicExample::initPhysics()
+void ChainExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
@@ -60,17 +55,11 @@ void BasicExample::initPhysics()
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
-
-
- //groundShape->initializePolyhedralFeatures();
-// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
-
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
- groundTransform.setOrigin(btVector3(0,-50,0));
-
+ groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
@@ -80,11 +69,8 @@ void BasicExample::initPhysics()
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
-
- btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
-
-
- //btCollisionShape* colShape = new btSphereShape(btScalar(1.));
+ btBoxShape* colShape = createBoxShape(btVector3(1,1,0.25));
+
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
@@ -99,25 +85,31 @@ void BasicExample::initPhysics()
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
-
-
- for (int k=0;k<ARRAY_SIZE_Y;k++)
- {
- for (int i=0;i<ARRAY_SIZE_X;i++)
- {
- for(int j = 0;j<ARRAY_SIZE_Z;j++)
- {
- startTransform.setOrigin(btVector3(
- btScalar(2.0*i),
- btScalar(20+2.0*k),
- btScalar(2.0*j)));
-
-
- createRigidBody(mass,startTransform,colShape);
-
-
- }
- }
+
+ btAlignedObjectArray<btRigidBody*> boxes;
+ int lastBoxIndex = TOTAL_BOXES-1;
+ for(int i=0;i<TOTAL_BOXES;++i) {
+ startTransform.setOrigin(btVector3(
+ btScalar(0),
+ btScalar(5+i*2),
+ btScalar(0)
+ )
+ );
+ boxes.push_back(createRigidBody((i==lastBoxIndex)?0:mass,startTransform,colShape));
+ }
+
+ //add N-1 spring constraints
+ for(int i=0;i<TOTAL_BOXES-1;++i) {
+ btRigidBody* b1 = boxes[i];
+ btRigidBody* b2 = boxes[i+1];
+
+ btPoint2PointConstraint* leftSpring = new btPoint2PointConstraint(*b1, *b2, btVector3(-0.5,1,0), btVector3(-0.5,-1,0));
+
+ m_dynamicsWorld->addConstraint(leftSpring);
+
+ btPoint2PointConstraint* rightSpring = new btPoint2PointConstraint(*b1, *b2, btVector3(0.5,1,0), btVector3(0.5,-1,0));
+
+ m_dynamicsWorld->addConstraint(rightSpring);
}
}
@@ -125,10 +117,9 @@ void BasicExample::initPhysics()
}
-void BasicExample::renderScene()
+void ChainExample::renderScene()
{
- CommonRigidBodyBase::renderScene();
-
+ CommonRigidBodyBase::renderScene();
}
@@ -137,9 +128,9 @@ void BasicExample::renderScene()
-CommonExampleInterface* BasicExampleCreateFunc(CommonExampleOptions& options)
+CommonExampleInterface* ET_ChainCreateFunc(CommonExampleOptions& options)
{
- return new BasicExample(options.m_guiHelper);
+ return new ChainExample(options.m_guiHelper);
}
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/ExtendedTutorials/Chain.h
similarity index 75%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/ExtendedTutorials/Chain.h
index 13c37a4..b9cea11 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/ExtendedTutorials/Chain.h
@@ -13,16 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
+#ifndef ET_CHAIN_EXAMPLE_H
+#define ET_CHAIN_EXAMPLE_H
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
+class CommonExampleInterface* ET_ChainCreateFunc(struct CommonExampleOptions& options);
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+#endif //ET_CHAIN_EXAMPLE_H
diff --git a/examples/ExtendedTutorials/InclinedPlane.cpp b/examples/ExtendedTutorials/InclinedPlane.cpp
new file mode 100644
index 0000000..dceafdb
--- /dev/null
+++ b/examples/ExtendedTutorials/InclinedPlane.cpp
@@ -0,0 +1,372 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "InclinedPlane.h"
+
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+
+static btScalar gTilt = 20.0f/180.0f*SIMD_PI; // tilt the ramp 20 degrees
+
+static btScalar gRampFriction = 1; // set ramp friction to 1
+
+static btScalar gRampRestitution = 0; // set ramp restitution to 0 (no restitution)
+
+static btScalar gBoxFriction = 1; // set box friction to 1
+
+static btScalar gBoxRestitution = 0; // set box restitution to 0
+
+static btScalar gSphereFriction = 1; // set sphere friction to 1
+
+static btScalar gSphereRollingFriction = 1; // set sphere rolling friction to 1
+
+static btScalar gSphereRestitution = 0; // set sphere restitution to 0
+
+// handles for changes
+static btRigidBody* ramp = NULL;
+static btRigidBody* gBox = NULL;
+static btRigidBody* gSphere = NULL;
+
+struct InclinedPlaneExample : public CommonRigidBodyBase
+{
+ InclinedPlaneExample(struct GUIHelperInterface* helper)
+ :CommonRigidBodyBase(helper)
+ {
+ }
+ virtual ~InclinedPlaneExample(){}
+ virtual void initPhysics();
+ virtual void resetScene();
+ virtual void renderScene();
+ virtual void stepSimulation(float deltaTime);
+ virtual bool keyboardCallback(int key, int state);
+ void resetCamera()
+ {
+ float dist = 41;
+ float pitch = 52;
+ float yaw = 35;
+ float targetPos[3]={0,0.46,0};
+ m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+ }
+
+
+
+};
+
+void onBoxFrictionChanged(float friction);
+
+void onBoxRestitutionChanged(float restitution);
+
+void onSphereFrictionChanged(float friction);
+
+void onSphereRestitutionChanged(float restitution);
+
+void onRampInclinationChanged(float inclination);
+
+void onRampFrictionChanged(float friction);
+
+void onRampRestitutionChanged(float restitution);
+
+void InclinedPlaneExample::initPhysics()
+{
+
+ { // create slider to change the ramp tilt
+ SliderParams slider("Ramp Tilt",&gTilt);
+ slider.m_minVal=0;
+ slider.m_maxVal=SIMD_PI/2.0f;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onRampInclinationChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+
+ { // create slider to change the ramp friction
+ SliderParams slider("Ramp Friction",&gRampFriction);
+ slider.m_minVal=0;
+ slider.m_maxVal=10;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onRampFrictionChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+
+ { // create slider to change the ramp restitution
+ SliderParams slider("Ramp Restitution",&gRampRestitution);
+ slider.m_minVal=0;
+ slider.m_maxVal=1;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onRampRestitutionChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+
+ { // create slider to change the box friction
+ SliderParams slider("Box Friction",&gBoxFriction);
+ slider.m_minVal=0;
+ slider.m_maxVal=10;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onBoxFrictionChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+
+ { // create slider to change the box restitution
+ SliderParams slider("Box Restitution",&gBoxRestitution);
+ slider.m_minVal=0;
+ slider.m_maxVal=1;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onBoxRestitutionChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+
+ { // create slider to change the sphere friction
+ SliderParams slider("Sphere Friction",&gSphereFriction);
+ slider.m_minVal=0;
+ slider.m_maxVal=10;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onSphereFrictionChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+
+ { // create slider to change the sphere rolling friction
+ SliderParams slider("Sphere Rolling Friction",&gSphereRollingFriction);
+ slider.m_minVal=0;
+ slider.m_maxVal=10;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onSphereRestitutionChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+
+ { // create slider to change the sphere restitution
+ SliderParams slider("Sphere Restitution",&gSphereRestitution);
+ slider.m_minVal=0;
+ slider.m_maxVal=1;
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+
+ m_guiHelper->setUpAxis(1); // set Y axis as up axis
+
+ createEmptyDynamicsWorld();
+
+ // create debug drawer
+ m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+ if (m_dynamicsWorld->getDebugDrawer())
+ m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
+
+
+ { // create a static ground
+ btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
+ m_collisionShapes.push_back(groundShape);
+
+ btTransform groundTransform;
+ groundTransform.setIdentity();
+ groundTransform.setOrigin(btVector3(0,-50,0));
+
+ btScalar mass(0.);
+ createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
+ }
+
+ { //create a static inclined plane
+ btBoxShape* inclinedPlaneShape = createBoxShape(btVector3(btScalar(20.),btScalar(1.),btScalar(10.)));
+ m_collisionShapes.push_back(inclinedPlaneShape);
+
+ btTransform startTransform;
+ startTransform.setIdentity();
+
+ // position the inclined plane above ground
+ startTransform.setOrigin(btVector3(
+ btScalar(0),
+ btScalar(15),
+ btScalar(0)));
+
+ btQuaternion incline;
+ incline.setRotation(btVector3(0,0,1),gTilt);
+ startTransform.setRotation(incline);
+
+ btScalar mass(0.);
+ ramp = createRigidBody(mass,startTransform,inclinedPlaneShape);
+ ramp->setFriction(gRampFriction);
+ ramp->setRestitution(gRampRestitution);
+ }
+
+
+ { //create a cube above the inclined plane
+ btBoxShape* boxShape = createBoxShape(btVector3(1,1,1));
+
+ m_collisionShapes.push_back(boxShape);
+
+ btTransform startTransform;
+ startTransform.setIdentity();
+
+ btScalar boxMass(1.f);
+
+ startTransform.setOrigin(
+ btVector3(btScalar(0), btScalar(20), btScalar(2)));
+
+ gBox = createRigidBody(boxMass, startTransform, boxShape);
+ gBox->forceActivationState(DISABLE_DEACTIVATION); // to prevent the box on the ramp from disabling
+ gBox->setFriction(gBoxFriction);
+ gBox->setRestitution(gBoxRestitution);
+ }
+
+ { //create a sphere above the inclined plane
+ btSphereShape* sphereShape = new btSphereShape(btScalar(1));
+
+ m_collisionShapes.push_back(sphereShape);
+
+ btTransform startTransform;
+ startTransform.setIdentity();
+
+ btScalar sphereMass(1.f);
+
+ startTransform.setOrigin(
+ btVector3(btScalar(0), btScalar(20), btScalar(4)));
+
+ gSphere = createRigidBody(sphereMass, startTransform, sphereShape);
+ gSphere->forceActivationState(DISABLE_DEACTIVATION); // to prevent the sphere on the ramp from disabling
+ gSphere->setFriction(gSphereFriction);
+ gSphere->setRestitution(gSphereRestitution);
+ gSphere->setRollingFriction(gSphereRollingFriction);
+ }
+
+ m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void InclinedPlaneExample::resetScene() {
+ { //reset a cube above the inclined plane
+
+ btTransform startTransform;
+ startTransform.setIdentity();
+
+ startTransform.setOrigin(
+ btVector3(btScalar(0), btScalar(20), btScalar(2)));
+
+ gBox->setWorldTransform(startTransform);
+ btVector3 zero(0, 0, 0);
+ gBox->setAngularVelocity(zero);
+ gBox->setLinearVelocity(zero);
+ gBox->clearForces();
+ }
+
+ { //reset a sphere above the inclined plane
+ btTransform startTransform;
+ startTransform.setIdentity();
+
+ startTransform.setOrigin(
+ btVector3(btScalar(0), btScalar(20), btScalar(4)));
+
+ gSphere->setWorldTransform(startTransform);
+ btVector3 zero(0, 0, 0);
+ gSphere->setAngularVelocity(zero);
+ gSphere->setLinearVelocity(zero);
+ gSphere->clearForces();
+ }
+}
+
+void InclinedPlaneExample::stepSimulation(float deltaTime)
+{
+ if (m_dynamicsWorld)
+ {
+ m_dynamicsWorld->stepSimulation(deltaTime);
+ }
+
+}
+
+
+void InclinedPlaneExample::renderScene()
+{
+ CommonRigidBodyBase::renderScene();
+}
+
+bool InclinedPlaneExample::keyboardCallback(int key, int state) {
+// b3Printf("Key pressed: %d in state %d \n",key,state);
+
+ switch (key) {
+ case 32 /*ASCII for space*/: {
+ resetScene();
+ break;
+ }
+ }
+
+ return false;
+}
+
+
+// GUI parameter modifiers
+void onBoxFrictionChanged(float friction){
+ if(gBox){
+ gBox->setFriction(friction);
+// b3Printf("Friction of box changed to %f",friction );
+ }
+}
+
+void onBoxRestitutionChanged(float restitution){
+ if(gBox){
+ gBox->setRestitution(restitution);
+ //b3Printf("Restitution of box changed to %f",restitution);
+ }
+}
+
+void onSphereFrictionChanged(float friction){
+ if(gSphere){
+ gSphere->setFriction(friction);
+ //b3Printf("Friction of sphere changed to %f",friction );
+ }
+}
+
+void onSphereRestitutionChanged(float restitution){
+ if(gSphere){
+ gSphere->setRestitution(restitution);
+ //b3Printf("Restitution of sphere changed to %f",restitution);
+ }
+}
+
+void onRampInclinationChanged(float inclination){
+ if(ramp){
+ btTransform startTransform;
+ startTransform.setIdentity();
+
+ // position the inclined plane above ground
+ startTransform.setOrigin(
+ btVector3(btScalar(0), btScalar(15), btScalar(0)));
+
+ btQuaternion incline;
+ incline.setRotation(btVector3(0,0,1),gTilt);
+ startTransform.setRotation(incline);
+ ramp->setWorldTransform(startTransform);
+ //b3Printf("Inclination of ramp changed to %f",inclination );
+ }
+}
+
+void onRampFrictionChanged(float friction){
+ if(ramp){
+ ramp->setFriction(friction);
+ //b3Printf("Friction of ramp changed to %f \n",friction );
+ }
+}
+
+void onRampRestitutionChanged(float restitution){
+ if(ramp){
+ ramp->setRestitution(restitution);
+ //b3Printf("Restitution of ramp changed to %f \n",restitution);
+ }
+}
+
+
+CommonExampleInterface* ET_InclinedPlaneCreateFunc(CommonExampleOptions& options)
+{
+ return new InclinedPlaneExample(options.m_guiHelper);
+}
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/ExtendedTutorials/InclinedPlane.h
similarity index 75%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/ExtendedTutorials/InclinedPlane.h
index 13c37a4..23ea92d 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/ExtendedTutorials/InclinedPlane.h
@@ -1,28 +1,22 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2015 Google Inc. http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
-
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
-
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef ET_INCLINED_PLANE_EXAMPLE_H
+#define ET_INCLINED_PLANE_EXAMPLE_H
+
+class CommonExampleInterface* ET_InclinedPlaneCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif //ET_INCLINED_PLANE_EXAMPLE_H
diff --git a/examples/ExtendedTutorials/MultiPendulum.cpp b/examples/ExtendedTutorials/MultiPendulum.cpp
new file mode 100644
index 0000000..038aab3
--- /dev/null
+++ b/examples/ExtendedTutorials/MultiPendulum.cpp
@@ -0,0 +1,435 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "MultiPendulum.h"
+
+#include <vector> // TODO: Should I use another data structure?
+#include <iterator>
+
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+
+static btScalar gPendulaQty = 2; //TODO: This would actually be an Integer, but the Slider does not like integers, so I floor it when changed
+
+static btScalar gDisplacedPendula = 1; //TODO: This is an int as well
+
+static btScalar gPendulaRestitution = 1; // Default pendulum restitution is 1 to restore all force
+
+static btScalar gSphereRadius = 1; // The sphere radius
+
+static btScalar gCurrentPendulumLength = 8;
+
+static btScalar gInitialPendulumLength = 8; // Default pendulum length (distance between two spheres)
+
+static btScalar gDisplacementForce = 30; // The default force with which we move the pendulum
+
+static btScalar gForceScalar = 0; // default force scalar to apply a displacement
+
+struct MultiPendulumExample: public CommonRigidBodyBase {
+ MultiPendulumExample(struct GUIHelperInterface* helper) :
+ CommonRigidBodyBase(helper) {
+ }
+
+ virtual ~MultiPendulumExample() {
+ }
+
+ virtual void initPhysics(); // build a multi pendulum
+ virtual void renderScene(); // render the scene to screen
+ virtual void createMultiPendulum(btSphereShape* colShape, btScalar pendulaQty, const btVector3& position, btScalar length, btScalar mass); // create a multi pendulum at the indicated x and y position, the specified number of pendula formed into a chain, each with indicated length and mass
+ virtual void changePendulaLength(btScalar length); // change the pendulum length
+ virtual void changePendulaRestitution(btScalar restitution); // change the pendula restitution
+ virtual void stepSimulation(float deltaTime); // step the simulation
+ virtual bool keyboardCallback(int key, int state); // handle keyboard callbacks
+ virtual void applyPendulumForce(btScalar pendulumForce);
+ void resetCamera() {
+ float dist = 41;
+ float pitch = 52;
+ float yaw = 35;
+ float targetPos[3] = { 0, 0.46, 0 };
+ m_guiHelper->resetCamera(dist, pitch, yaw, targetPos[0], targetPos[1],
+ targetPos[2]);
+ }
+
+ std::vector<btSliderConstraint*> constraints; // keep a handle to the slider constraints
+ std::vector<btRigidBody*> pendula; // keep a handle to the pendula
+};
+
+static MultiPendulumExample* mex = NULL; // Handle to the example to access it via functions. Do not use this in your simulation!
+
+void onMultiPendulaLengthChanged(float pendulaLength); // Change the pendula length
+
+void onMultiPendulaRestitutionChanged(float pendulaRestitution); // change the pendula restitution
+
+void floorMSliderValue(float notUsed); // floor the slider values which should be integers
+
+void applyMForceWithForceScalar(float forceScalar);
+
+void MultiPendulumExample::initPhysics() { // Setup your physics scene
+
+ { // create a slider to change the number of pendula
+ SliderParams slider("Number of Pendula", &gPendulaQty);
+ slider.m_minVal = 1;
+ slider.m_maxVal = 50;
+ slider.m_callback = floorMSliderValue; // hack to get integer values
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the number of displaced pendula
+ SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula);
+ slider.m_minVal = 0;
+ slider.m_maxVal = 49;
+ slider.m_callback = floorMSliderValue; // hack to get integer values
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the pendula restitution
+ SliderParams slider("Pendula Restitution", &gPendulaRestitution);
+ slider.m_minVal = 0;
+ slider.m_maxVal = 1;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onMultiPendulaRestitutionChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the pendulum length
+ SliderParams slider("Pendula Length", &gCurrentPendulumLength);
+ slider.m_minVal = 0;
+ slider.m_maxVal = 49;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onMultiPendulaLengthChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the force to displace the lowest pendulum
+ SliderParams slider("Displacement force", &gDisplacementForce);
+ slider.m_minVal = 0.1;
+ slider.m_maxVal = 200;
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to apply the force by slider
+ SliderParams slider("Apply displacement force", &gForceScalar);
+ slider.m_minVal = -1;
+ slider.m_maxVal = 1;
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ m_guiHelper->setUpAxis(1);
+
+ createEmptyDynamicsWorld();
+
+ // create a debug drawer
+ m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+ if (m_dynamicsWorld->getDebugDrawer())
+ m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+ btIDebugDraw::DBG_DrawWireframe
+ + btIDebugDraw::DBG_DrawContactPoints
+ + btIDebugDraw::DBG_DrawConstraints
+ + btIDebugDraw::DBG_DrawConstraintLimits);
+
+ { // create the multipendulum starting at the indicated position below and where each pendulum has the following mass
+ btScalar pendulumMass(1.f);
+
+ btVector3 position(0.0f,15.0f,0.0f); // initial top-most pendulum position
+
+ // Re-using the same collision is better for memory usage and performance
+ btSphereShape* pendulumShape = new btSphereShape(gSphereRadius);
+ m_collisionShapes.push_back(pendulumShape);
+
+ // create multi-pendulum
+ createMultiPendulum(pendulumShape, floor(gPendulaQty), position,
+ gInitialPendulumLength, pendulumMass);
+ }
+
+ m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void MultiPendulumExample::stepSimulation(float deltaTime) {
+
+ applyMForceWithForceScalar(gForceScalar); // apply force defined by apply force slider
+
+ if (m_dynamicsWorld) {
+ m_dynamicsWorld->stepSimulation(deltaTime);
+ }
+}
+
+void MultiPendulumExample::createMultiPendulum(btSphereShape* colShape,
+ btScalar pendulaQty, const btVector3& position,
+ btScalar length, btScalar mass) {
+
+ // The multi-pendulum looks like this (names when built):
+ //..........0......./.......1...../.......2......./..etc...:pendulum build iterations
+ // O parentSphere
+ // |
+ // O childSphere / parentSphere
+ // |
+ // O ............./ childSphere / parentSphere
+ // |
+ // O .........................../ childSphere
+ // etc.
+
+ //create the top element of the pendulum
+ btTransform startTransform;
+ startTransform.setIdentity();
+
+ // position the top sphere
+ startTransform.setOrigin(position);
+
+ startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation
+
+ btRigidBody* topSphere = createRigidBody(mass, startTransform, colShape);
+
+ // disable the deactivation when object does not move anymore
+ topSphere->setActivationState(DISABLE_DEACTIVATION);
+
+ //make top sphere position "fixed" in the world by attaching it to a the world with a point to point constraint
+ // The pivot is defined in the reference frame of topSphere, so the attachment should be exactly at the center of topSphere
+ btVector3 constraintPivot(0.0f, 0.0f, 0.0f);
+ btPoint2PointConstraint* p2pconst = new btPoint2PointConstraint(
+ *topSphere, constraintPivot);
+
+ p2pconst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing
+
+ // add the constraint to the world
+ m_dynamicsWorld->addConstraint(p2pconst, true);
+
+ btRigidBody* parentSphere = topSphere; // set the top sphere as the parent sphere for the next sphere to be created
+
+ for (int i = 0; i < pendulaQty; i++) { // produce the number of pendula
+
+ // create joint element to make the pendulum rotate it
+
+ // position the joint sphere at the same position as the top sphere
+ startTransform.setOrigin(position - btVector3(0,length*(i),0));
+
+ startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation
+
+ btRigidBody* jointSphere = createRigidBody(mass, startTransform,
+ colShape);
+ jointSphere->setFriction(0); // we do not need friction here
+
+ // disable the deactivation when object does not move anymore
+ jointSphere->setActivationState(DISABLE_DEACTIVATION);
+
+ //create constraint between parentSphere and jointSphere
+ // this is represented by the constraint pivot in the local frames of reference of both constrained spheres
+ btTransform constraintPivotInParentSphereRF, constraintPivotInJointSphereRF;
+
+ constraintPivotInParentSphereRF.setIdentity();
+ constraintPivotInJointSphereRF.setIdentity();
+
+ // the orientation of a point-to-point constraint does not matter, as is has no rotational limits
+
+ //Obtain the position of parentSphere in local reference frame of the jointSphere (the pivot is therefore in the center of parentSphere)
+ btVector3 parentSphereInJointSphereRF =
+ (jointSphere->getWorldTransform().inverse()(
+ parentSphere->getWorldTransform().getOrigin()));
+ constraintPivotInJointSphereRF.setOrigin(parentSphereInJointSphereRF);
+
+ btPoint2PointConstraint* p2pconst = new btPoint2PointConstraint(
+ *parentSphere,*jointSphere,constraintPivotInParentSphereRF.getOrigin(), constraintPivotInJointSphereRF.getOrigin());
+
+ p2pconst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing
+
+ // add the constraint to the world
+ m_dynamicsWorld->addConstraint(p2pconst, true);
+
+ // create a slider constraint to change the length of the pendula while it swings
+
+ startTransform.setIdentity(); // reset start transform
+
+ // position the child sphere below the joint sphere
+ startTransform.setOrigin(position - btVector3(0,length*(i+1),0));
+
+ startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation
+
+ btRigidBody* childSphere = createRigidBody(mass, startTransform,
+ colShape);
+ childSphere->setFriction(0); // we do not need friction here
+ pendula.push_back(childSphere);
+
+ // disable the deactivation when object does not move anymore
+ childSphere->setActivationState(DISABLE_DEACTIVATION);
+
+ //create slider constraint between jointSphere and childSphere
+ // this is represented by the constraint pivot in the local frames of reference of both constrained spheres
+ // furthermore we need to rotate the constraint appropriately to orient it correctly in space
+ btTransform constraintPivotInChildSphereRF;
+
+ constraintPivotInJointSphereRF.setIdentity();
+ constraintPivotInChildSphereRF.setIdentity();
+
+ // the orientation of a point-to-point constraint does not matter, as is has no rotational limits
+
+ //Obtain the position of jointSphere in local reference frame of the childSphere (the pivot is therefore in the center of jointSphere)
+ btVector3 jointSphereInChildSphereRF =
+ (childSphere->getWorldTransform().inverse()(
+ jointSphere->getWorldTransform().getOrigin()));
+ constraintPivotInChildSphereRF.setOrigin(jointSphereInChildSphereRF);
+
+ // the slider constraint is x aligned per default, but we want it to be y aligned, therefore we rotate it
+ btQuaternion qt;
+ qt.setEuler(0, 0, -SIMD_HALF_PI);
+ constraintPivotInJointSphereRF.setRotation(qt); //we use Y like up Axis
+ constraintPivotInChildSphereRF.setRotation(qt); //we use Y like up Axis
+
+ btSliderConstraint* sliderConst = new btSliderConstraint(*jointSphere,
+ *childSphere, constraintPivotInJointSphereRF, constraintPivotInChildSphereRF, true);
+
+ sliderConst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing
+
+ // set limits
+ // the initial setup of the constraint defines the origins of the limit dimensions,
+ // therefore we set both limits directly to the current position of the parentSphere
+ sliderConst->setLowerLinLimit(btScalar(0));
+ sliderConst->setUpperLinLimit(btScalar(0));
+ sliderConst->setLowerAngLimit(btScalar(0));
+ sliderConst->setUpperAngLimit(btScalar(0));
+ constraints.push_back(sliderConst);
+
+ // add the constraint to the world
+ m_dynamicsWorld->addConstraint(sliderConst, true);
+ parentSphere = childSphere;
+ }
+}
+
+void MultiPendulumExample::changePendulaLength(btScalar length) {
+ btScalar lowerLimit = -gInitialPendulumLength;
+ for (std::vector<btSliderConstraint*>::iterator sit = constraints.begin();
+ sit != constraints.end(); sit++) {
+ btAssert((*sit) && "Null constraint");
+
+ // if the pendulum is being shortened beyond it's own length, we don't let the lower sphere to go past the upper one
+ if (lowerLimit <= length) {
+ (*sit)->setLowerLinLimit(length + lowerLimit);
+ (*sit)->setUpperLinLimit(length + lowerLimit);
+ }
+ }
+}
+
+void MultiPendulumExample::changePendulaRestitution(btScalar restitution) {
+ for (std::vector<btRigidBody*>::iterator rit = pendula.begin();
+ rit != pendula.end(); rit++) {
+ btAssert((*rit) && "Null constraint");
+
+ (*rit)->setRestitution(restitution);
+ }
+}
+
+void MultiPendulumExample::renderScene() {
+ CommonRigidBodyBase::renderScene();
+}
+
+bool MultiPendulumExample::keyboardCallback(int key, int state) {
+
+ //b3Printf("Key pressed: %d in state %d \n",key,state);
+
+ //key 1, key 2, key 3
+ switch (key) {
+ case '1' /*ASCII for 1*/: {
+
+ //assumption: Sphere are aligned in Z axis
+ btScalar newLimit = btScalar(gCurrentPendulumLength + 0.1);
+
+ changePendulaLength(newLimit);
+ gCurrentPendulumLength = newLimit;
+
+ b3Printf("Increase pendulum length to %f", gCurrentPendulumLength);
+ return true;
+ }
+ case '2' /*ASCII for 2*/: {
+
+ //assumption: Sphere are aligned in Z axis
+ btScalar newLimit = btScalar(gCurrentPendulumLength - 0.1);
+
+ //is being shortened beyond it's own length, we don't let the lower sphere to go over the upper one
+ if (0 <= newLimit) {
+ changePendulaLength(newLimit);
+ gCurrentPendulumLength = newLimit;
+ }
+
+ b3Printf("Decrease pendulum length to %f", gCurrentPendulumLength);
+ return true;
+ }
+ case '3' /*ASCII for 3*/: {
+ applyPendulumForce(gDisplacementForce);
+ return true;
+ }
+ }
+
+ return false;
+}
+
+void MultiPendulumExample::applyPendulumForce(btScalar pendulumForce){
+ if(pendulumForce != 0){
+ b3Printf("Apply %f to pendulum",pendulumForce);
+ for (int i = 0; i < gDisplacedPendula; i++) {
+ if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty)
+ pendula[i]->applyCentralForce(btVector3(pendulumForce, 0, 0));
+ }
+ }
+}
+
+// GUI parameter modifiers
+
+void onMultiPendulaLengthChanged(float pendulaLength) { // Change the pendula length
+ if (mex){
+ mex->changePendulaLength(pendulaLength);
+ }
+ //b3Printf("Pendula length changed to %f \n",sliderValue );
+
+}
+
+void onMultiPendulaRestitutionChanged(float pendulaRestitution) { // change the pendula restitution
+ if (mex){
+ mex->changePendulaRestitution(pendulaRestitution);
+ }
+
+}
+
+void floorMSliderValue(float notUsed) { // floor the slider values which should be integers
+ gPendulaQty = floor(gPendulaQty);
+ gDisplacedPendula = floor(gDisplacedPendula);
+}
+
+void applyMForceWithForceScalar(float forceScalar) {
+ if(mex){
+ btScalar appliedForce = forceScalar * gDisplacementForce;
+
+ if(fabs(gForceScalar) < 0.2f)
+ gForceScalar = 0;
+
+ mex->applyPendulumForce(appliedForce);
+ }
+}
+
+CommonExampleInterface* ET_MultiPendulumCreateFunc(
+ CommonExampleOptions& options) {
+ mex = new MultiPendulumExample(options.m_guiHelper);
+ return mex;
+}
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/ExtendedTutorials/MultiPendulum.h
similarity index 75%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/ExtendedTutorials/MultiPendulum.h
index 13c37a4..336c1be 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/ExtendedTutorials/MultiPendulum.h
@@ -1,28 +1,22 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2015 Google Inc. http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
-
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
-
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef ET_MULTI_PENDULUM_EXAMPLE_H
+#define ET_MULTI_PENDULUM_EXAMPLE_H
+
+class CommonExampleInterface* ET_MultiPendulumCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif //ET_MULTI_PENDULUM_EXAMPLE_H
diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/ExtendedTutorials/MultipleBoxes.cpp
similarity index 69%
copy from examples/BasicDemo/BasicExample.cpp
copy to examples/ExtendedTutorials/MultipleBoxes.cpp
index d501b51..c955593 100644
--- a/examples/BasicDemo/BasicExample.cpp
+++ b/examples/ExtendedTutorials/MultipleBoxes.cpp
@@ -15,26 +15,21 @@ subject to the following restrictions:
-#include "BasicExample.h"
+#include "MultipleBoxes.h"
#include "btBulletDynamicsCommon.h"
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Z 5
-
#include "LinearMath/btVector3.h"
-#include "LinearMath/btAlignedObjectArray.h"
-
+#include "LinearMath/btAlignedObjectArray.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
-
-struct BasicExample : public CommonRigidBodyBase
+const int TOTAL_BOXES = 10;
+struct MultipleBoxesExample : public CommonRigidBodyBase
{
- BasicExample(struct GUIHelperInterface* helper)
+ MultipleBoxesExample(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
{
}
- virtual ~BasicExample(){}
+ virtual ~MultipleBoxesExample(){}
virtual void initPhysics();
virtual void renderScene();
void resetCamera()
@@ -47,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase
}
};
-void BasicExample::initPhysics()
+void MultipleBoxesExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
@@ -60,17 +55,11 @@ void BasicExample::initPhysics()
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
-
-
- //groundShape->initializePolyhedralFeatures();
-// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
-
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
- groundTransform.setOrigin(btVector3(0,-50,0));
-
+ groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
@@ -80,11 +69,8 @@ void BasicExample::initPhysics()
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
-
- btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
-
-
- //btCollisionShape* colShape = new btSphereShape(btScalar(1.));
+ btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
+
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
@@ -100,24 +86,13 @@ void BasicExample::initPhysics()
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
-
- for (int k=0;k<ARRAY_SIZE_Y;k++)
- {
- for (int i=0;i<ARRAY_SIZE_X;i++)
- {
- for(int j = 0;j<ARRAY_SIZE_Z;j++)
- {
- startTransform.setOrigin(btVector3(
- btScalar(2.0*i),
- btScalar(20+2.0*k),
- btScalar(2.0*j)));
-
-
- createRigidBody(mass,startTransform,colShape);
-
-
- }
- }
+
+ for(int i=0;i<TOTAL_BOXES;++i) {
+ startTransform.setOrigin(btVector3(
+ btScalar(0),
+ btScalar(20+i*2),
+ btScalar(0)));
+ createRigidBody(mass,startTransform,colShape);
}
}
@@ -125,10 +100,9 @@ void BasicExample::initPhysics()
}
-void BasicExample::renderScene()
+void MultipleBoxesExample::renderScene()
{
- CommonRigidBodyBase::renderScene();
-
+ CommonRigidBodyBase::renderScene();
}
@@ -137,9 +111,9 @@ void BasicExample::renderScene()
-CommonExampleInterface* BasicExampleCreateFunc(CommonExampleOptions& options)
+CommonExampleInterface* ET_MultipleBoxesCreateFunc(CommonExampleOptions& options)
{
- return new BasicExample(options.m_guiHelper);
+ return new MultipleBoxesExample(options.m_guiHelper);
}
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/ExtendedTutorials/MultipleBoxes.h
similarity index 75%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/ExtendedTutorials/MultipleBoxes.h
index 13c37a4..56e28b6 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/ExtendedTutorials/MultipleBoxes.h
@@ -13,16 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
+#ifndef ET_MULTIPLE_BOXES_EXAMPLE_H
+#define ET_MULTIPLE_BOXES_EXAMPLE_H
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
+class CommonExampleInterface* ET_MultipleBoxesCreateFunc(struct CommonExampleOptions& options);
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+#endif //ET_MULTIPLE_BOXES_EXAMPLE_H
diff --git a/examples/ExtendedTutorials/NewtonsCradle.cpp b/examples/ExtendedTutorials/NewtonsCradle.cpp
new file mode 100644
index 0000000..0d4c520
--- /dev/null
+++ b/examples/ExtendedTutorials/NewtonsCradle.cpp
@@ -0,0 +1,380 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "NewtonsCradle.h"
+
+#include <vector> // TODO: Should I use another data structure?
+#include <iterator>
+
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+
+static btScalar gPendulaQty = 5; // Number of pendula in newton's cradle
+//TODO: This would actually be an Integer, but the Slider does not like integers, so I floor it when changed
+
+static btScalar gDisplacedPendula = 1; // number of displaced pendula
+//TODO: This is an int as well
+
+static btScalar gPendulaRestitution = 1; // pendula restitution when hitting against each other
+
+static btScalar gSphereRadius = 1; // pendula radius
+
+static btScalar gCurrentPendulumLength = 8; // current pendula length
+
+static btScalar gInitialPendulumLength = 8; // default pendula length
+
+static btScalar gDisplacementForce = 30; // default force to displace the pendula
+
+static btScalar gForceScalar = 0; // default force scalar to apply a displacement
+
+struct NewtonsCradleExample: public CommonRigidBodyBase {
+ NewtonsCradleExample(struct GUIHelperInterface* helper) :
+ CommonRigidBodyBase(helper) {
+ }
+ virtual ~NewtonsCradleExample() {
+ }
+ virtual void initPhysics();
+ virtual void renderScene();
+ virtual void createPendulum(btSphereShape* colShape, const btVector3& position, btScalar length, btScalar mass);
+ virtual void changePendulaLength(btScalar length);
+ virtual void changePendulaRestitution(btScalar restitution);
+ virtual void stepSimulation(float deltaTime);
+ virtual bool keyboardCallback(int key, int state);
+ virtual void applyPendulumForce(btScalar pendulumForce);
+ void resetCamera() {
+ float dist = 41;
+ float pitch = 52;
+ float yaw = 35;
+ float targetPos[3] = { 0, 0.46, 0 };
+ m_guiHelper->resetCamera(dist, pitch, yaw, targetPos[0], targetPos[1],
+ targetPos[2]);
+ }
+
+ std::vector<btSliderConstraint*> constraints; // keep a handle to the slider constraints
+ std::vector<btRigidBody*> pendula; // keep a handle to the pendula
+};
+
+static NewtonsCradleExample* nex = NULL;
+
+void onPendulaLengthChanged(float pendulaLength); // Change the pendula length
+
+void onPendulaRestitutionChanged(float pendulaRestitution); // change the pendula restitution
+
+void floorSliderValue(float notUsed); // floor the slider values which should be integers
+
+void applyForceWithForceScalar(float forceScalar);
+
+void NewtonsCradleExample::initPhysics() {
+
+ { // create a slider to change the number of pendula
+ SliderParams slider("Number of Pendula", &gPendulaQty);
+ slider.m_minVal = 1;
+ slider.m_maxVal = 50;
+ slider.m_callback = floorSliderValue; // hack to get integer values
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the number of displaced pendula
+ SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula);
+ slider.m_minVal = 0;
+ slider.m_maxVal = 49;
+ slider.m_callback = floorSliderValue; // hack to get integer values
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the pendula restitution
+ SliderParams slider("Pendula Restitution", &gPendulaRestitution);
+ slider.m_minVal = 0;
+ slider.m_maxVal = 1;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onPendulaRestitutionChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the pendulum length
+ SliderParams slider("Pendula Length", &gCurrentPendulumLength);
+ slider.m_minVal = 0;
+ slider.m_maxVal = 49;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onPendulaLengthChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the force to displace the lowest pendulum
+ SliderParams slider("Displacement force", &gDisplacementForce);
+ slider.m_minVal = 0.1;
+ slider.m_maxVal = 200;
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to apply the force by slider
+ SliderParams slider("Apply displacement force", &gForceScalar);
+ slider.m_minVal = -1;
+ slider.m_maxVal = 1;
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ m_guiHelper->setUpAxis(1);
+
+ createEmptyDynamicsWorld();
+
+ // create a debug drawer
+ m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+ if (m_dynamicsWorld->getDebugDrawer())
+ m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+ btIDebugDraw::DBG_DrawWireframe
+ + btIDebugDraw::DBG_DrawContactPoints
+ + btIDebugDraw::DBG_DrawConstraints
+ + btIDebugDraw::DBG_DrawConstraintLimits);
+
+ { // create the pendula starting at the indicated position below and where each pendulum has the following mass
+ btScalar pendulumMass(1.f);
+
+ btVector3 position(0.0f,15.0f,0.0f); // initial left-most pendulum position
+ btQuaternion orientation(0,0,0,1); // orientation of the pendula
+
+ // Re-using the same collision is better for memory usage and performance
+ btSphereShape* pendulumShape = new btSphereShape(gSphereRadius);
+ m_collisionShapes.push_back(pendulumShape);
+
+ for (int i = 0; i < floor(gPendulaQty); i++) {
+
+ // create pendulum
+ createPendulum(pendulumShape, position, gInitialPendulumLength, pendulumMass);
+
+ // displace the pendula 1.05 sphere size, so that they all nearly touch (small spacings in between
+ position.setX(position.x()-2.1f * gSphereRadius);
+ }
+ }
+
+ m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void NewtonsCradleExample::stepSimulation(float deltaTime) {
+
+ applyForceWithForceScalar(gForceScalar); // apply force defined by apply force slider
+
+ if (m_dynamicsWorld) {
+ m_dynamicsWorld->stepSimulation(deltaTime);
+ }
+}
+
+void NewtonsCradleExample::createPendulum(btSphereShape* colShape, const btVector3& position, btScalar length, btScalar mass) {
+
+ // The pendulum looks like this (names when built):
+ // O topSphere
+ // |
+ // O bottomSphere
+
+ //create a dynamic pendulum
+ btTransform startTransform;
+ startTransform.setIdentity();
+
+ // position the top sphere above ground with a moving x position
+ startTransform.setOrigin(position);
+ startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation
+ btRigidBody* topSphere = createRigidBody(mass, startTransform, colShape);
+
+ // position the bottom sphere below the top sphere
+ startTransform.setOrigin(
+ btVector3(position.x(), btScalar(position.y() - length),
+ position.z()));
+
+ startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation
+ btRigidBody* bottomSphere = createRigidBody(mass, startTransform, colShape);
+ bottomSphere->setFriction(0); // we do not need friction here
+ pendula.push_back(bottomSphere);
+
+ // disable the deactivation when objects do not move anymore
+ topSphere->setActivationState(DISABLE_DEACTIVATION);
+ bottomSphere->setActivationState(DISABLE_DEACTIVATION);
+
+ bottomSphere->setRestitution(gPendulaRestitution); // set pendula restitution
+
+ //make the top sphere position "fixed" to the world by attaching with a point to point constraint
+ // The pivot is defined in the reference frame of topSphere, so the attachment is exactly at the center of the topSphere
+ btVector3 constraintPivot(btVector3(0.0f, 0.0f, 0.0f));
+ btPoint2PointConstraint* p2pconst = new btPoint2PointConstraint(*topSphere,
+ constraintPivot);
+
+ p2pconst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing
+
+ // add the constraint to the world
+ m_dynamicsWorld->addConstraint(p2pconst, true);
+
+ //create constraint between spheres
+ // this is represented by the constraint pivot in the local frames of reference of both constrained spheres
+ // furthermore we need to rotate the constraint appropriately to orient it correctly in space
+ btTransform constraintPivotInTopSphereRF, constraintPivotInBottomSphereRF;
+
+ constraintPivotInTopSphereRF.setIdentity();
+ constraintPivotInBottomSphereRF.setIdentity();
+
+ // the slider constraint is x aligned per default, but we want it to be y aligned, therefore we rotate it
+ btQuaternion qt;
+ qt.setEuler(0, 0, -SIMD_HALF_PI);
+ constraintPivotInTopSphereRF.setRotation(qt); //we use Y like up Axis
+ constraintPivotInBottomSphereRF.setRotation(qt); //we use Y like up Axis
+
+ //Obtain the position of topSphere in local reference frame of bottomSphere (the pivot is therefore in the center of topSphere)
+ btVector3 topSphereInBottomSphereRF =
+ (bottomSphere->getWorldTransform().inverse()(
+ topSphere->getWorldTransform().getOrigin()));
+ constraintPivotInBottomSphereRF.setOrigin(topSphereInBottomSphereRF);
+
+ btSliderConstraint* sliderConst = new btSliderConstraint(*topSphere,
+ *bottomSphere, constraintPivotInTopSphereRF, constraintPivotInBottomSphereRF, true);
+
+ sliderConst->setDbgDrawSize(btScalar(5.f)); // set the size of the debug drawing
+
+ // set limits
+ // the initial setup of the constraint defines the origins of the limit dimensions,
+ // therefore we set both limits directly to the current position of the topSphere
+ sliderConst->setLowerLinLimit(btScalar(0));
+ sliderConst->setUpperLinLimit(btScalar(0));
+ sliderConst->setLowerAngLimit(btScalar(0));
+ sliderConst->setUpperAngLimit(btScalar(0));
+ constraints.push_back(sliderConst);
+
+ // add the constraint to the world
+ m_dynamicsWorld->addConstraint(sliderConst, true);
+}
+
+void NewtonsCradleExample::changePendulaLength(btScalar length) {
+ btScalar lowerLimit = -gInitialPendulumLength;
+ for (std::vector<btSliderConstraint*>::iterator sit = constraints.begin();
+ sit != constraints.end(); sit++) {
+ btAssert((*sit) && "Null constraint");
+
+ //if the pendulum is being shortened beyond it's own length, we don't let the lower sphere to go past the upper one
+ if (lowerLimit <= length) {
+ (*sit)->setLowerLinLimit(length + lowerLimit);
+ (*sit)->setUpperLinLimit(length + lowerLimit);
+ }
+ }
+}
+
+void NewtonsCradleExample::changePendulaRestitution(btScalar restitution) {
+ for (std::vector<btRigidBody*>::iterator rit = pendula.begin();
+ rit != pendula.end(); rit++) {
+ btAssert((*rit) && "Null constraint");
+
+ (*rit)->setRestitution(restitution);
+ }
+}
+
+void NewtonsCradleExample::renderScene() {
+ CommonRigidBodyBase::renderScene();
+}
+
+bool NewtonsCradleExample::keyboardCallback(int key, int state) {
+ //b3Printf("Key pressed: %d in state %d \n",key,state);
+
+ //key 1, key 2, key 3
+ switch (key) {
+ case '1' /*ASCII for 1*/: {
+
+ //assumption: Sphere are aligned in Z axis
+ btScalar newLimit = btScalar(gCurrentPendulumLength + 0.1);
+
+ changePendulaLength(newLimit);
+ gCurrentPendulumLength = newLimit;
+
+ b3Printf("Increase pendulum length to %f", gCurrentPendulumLength);
+ return true;
+ }
+ case '2' /*ASCII for 2*/: {
+
+ //assumption: Sphere are aligned in Z axis
+ btScalar newLimit = btScalar(gCurrentPendulumLength - 0.1);
+
+ //is being shortened beyond it's own length, we don't let the lower sphere to go over the upper one
+ if (0 <= newLimit) {
+ changePendulaLength(newLimit);
+ gCurrentPendulumLength = newLimit;
+ }
+
+ b3Printf("Decrease pendulum length to %f", gCurrentPendulumLength);
+ return true;
+ }
+ case '3' /*ASCII for 3*/: {
+ applyPendulumForce(gDisplacementForce);
+ return true;
+ }
+ }
+
+ return false;
+}
+
+void NewtonsCradleExample::applyPendulumForce(btScalar pendulumForce){
+ if(pendulumForce != 0){
+ b3Printf("Apply %f to pendulum",pendulumForce);
+ for (int i = 0; i < gDisplacedPendula; i++) {
+ if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty)
+ pendula[i]->applyCentralForce(btVector3(pendulumForce, 0, 0));
+ }
+ }
+}
+
+// GUI parameter modifiers
+
+void onPendulaLengthChanged(float pendulaLength) {
+ if (nex){
+ nex->changePendulaLength(pendulaLength);
+ //b3Printf("Pendula length changed to %f \n",sliderValue );
+ }
+}
+
+void onPendulaRestitutionChanged(float pendulaRestitution) {
+ if (nex){
+ nex->changePendulaRestitution(pendulaRestitution);
+ }
+}
+
+void floorSliderValue(float notUsed) {
+ gPendulaQty = floor(gPendulaQty);
+ gDisplacedPendula = floor(gDisplacedPendula);
+
+}
+
+void applyForceWithForceScalar(float forceScalar) {
+ if(nex){
+ btScalar appliedForce = forceScalar * gDisplacementForce;
+
+ if(fabs(gForceScalar) < 0.2f)
+ gForceScalar = 0;
+
+ nex->applyPendulumForce(appliedForce);
+ }
+}
+
+CommonExampleInterface* ET_NewtonsCradleCreateFunc(
+ CommonExampleOptions& options) {
+ nex = new NewtonsCradleExample(options.m_guiHelper);
+ return nex;
+}
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/ExtendedTutorials/NewtonsCradle.h
similarity index 75%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/ExtendedTutorials/NewtonsCradle.h
index 13c37a4..028754f 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/ExtendedTutorials/NewtonsCradle.h
@@ -1,28 +1,22 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2015 Google Inc. http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
-
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
-
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef ET_NEWTONS_CRADLE_EXAMPLE_H
+#define ET_NEWTONS_CRADLE_EXAMPLE_H
+
+class CommonExampleInterface* ET_NewtonsCradleCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif //ET_NEWTONS_CRADLE_EXAMPLE_H
diff --git a/examples/ExtendedTutorials/NewtonsRopeCradle.cpp b/examples/ExtendedTutorials/NewtonsRopeCradle.cpp
new file mode 100644
index 0000000..94c96a7
--- /dev/null
+++ b/examples/ExtendedTutorials/NewtonsRopeCradle.cpp
@@ -0,0 +1,387 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "NewtonsRopeCradle.h"
+
+#include <vector> // TODO: Should I use another data structure?
+#include <iterator>
+
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+
+#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+
+static btScalar gPendulaQty = 5; // Number of pendula in newton's cradle
+//TODO: This would actually be an Integer, but the Slider does not like integers, so I floor it when changed
+
+static btScalar gDisplacedPendula = 1; // number of displaced pendula
+//TODO: This is an int as well
+
+static btScalar gPendulaRestitution = 1; // pendula restition when hitting against each other
+
+static btScalar gSphereRadius = 1; // pendula radius
+
+static btScalar gInitialPendulumWidth = 4; // default pendula width
+
+static btScalar gInitialPendulumHeight = 8; // default pendula height
+
+static btScalar gRopeResolution = 1; // default rope resolution (number of links as in a chain)
+
+static btScalar gDisplacementForce = 30; // default force to displace the pendula
+
+static btScalar gForceScalar = 0; // default force scalar to apply a displacement
+
+struct NewtonsRopeCradleExample : public CommonRigidBodyBase {
+ NewtonsRopeCradleExample(struct GUIHelperInterface* helper) :
+ CommonRigidBodyBase(helper) {
+ }
+ virtual ~NewtonsRopeCradleExample(){}
+ virtual void initPhysics();
+ virtual void stepSimulation(float deltaTime);
+ virtual void renderScene();
+ virtual void applyPendulumForce(btScalar pendulumForce);
+ void createEmptyDynamicsWorld()
+ {
+ m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+ m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+ m_broadphase = new btDbvtBroadphase();
+
+ m_solver = new btSequentialImpulseConstraintSolver;
+
+ m_dynamicsWorld = new btSoftRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
+ m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
+
+ softBodyWorldInfo.m_broadphase = m_broadphase;
+ softBodyWorldInfo.m_dispatcher = m_dispatcher;
+ softBodyWorldInfo.m_gravity = m_dynamicsWorld->getGravity();
+ softBodyWorldInfo.m_sparsesdf.Initialize();
+ }
+
+ virtual void createRopePendulum(btSphereShape* colShape,
+ const btVector3& position, const btQuaternion& pendulumOrientation, btScalar width, btScalar height, btScalar mass);
+ virtual void changePendulaRestitution(btScalar restitution);
+ virtual void connectWithRope(btRigidBody* body1, btRigidBody* body2);
+ virtual bool keyboardCallback(int key, int state);
+
+ virtual btSoftRigidDynamicsWorld* getSoftDynamicsWorld()
+ {
+ ///just make it a btSoftRigidDynamicsWorld please
+ ///or we will add type checking
+ return (btSoftRigidDynamicsWorld*) m_dynamicsWorld;
+ }
+ void resetCamera()
+ {
+ float dist = 41;
+ float pitch = 52;
+ float yaw = 35;
+ float targetPos[3]={0,0.46,0};
+ m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+ }
+
+ std::vector<btSliderConstraint*> constraints;
+ std::vector<btRigidBody*> pendula;
+
+ btSoftBodyWorldInfo softBodyWorldInfo;
+
+};
+
+static NewtonsRopeCradleExample* nex = NULL;
+
+void onRopePendulaRestitutionChanged(float pendulaRestitution);
+
+void floorRSliderValue(float notUsed);
+
+void applyRForceWithForceScalar(float forceScalar);
+
+void NewtonsRopeCradleExample::initPhysics()
+{
+
+ { // create a slider to change the number of pendula
+ SliderParams slider("Number of Pendula", &gPendulaQty);
+ slider.m_minVal = 1;
+ slider.m_maxVal = 50;
+ slider.m_callback = floorRSliderValue; // hack to get integer values
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the number of displaced pendula
+ SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula);
+ slider.m_minVal = 0;
+ slider.m_maxVal = 49;
+ slider.m_callback = floorRSliderValue; // hack to get integer values
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the pendula restitution
+ SliderParams slider("Pendula Restitution", &gPendulaRestitution);
+ slider.m_minVal = 0;
+ slider.m_maxVal = 1;
+ slider.m_clampToNotches = false;
+ slider.m_callback = onRopePendulaRestitutionChanged;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the rope resolution
+ SliderParams slider("Rope Resolution", &gRopeResolution);
+ slider.m_minVal = 1;
+ slider.m_maxVal = 20;
+ slider.m_clampToNotches = false;
+ slider.m_callback = floorRSliderValue;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the pendulum width
+ SliderParams slider("Pendulum Width", &gInitialPendulumWidth);
+ slider.m_minVal = 0;
+ slider.m_maxVal = 40;
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the pendulum height
+ SliderParams slider("Pendulum Height", &gInitialPendulumHeight);
+ slider.m_minVal = 0;
+ slider.m_maxVal = 40;
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to change the force to displace the lowest pendulum
+ SliderParams slider("Displacement force", &gDisplacementForce);
+ slider.m_minVal = 0.1;
+ slider.m_maxVal = 200;
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ { // create a slider to apply the force by slider
+ SliderParams slider("Apply displacement force", &gForceScalar);
+ slider.m_minVal = -1;
+ slider.m_maxVal = 1;
+ slider.m_clampToNotches = false;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+ slider);
+ }
+
+ m_guiHelper->setUpAxis(1);
+
+ createEmptyDynamicsWorld();
+
+ // create a debug drawer
+ m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+ if (m_dynamicsWorld->getDebugDrawer())
+ m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+ btIDebugDraw::DBG_DrawWireframe
+ + btIDebugDraw::DBG_DrawContactPoints
+ + btIDebugDraw::DBG_DrawConstraints
+ + btIDebugDraw::DBG_DrawConstraintLimits);
+
+ { // create the pendula starting at the indicated position below and where each pendulum has the following mass
+ btScalar pendulumMass(1.0f);
+
+ btVector3 position(0.0f,15.0f,0.0f); // initial left-most pendulum position
+ btQuaternion orientation(0,0,0,1); // orientation of the pendula
+
+ // Re-using the same collision is better for memory usage and performance
+ btSphereShape* pendulumShape = new btSphereShape(gSphereRadius);
+ m_collisionShapes.push_back(pendulumShape);
+
+ for (int i = 0; i < floor(gPendulaQty); i++) {
+
+ // create pendulum
+ createRopePendulum(pendulumShape, position, orientation,gInitialPendulumWidth,
+ gInitialPendulumHeight, pendulumMass);
+
+ // displace the pendula 1.05 sphere size, so that they all nearly touch (small spacings in between)
+ position.setX(position.x()-2.1f * gSphereRadius);
+ }
+ }
+
+ m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void NewtonsRopeCradleExample::connectWithRope(btRigidBody* body1, btRigidBody* body2)
+{
+ btSoftBody* softBodyRope0 = btSoftBodyHelpers::CreateRope(softBodyWorldInfo,body1->getWorldTransform().getOrigin(),body2->getWorldTransform().getOrigin(),gRopeResolution,0);
+ softBodyRope0->setTotalMass(0.1f);
+
+ softBodyRope0->appendAnchor(0,body1);
+ softBodyRope0->appendAnchor(softBodyRope0->m_nodes.size()-1,body2);
+
+ softBodyRope0->m_cfg.piterations = 5;
+ softBodyRope0->m_cfg.kDP = 0.005f;
+ softBodyRope0->m_cfg.kSHR = 1;
+ softBodyRope0->m_cfg.kCHR = 1;
+ softBodyRope0->m_cfg.kKHR = 1;
+
+ getSoftDynamicsWorld()->addSoftBody(softBodyRope0);
+}
+
+void NewtonsRopeCradleExample::stepSimulation(float deltaTime) {
+
+ applyRForceWithForceScalar(gForceScalar); // apply force defined by apply force slider
+
+ if (m_dynamicsWorld) {
+ m_dynamicsWorld->stepSimulation(deltaTime);
+ }
+}
+
+void NewtonsRopeCradleExample::createRopePendulum(btSphereShape* colShape,
+ const btVector3& position, const btQuaternion& pendulumOrientation, btScalar width, btScalar height, btScalar mass) {
+
+ // The pendulum looks like this (names when built):
+ // O O topSphere1 topSphere2
+ // \ /
+ // O bottomSphere
+
+ //create a dynamic pendulum
+ btTransform startTransform;
+ startTransform.setIdentity();
+
+ // calculate sphere positions
+ btVector3 topSphere1RelPosition(0,0,width);
+ btVector3 topSphere2RelPosition(0,0,-width);
+ btVector3 bottomSphereRelPosition(0,-height,0);
+
+
+ // position the top sphere above ground with appropriate orientation
+ startTransform.setOrigin(btVector3(0,0,0)); // no translation intitially
+ startTransform.setRotation(pendulumOrientation); // pendulum rotation
+ startTransform.setOrigin(startTransform * topSphere1RelPosition); // rotate this position
+ startTransform.setOrigin(position + startTransform.getOrigin()); // add non-rotated position to the relative position
+ btRigidBody* topSphere1 = createRigidBody(0, startTransform, colShape); // make top sphere static
+
+ // position the top sphere above ground with appropriate orientation
+ startTransform.setOrigin(btVector3(0,0,0)); // no translation intitially
+ startTransform.setRotation(pendulumOrientation); // pendulum rotation
+ startTransform.setOrigin(startTransform * topSphere2RelPosition); // rotate this position
+ startTransform.setOrigin(position + startTransform.getOrigin()); // add non-rotated position to the relative position
+ btRigidBody* topSphere2 = createRigidBody(0, startTransform, colShape); // make top sphere static
+
+ // position the bottom sphere below the top sphere
+ startTransform.setOrigin(btVector3(0,0,0)); // no translation intitially
+ startTransform.setRotation(pendulumOrientation); // pendulum rotation
+ startTransform.setOrigin(startTransform * bottomSphereRelPosition); // rotate this position
+ startTransform.setOrigin(position + startTransform.getOrigin()); // add non-rotated position to the relative position
+ btRigidBody* bottomSphere = createRigidBody(mass, startTransform, colShape);
+ bottomSphere->setFriction(0); // we do not need friction here
+ pendula.push_back(bottomSphere);
+
+ // disable the deactivation when objects do not move anymore
+ topSphere1->setActivationState(DISABLE_DEACTIVATION);
+ topSphere2->setActivationState(DISABLE_DEACTIVATION);
+ bottomSphere->setActivationState(DISABLE_DEACTIVATION);
+
+ bottomSphere->setRestitution(gPendulaRestitution); // set pendula restitution
+
+ // add ropes between spheres
+ connectWithRope(topSphere1, bottomSphere);
+ connectWithRope(topSphere2, bottomSphere);
+}
+
+void NewtonsRopeCradleExample::renderScene()
+{
+ CommonRigidBodyBase::renderScene();
+ btSoftRigidDynamicsWorld* softWorld = getSoftDynamicsWorld();
+
+ for ( int i=0;i<softWorld->getSoftBodyArray().size();i++)
+ {
+ btSoftBody* psb=(btSoftBody*)softWorld->getSoftBodyArray()[i];
+ //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
+ {
+ btSoftBodyHelpers::DrawFrame(psb,softWorld->getDebugDrawer());
+ btSoftBodyHelpers::Draw(psb,softWorld->getDebugDrawer(),softWorld->getDrawFlags());
+ }
+ }
+}
+
+void NewtonsRopeCradleExample::changePendulaRestitution(btScalar restitution) {
+ for (std::vector<btRigidBody*>::iterator rit = pendula.begin();
+ rit != pendula.end(); rit++) {
+ btAssert((*rit) && "Null constraint");
+
+ (*rit)->setRestitution(restitution);
+ }
+}
+
+bool NewtonsRopeCradleExample::keyboardCallback(int key, int state) {
+ //b3Printf("Key pressed: %d in state %d \n",key,state);
+
+ // key 3
+ switch (key) {
+ case '3' /*ASCII for 3*/: {
+ applyPendulumForce(gDisplacementForce);
+ return true;
+ }
+ }
+
+ return false;
+}
+
+void NewtonsRopeCradleExample::applyPendulumForce(btScalar pendulumForce){
+ if(pendulumForce != 0){
+ b3Printf("Apply %f to pendulum",pendulumForce);
+ for (int i = 0; i < gDisplacedPendula; i++) {
+ if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty)
+ pendula[i]->applyCentralForce(btVector3(pendulumForce, 0, 0));
+ }
+ }
+}
+
+// GUI parameter modifiers
+
+void onRopePendulaRestitutionChanged(float pendulaRestitution) {
+ if (nex){
+ nex->changePendulaRestitution(pendulaRestitution);
+ }
+}
+
+void floorRSliderValue(float notUsed) {
+ gPendulaQty = floor(gPendulaQty);
+ gDisplacedPendula = floor(gDisplacedPendula);
+ gRopeResolution = floor(gRopeResolution);
+}
+
+void applyRForceWithForceScalar(float forceScalar) {
+ if(nex){
+ btScalar appliedForce = forceScalar * gDisplacementForce;
+
+ if(fabs(gForceScalar) < 0.2f)
+ gForceScalar = 0;
+
+ nex->applyPendulumForce(appliedForce);
+ }
+}
+
+CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(
+ CommonExampleOptions& options) {
+ nex = new NewtonsRopeCradleExample(options.m_guiHelper);
+ return nex;
+}
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/ExtendedTutorials/NewtonsRopeCradle.h
similarity index 75%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/ExtendedTutorials/NewtonsRopeCradle.h
index 13c37a4..3edbcd5 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/ExtendedTutorials/NewtonsRopeCradle.h
@@ -1,28 +1,22 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2015 Google Inc. http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
-
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
-
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
+#define ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
+
+class CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif //ET_NEWTONS_ROPE_CRADLE_EXAMPLE_H
diff --git a/examples/ExtendedTutorials/RigidBodyFromObj.cpp b/examples/ExtendedTutorials/RigidBodyFromObj.cpp
new file mode 100644
index 0000000..9e09f9d
--- /dev/null
+++ b/examples/ExtendedTutorials/RigidBodyFromObj.cpp
@@ -0,0 +1,165 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "RigidBodyFromObj.h"
+
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+
+#include "../Utils/b3ResourcePath.h"
+#include "Bullet3Common/b3FileUtils.h"
+#include "../Importers/ImportObjDemo/LoadMeshFromObj.h"
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+
+
+struct RigidBodyFromObjExample : public CommonRigidBodyBase
+{
+ int m_options;
+
+ RigidBodyFromObjExample(struct GUIHelperInterface* helper, int options)
+ :CommonRigidBodyBase(helper),
+ m_options(options)
+ {
+ }
+ virtual ~RigidBodyFromObjExample(){}
+ virtual void initPhysics();
+ virtual void renderScene();
+ void resetCamera()
+ {
+ float dist = 11;
+ float pitch = 52;
+ float yaw = 35;
+ float targetPos[3]={0,0.46,0};
+ m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+ }
+};
+
+void RigidBodyFromObjExample::initPhysics()
+{
+ m_guiHelper->setUpAxis(1);
+
+ createEmptyDynamicsWorld();
+
+ m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+ //if (m_dynamicsWorld->getDebugDrawer())
+ // m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
+
+ ///create a few basic rigid bodies
+ btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
+ m_collisionShapes.push_back(groundShape);
+
+ btTransform groundTransform;
+ groundTransform.setIdentity();
+ groundTransform.setOrigin(btVector3(0,-50,0));
+ {
+ btScalar mass(0.);
+ createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
+ }
+
+ //load our obj mesh
+ const char* fileName = "teddy.obj";//sphere8.obj";//sponza_closed.obj";//sphere8.obj";
+ char relativeFileName[1024];
+ if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
+ {
+ char pathPrefix[1024];
+ b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
+ }
+
+ GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, "");
+ printf("[INFO] Obj loaded: Extracted %d verticed from obj file [%s]\n", glmesh->m_numvertices, fileName);
+
+ const GLInstanceVertex& v = glmesh->m_vertices->at(0);
+ btConvexHullShape* shape = new btConvexHullShape((const btScalar*)(&(v.xyzw[0])), glmesh->m_numvertices, sizeof(GLInstanceVertex));
+
+ float scaling[4] = {0.1,0.1,0.1,1};
+
+ btVector3 localScaling(scaling[0],scaling[1],scaling[2]);
+ shape->setLocalScaling(localScaling);
+
+ if (m_options & OptimizeConvexObj)
+ {
+ shape->optimizeConvexHull();
+ }
+
+ if (m_options & ComputePolyhedralFeatures)
+ {
+ shape->initializePolyhedralFeatures();
+ }
+
+
+
+ //shape->setMargin(0.001);
+ m_collisionShapes.push_back(shape);
+
+ btTransform startTransform;
+ startTransform.setIdentity();
+
+ btScalar mass(1.f);
+ bool isDynamic = (mass != 0.f);
+ btVector3 localInertia(0,0,0);
+ if (isDynamic)
+ shape->calculateLocalInertia(mass,localInertia);
+
+ float color[4] = {1,1,1,1};
+ float orn[4] = {0,0,0,1};
+ float pos[4] = {0,3,0,0};
+ btVector3 position(pos[0],pos[1],pos[2]);
+ startTransform.setOrigin(position);
+ btRigidBody* body = createRigidBody(mass,startTransform,shape);
+
+
+
+ bool useConvexHullForRendering = ((m_options & ObjUseConvexHullForRendering)!=0);
+
+
+ if (!useConvexHullForRendering)
+ {
+ int shapeId = m_guiHelper->registerGraphicsShape(&glmesh->m_vertices->at(0).xyzw[0],
+ glmesh->m_numvertices,
+ &glmesh->m_indices->at(0),
+ glmesh->m_numIndices,
+ B3_GL_TRIANGLES, -1);
+ shape->setUserIndex(shapeId);
+ int renderInstance = m_guiHelper->registerGraphicsInstance(shapeId,pos,orn,color,scaling);
+ body->setUserIndex(renderInstance);
+ }
+
+ m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+
+void RigidBodyFromObjExample::renderScene()
+{
+ CommonRigidBodyBase::renderScene();
+}
+
+
+
+
+
+
+
+CommonExampleInterface* ET_RigidBodyFromObjCreateFunc(CommonExampleOptions& options)
+{
+ return new RigidBodyFromObjExample(options.m_guiHelper,options.m_option);
+}
+
+B3_STANDALONE_EXAMPLE(ET_RigidBodyFromObjCreateFunc)
+
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/ExtendedTutorials/RigidBodyFromObj.h
similarity index 72%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/ExtendedTutorials/RigidBodyFromObj.h
index 13c37a4..829e9e1 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/ExtendedTutorials/RigidBodyFromObj.h
@@ -13,16 +13,16 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
+#ifndef ET_RIGIDBODYFROMOBJ_EXAMPLE_H
+#define ET_RIGIDBODYFROMOBJ_EXAMPLE_H
-enum btInverseDynamicsExampleOptions
+enum ObjToRigidBodyOptionsEnum
{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
+ ObjUseConvexHullForRendering=1,
+ OptimizeConvexObj=2,
+ ComputePolyhedralFeatures=4,
};
+class CommonExampleInterface* ET_RigidBodyFromObjCreateFunc(struct CommonExampleOptions& options);
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+#endif //ET_RIGIDBODYFROMOBJ_EXAMPLE_H
diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/ExtendedTutorials/SimpleBox.cpp
similarity index 69%
copy from examples/BasicDemo/BasicExample.cpp
copy to examples/ExtendedTutorials/SimpleBox.cpp
index d501b51..474a507 100644
--- a/examples/BasicDemo/BasicExample.cpp
+++ b/examples/ExtendedTutorials/SimpleBox.cpp
@@ -15,26 +15,21 @@ subject to the following restrictions:
-#include "BasicExample.h"
+#include "SimpleBox.h"
#include "btBulletDynamicsCommon.h"
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Z 5
-
#include "LinearMath/btVector3.h"
-#include "LinearMath/btAlignedObjectArray.h"
-
+#include "LinearMath/btAlignedObjectArray.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
-struct BasicExample : public CommonRigidBodyBase
+struct SimpleBoxExample : public CommonRigidBodyBase
{
- BasicExample(struct GUIHelperInterface* helper)
+ SimpleBoxExample(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
{
}
- virtual ~BasicExample(){}
+ virtual ~SimpleBoxExample(){}
virtual void initPhysics();
virtual void renderScene();
void resetCamera()
@@ -47,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase
}
};
-void BasicExample::initPhysics()
+void SimpleBoxExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
@@ -60,17 +55,11 @@ void BasicExample::initPhysics()
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
-
-
- //groundShape->initializePolyhedralFeatures();
-// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
-
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
- groundTransform.setOrigin(btVector3(0,-50,0));
-
+ groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
@@ -80,11 +69,8 @@ void BasicExample::initPhysics()
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
-
- btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
-
-
- //btCollisionShape* colShape = new btSphereShape(btScalar(1.));
+ btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
+
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
@@ -101,34 +87,20 @@ void BasicExample::initPhysics()
colShape->calculateLocalInertia(mass,localInertia);
- for (int k=0;k<ARRAY_SIZE_Y;k++)
- {
- for (int i=0;i<ARRAY_SIZE_X;i++)
- {
- for(int j = 0;j<ARRAY_SIZE_Z;j++)
- {
- startTransform.setOrigin(btVector3(
- btScalar(2.0*i),
- btScalar(20+2.0*k),
- btScalar(2.0*j)));
-
-
- createRigidBody(mass,startTransform,colShape);
-
-
- }
- }
- }
+ startTransform.setOrigin(btVector3(
+ btScalar(0),
+ btScalar(20),
+ btScalar(0)));
+ createRigidBody(mass,startTransform,colShape);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
-void BasicExample::renderScene()
+void SimpleBoxExample::renderScene()
{
- CommonRigidBodyBase::renderScene();
-
+ CommonRigidBodyBase::renderScene();
}
@@ -137,9 +109,9 @@ void BasicExample::renderScene()
-CommonExampleInterface* BasicExampleCreateFunc(CommonExampleOptions& options)
+CommonExampleInterface* ET_SimpleBoxCreateFunc(CommonExampleOptions& options)
{
- return new BasicExample(options.m_guiHelper);
+ return new SimpleBoxExample(options.m_guiHelper);
}
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/ExtendedTutorials/SimpleBox.h
similarity index 75%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/ExtendedTutorials/SimpleBox.h
index 13c37a4..494b3fd 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/ExtendedTutorials/SimpleBox.h
@@ -13,16 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
+#ifndef ET_SIMPLE_BOX_EXAMPLE_H
+#define ET_SIMPLE_BOX_EXAMPLE_H
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
+class CommonExampleInterface* ET_SimpleBoxCreateFunc(struct CommonExampleOptions& options);
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+#endif //ET_SIMPLE_BOX_EXAMPLE_H
diff --git a/examples/ExtendedTutorials/SimpleCloth.cpp b/examples/ExtendedTutorials/SimpleCloth.cpp
new file mode 100644
index 0000000..33a3815
--- /dev/null
+++ b/examples/ExtendedTutorials/SimpleCloth.cpp
@@ -0,0 +1,162 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "SimpleCloth.h"
+
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+
+#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+
+struct SimpleClothExample : public CommonRigidBodyBase
+{
+ SimpleClothExample(struct GUIHelperInterface* helper)
+ :CommonRigidBodyBase(helper)
+ {
+ }
+ virtual ~SimpleClothExample(){}
+ virtual void initPhysics();
+ virtual void renderScene();
+ void createEmptyDynamicsWorld()
+ {
+ m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+ m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+ m_broadphase = new btDbvtBroadphase();
+
+ m_solver = new btSequentialImpulseConstraintSolver;
+
+ m_dynamicsWorld = new btSoftRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
+ m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
+
+ softBodyWorldInfo.m_broadphase = m_broadphase;
+ softBodyWorldInfo.m_dispatcher = m_dispatcher;
+ softBodyWorldInfo.m_gravity = m_dynamicsWorld->getGravity();
+ softBodyWorldInfo.m_sparsesdf.Initialize();
+ }
+ virtual btSoftRigidDynamicsWorld* getSoftDynamicsWorld()
+ {
+ ///just make it a btSoftRigidDynamicsWorld please
+ ///or we will add type checking
+ return (btSoftRigidDynamicsWorld*) m_dynamicsWorld;
+ }
+ void resetCamera()
+ {
+ float dist = 41;
+ float pitch = 52;
+ float yaw = 35;
+ float targetPos[3]={0,0.46,0};
+ m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+ }
+
+ void createSoftBody(const btScalar size, const int num_x, const int num_z, const int fixed=1+2);
+ btSoftBodyWorldInfo softBodyWorldInfo;
+};
+
+void SimpleClothExample::initPhysics()
+{
+ m_guiHelper->setUpAxis(1);
+
+ createEmptyDynamicsWorld();
+
+ m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+ if (m_dynamicsWorld->getDebugDrawer())
+ m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
+
+ ///create a few basic rigid bodies
+ btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
+ m_collisionShapes.push_back(groundShape);
+
+ btTransform groundTransform;
+ groundTransform.setIdentity();
+ groundTransform.setOrigin(btVector3(0,-50,0));
+ {
+ btScalar mass(0.);
+ createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
+ }
+
+
+ {
+ const btScalar s=4; //size of cloth patch
+ const int NUM_X=31; //vertices on X axis
+ const int NUM_Z=31; //vertices on Z axis
+ createSoftBody(s,NUM_X, NUM_Z);
+ }
+
+ m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void SimpleClothExample::createSoftBody(const btScalar s,
+ const int numX,
+ const int numY,
+ const int fixed) {
+
+
+
+ btSoftBody* cloth=btSoftBodyHelpers::CreatePatch(softBodyWorldInfo,
+ btVector3(-s/2,s+1,0),
+ btVector3(+s/2,s+1,0),
+ btVector3(-s/2,s+1,+s),
+ btVector3(+s/2,s+1,+s),
+ numX,numY,
+ fixed,true);
+
+ cloth->getCollisionShape()->setMargin(0.001f);
+ cloth->generateBendingConstraints(2,cloth->appendMaterial());
+ cloth->setTotalMass(10);
+ //cloth->m_cfg.citerations = 10;
+// cloth->m_cfg.diterations = 10;
+ cloth->m_cfg.piterations = 5;
+ cloth->m_cfg.kDP = 0.005f;
+ getSoftDynamicsWorld()->addSoftBody(cloth);
+
+}
+
+void SimpleClothExample::renderScene()
+{
+ CommonRigidBodyBase::renderScene();
+ btSoftRigidDynamicsWorld* softWorld = getSoftDynamicsWorld();
+
+ for ( int i=0;i<softWorld->getSoftBodyArray().size();i++)
+ {
+ btSoftBody* psb=(btSoftBody*)softWorld->getSoftBodyArray()[i];
+ //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
+ {
+ btSoftBodyHelpers::DrawFrame(psb,softWorld->getDebugDrawer());
+ btSoftBodyHelpers::Draw(psb,softWorld->getDebugDrawer(),softWorld->getDrawFlags());
+ }
+ }
+}
+
+
+
+
+
+
+
+CommonExampleInterface* ET_SimpleClothCreateFunc(CommonExampleOptions& options)
+{
+ return new SimpleClothExample(options.m_guiHelper);
+}
+
+
+
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/ExtendedTutorials/SimpleCloth.h
similarity index 75%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/ExtendedTutorials/SimpleCloth.h
index 13c37a4..d956833 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/ExtendedTutorials/SimpleCloth.h
@@ -13,16 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
+#ifndef ET_SIMPLE_CLOTH_EXAMPLE_H
+#define ET_SIMPLE_CLOTH_EXAMPLE_H
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
+class CommonExampleInterface* ET_SimpleClothCreateFunc(struct CommonExampleOptions& options);
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+#endif //ET_SIMPLE_CLOTH_EXAMPLE_H
diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/ExtendedTutorials/SimpleJoint.cpp
similarity index 66%
copy from examples/BasicDemo/BasicExample.cpp
copy to examples/ExtendedTutorials/SimpleJoint.cpp
index d501b51..da30449 100644
--- a/examples/BasicDemo/BasicExample.cpp
+++ b/examples/ExtendedTutorials/SimpleJoint.cpp
@@ -15,26 +15,21 @@ subject to the following restrictions:
-#include "BasicExample.h"
+#include "SimpleJoint.h"
#include "btBulletDynamicsCommon.h"
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Z 5
-
#include "LinearMath/btVector3.h"
-#include "LinearMath/btAlignedObjectArray.h"
-
+#include "LinearMath/btAlignedObjectArray.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
-struct BasicExample : public CommonRigidBodyBase
+struct SimpleJointExample : public CommonRigidBodyBase
{
- BasicExample(struct GUIHelperInterface* helper)
+ SimpleJointExample(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
{
}
- virtual ~BasicExample(){}
+ virtual ~SimpleJointExample(){}
virtual void initPhysics();
virtual void renderScene();
void resetCamera()
@@ -47,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase
}
};
-void BasicExample::initPhysics()
+void SimpleJointExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
@@ -60,17 +55,11 @@ void BasicExample::initPhysics()
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
-
-
- //groundShape->initializePolyhedralFeatures();
-// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
-
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
- groundTransform.setOrigin(btVector3(0,-50,0));
-
+ groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
@@ -80,11 +69,8 @@ void BasicExample::initPhysics()
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
-
- btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
-
-
- //btCollisionShape* colShape = new btSphereShape(btScalar(1.));
+ btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
+
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
@@ -101,34 +87,35 @@ void BasicExample::initPhysics()
colShape->calculateLocalInertia(mass,localInertia);
- for (int k=0;k<ARRAY_SIZE_Y;k++)
- {
- for (int i=0;i<ARRAY_SIZE_X;i++)
- {
- for(int j = 0;j<ARRAY_SIZE_Z;j++)
- {
- startTransform.setOrigin(btVector3(
- btScalar(2.0*i),
- btScalar(20+2.0*k),
- btScalar(2.0*j)));
-
-
- createRigidBody(mass,startTransform,colShape);
-
-
- }
- }
- }
+ startTransform.setOrigin(btVector3(
+ btScalar(0),
+ btScalar(10),
+ btScalar(0)));
+ btRigidBody* dynamicBox = createRigidBody(mass,startTransform,colShape);
+
+ //create a static rigid body
+ mass = 0;
+ startTransform.setOrigin(btVector3(
+ btScalar(0),
+ btScalar(20),
+ btScalar(0)));
+
+ btRigidBody* staticBox = createRigidBody(mass,startTransform,colShape);
+
+ //create a simple p2pjoint constraint
+ btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*dynamicBox, *staticBox, btVector3(0,3,0), btVector3(0,0,0));
+ p2p->m_setting.m_damping = 0.0625;
+ p2p->m_setting.m_impulseClamp = 0.95;
+ m_dynamicsWorld->addConstraint(p2p);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
-void BasicExample::renderScene()
+void SimpleJointExample::renderScene()
{
- CommonRigidBodyBase::renderScene();
-
+ CommonRigidBodyBase::renderScene();
}
@@ -137,9 +124,9 @@ void BasicExample::renderScene()
-CommonExampleInterface* BasicExampleCreateFunc(CommonExampleOptions& options)
+CommonExampleInterface* ET_SimpleJointCreateFunc(CommonExampleOptions& options)
{
- return new BasicExample(options.m_guiHelper);
+ return new SimpleJointExample(options.m_guiHelper);
}
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/ExtendedTutorials/SimpleJoint.h
similarity index 75%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/ExtendedTutorials/SimpleJoint.h
index 13c37a4..a3d245a 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/ExtendedTutorials/SimpleJoint.h
@@ -13,16 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
+#ifndef ET_SIMPLE_JOINT_EXAMPLE_H
+#define ET_SIMPLE_JOINT_EXAMPLE_H
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
+class CommonExampleInterface* ET_SimpleJointCreateFunc(struct CommonExampleOptions& options);
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+#endif //ET_SIMPLE_JOINT_EXAMPLE_H
diff --git a/examples/ExtendedTutorials/premake4.lua b/examples/ExtendedTutorials/premake4.lua
new file mode 100644
index 0000000..41126e0
--- /dev/null
+++ b/examples/ExtendedTutorials/premake4.lua
@@ -0,0 +1,224 @@
+
+project "App_RigidBodyFromObjExample"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+includedirs {"../../src"}
+
+links {
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","BulletDynamics","BulletCollision", "LinearMath"
+}
+
+language "C++"
+
+files {
+ "RigidBodyFromObj.cpp",
+ "**.h",
+ "../StandaloneMain/main_console_single_example.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.h",
+ "../RenderingExamples/TimeSeriesCanvas.cpp",
+ "../RenderingExamples/TimeSeriesFontData.cpp",
+ "../MultiBody/InvertedPendulumPDControl.cpp",
+ "../ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../ThirdPartyLibs/stb_image/stb_image.cpp",
+}
+
+
+project "App_RigidBodyFromObjExampleGui"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
+}
+
+ initOpenGL()
+ initGlew()
+
+
+language "C++"
+
+files {
+ "RigidBodyFromObj.cpp",
+ "*.h",
+ "../StandaloneMain/main_opengl_single_example.cpp",
+ "../ExampleBrowser/OpenGLGuiHelper.cpp",
+ "../ExampleBrowser/GL_ShapeDrawer.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.h",
+ "../RenderingExamples/TimeSeriesCanvas.cpp",
+ "../RenderingExamples/TimeSeriesFontData.cpp",
+ "../MultiBody/InvertedPendulumPDControl.cpp",
+ "../ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../ThirdPartyLibs/stb_image/stb_image.cpp",
+ "../Utils/b3Clock.cpp",
+ "../Utils/b3Clock.h",
+}
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+end
+
+
+
+project "App_RigidBodyFromObjExampleGuiWithSoftwareRenderer"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
+}
+
+ initOpenGL()
+ initGlew()
+
+
+language "C++"
+
+files {
+ "RigidBodyFromObj.cpp",
+ "*.h",
+ "../StandaloneMain/main_sw_tinyrenderer_single_example.cpp",
+ "../ExampleBrowser/OpenGLGuiHelper.cpp",
+ "../ExampleBrowser/GL_ShapeDrawer.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../TinyRenderer/geometry.cpp",
+ "../TinyRenderer/model.cpp",
+ "../TinyRenderer/tgaimage.cpp",
+ "../TinyRenderer/our_gl.cpp",
+ "../TinyRenderer/TinyRenderer.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.h",
+ "../RenderingExamples/TimeSeriesCanvas.cpp",
+ "../RenderingExamples/TimeSeriesFontData.cpp",
+ "../MultiBody/InvertedPendulumPDControl.cpp",
+ "../ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../ThirdPartyLibs/stb_image/stb_image.cpp",
+}
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+end
+
+
+
+project "App_RigidBodyFromObjExampleTinyRenderer"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "Bullet3Common"
+}
+
+
+language "C++"
+
+files {
+ "RigidBodyFromObj.cpp",
+ "*.h",
+ "../StandaloneMain/main_tinyrenderer_single_example.cpp",
+ "../OpenGLWindow/SimpleCamera.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../TinyRenderer/geometry.cpp",
+ "../TinyRenderer/model.cpp",
+ "../TinyRenderer/tgaimage.cpp",
+ "../TinyRenderer/our_gl.cpp",
+ "../TinyRenderer/TinyRenderer.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.h",
+ "../RenderingExamples/TimeSeriesCanvas.cpp",
+ "../RenderingExamples/TimeSeriesFontData.cpp",
+ "../MultiBody/InvertedPendulumPDControl.cpp",
+ "../ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../ThirdPartyLibs/stb_image/stb_image.cpp",
+}
+
diff --git a/examples/ForkLift/ForkLiftDemo.cpp b/examples/ForkLift/ForkLiftDemo.cpp
index 9714384..f315daa 100644
--- a/examples/ForkLift/ForkLiftDemo.cpp
+++ b/examples/ForkLift/ForkLiftDemo.cpp
@@ -365,6 +365,7 @@ void ForkLiftDemo::initPhysics()
{
m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 128;//for direct solver, it is better to solve multiple objects together, small batches have high overhead
}
+ m_dynamicsWorld->getSolverInfo().m_globalCfm = 0.00001;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
diff --git a/examples/HelloWorld/HelloWorld.cpp b/examples/HelloWorld/HelloWorld.cpp
index f7a1957..e51e6d5 100644
--- a/examples/HelloWorld/HelloWorld.cpp
+++ b/examples/HelloWorld/HelloWorld.cpp
@@ -44,20 +44,24 @@ int main(int argc, char** argv)
///-----initialization_end-----
- ///create a few basic rigid bodies
- btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
-
//keep track of the shapes, we release memory at exit.
//make sure to re-use collision shapes among rigid bodies whenever possible!
btAlignedObjectArray<btCollisionShape*> collisionShapes;
- collisionShapes.push_back(groundShape);
- btTransform groundTransform;
- groundTransform.setIdentity();
- groundTransform.setOrigin(btVector3(0,-56,0));
+ ///create a few basic rigid bodies
+ //the ground is a cube of side 100 at position y = -56.
+ //the sphere will hit it at y = -6, with center at -5
{
+ btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
+
+ collisionShapes.push_back(groundShape);
+
+ btTransform groundTransform;
+ groundTransform.setIdentity();
+ groundTransform.setOrigin(btVector3(0,-56,0));
+
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
@@ -113,7 +117,7 @@ int main(int argc, char** argv)
///-----stepsimulation_start-----
- for (i=0;i<100;i++)
+ for (i=0;i<150;i++)
{
dynamicsWorld->stepSimulation(1.f/60.f,10);
@@ -178,9 +182,5 @@ int main(int argc, char** argv)
//next line is optional: it will be cleared by the destructor when the array goes out of scope
collisionShapes.clear();
-
- ///-----cleanup_end-----
- printf("Press a key to exit\n");
- getchar();
}
diff --git a/examples/Importers/ImportBsp/ImportBspExample.cpp b/examples/Importers/ImportBsp/ImportBspExample.cpp
index 0383423..eec8a4f 100644
--- a/examples/Importers/ImportBsp/ImportBspExample.cpp
+++ b/examples/Importers/ImportBsp/ImportBspExample.cpp
@@ -285,7 +285,7 @@ char* makeExeToBspFilename(const char* lpCmdLine)
}
-struct CommonExampleInterface* ImportBspCreateFunc(struct CommonExampleOptions& options)
+CommonExampleInterface* ImportBspCreateFunc(struct CommonExampleOptions& options)
{
BspDemo* demo = new BspDemo(options.m_guiHelper);
diff --git a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
index 317fe9c..8b47f55 100644
--- a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
+++ b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
@@ -20,7 +20,7 @@ subject to the following restrictions:
#include <stdio.h> //fopen
#include "Bullet3Common/b3AlignedObjectArray.h"
#include <string>
-#include "tinyxml/tinyxml.h"
+#include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
#include "Bullet3Common/b3FileUtils.h"
#include "LinearMath/btHashMap.h"
@@ -28,6 +28,7 @@ subject to the following restrictions:
#include "btMatrix4x4.h"
+#define MAX_VISUAL_SHAPES 512
struct VertexSource
@@ -288,42 +289,47 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
}//for each mesh
int shapeIndex = visualShapes.size();
- GLInstanceGraphicsShape& visualShape = visualShapes.expand();
- {
- visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
- visualShape.m_indices = new b3AlignedObjectArray<int>;
- int indexBase = 0;
-
- btAssert(vertexNormals.size()==vertexPositions.size());
- for (int v=0;v<vertexPositions.size();v++)
- {
- GLInstanceVertex vtx;
- vtx.xyzw[0] = vertexPositions[v].x();
- vtx.xyzw[1] = vertexPositions[v].y();
- vtx.xyzw[2] = vertexPositions[v].z();
- vtx.xyzw[3] = 1.f;
- vtx.normal[0] = vertexNormals[v].x();
- vtx.normal[1] = vertexNormals[v].y();
- vtx.normal[2] = vertexNormals[v].z();
- vtx.uv[0] = 0.5f;
- vtx.uv[1] = 0.5f;
- visualShape.m_vertices->push_back(vtx);
- }
-
- for (int index=0;index<indices.size();index++)
- {
- visualShape.m_indices->push_back(indices[index]+indexBase);
- }
-
-
- printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size());
- indexBase=visualShape.m_vertices->size();
- visualShape.m_numIndices = visualShape.m_indices->size();
- visualShape.m_numvertices = visualShape.m_vertices->size();
- }
- printf("geometry name=%s\n",geometryName);
- name2Shape.insert(geometryName,shapeIndex);
-
+ if (shapeIndex<MAX_VISUAL_SHAPES)
+ {
+ GLInstanceGraphicsShape& visualShape = visualShapes.expand();
+ {
+ visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
+ visualShape.m_indices = new b3AlignedObjectArray<int>;
+ int indexBase = 0;
+
+ btAssert(vertexNormals.size()==vertexPositions.size());
+ for (int v=0;v<vertexPositions.size();v++)
+ {
+ GLInstanceVertex vtx;
+ vtx.xyzw[0] = vertexPositions[v].x();
+ vtx.xyzw[1] = vertexPositions[v].y();
+ vtx.xyzw[2] = vertexPositions[v].z();
+ vtx.xyzw[3] = 1.f;
+ vtx.normal[0] = vertexNormals[v].x();
+ vtx.normal[1] = vertexNormals[v].y();
+ vtx.normal[2] = vertexNormals[v].z();
+ vtx.uv[0] = 0.5f;
+ vtx.uv[1] = 0.5f;
+ visualShape.m_vertices->push_back(vtx);
+ }
+
+ for (int index=0;index<indices.size();index++)
+ {
+ visualShape.m_indices->push_back(indices[index]+indexBase);
+ }
+
+
+ //b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size());
+ indexBase=visualShape.m_vertices->size();
+ visualShape.m_numIndices = visualShape.m_indices->size();
+ visualShape.m_numvertices = visualShape.m_vertices->size();
+ }
+ //b3Printf("geometry name=%s\n",geometryName);
+ name2Shape.insert(geometryName,shapeIndex);
+ } else
+ {
+ b3Warning("DAE exceeds number of visual shapes (%d/%d)",shapeIndex, MAX_VISUAL_SHAPES);
+ }
}//for each geometry
}
@@ -331,7 +337,7 @@ void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGr
void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat)
{
const char* nodeName = node->Attribute("id");
- printf("processing node %s\n", nodeName);
+ //printf("processing node %s\n", nodeName);
btMatrix4x4 nodeTrans;
@@ -356,7 +362,7 @@ void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shap
nodeTrans = nodeTrans*t;
} else
{
- printf("Error: expected 16 elements in a <matrix> element, skipping\n");
+ b3Warning("Error: expected 16 elements in a <matrix> element, skipping\n");
}
}
}
@@ -412,19 +418,19 @@ void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shap
instanceGeom=instanceGeom->NextSiblingElement("instance_geometry"))
{
const char* geomUrl = instanceGeom->Attribute("url");
- printf("node referring to geom %s\n", geomUrl);
+ //printf("node referring to geom %s\n", geomUrl);
geomUrl++;
int* shapeIndexPtr = name2Shape[geomUrl];
if (shapeIndexPtr)
{
// int index = *shapeIndexPtr;
- printf("found geom with index %d\n", *shapeIndexPtr);
+ //printf("found geom with index %d\n", *shapeIndexPtr);
ColladaGraphicsInstance& instance = visualShapeInstances.expand();
instance.m_shapeIndex = *shapeIndexPtr;
instance.m_worldTransform = nodeTrans;
} else
{
- printf("geom not found\n");
+ b3Warning("geom not found\n");
}
}
@@ -492,7 +498,7 @@ void getUnitMeterScalingAndUpAxisTransform(TiXmlDocument& doc, btTransform& tr,
if (unitMeter)
{
const char* meterText = unitMeter->Attribute("meter");
- printf("meterText=%s\n", meterText);
+ //printf("meterText=%s\n", meterText);
unitMeterScaling = atof(meterText);
}
@@ -557,7 +563,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
// GLInstanceGraphicsShape* instance = 0;
//usually COLLADA files don't have that many visual geometries/shapes
- visualShapes.reserve(32);
+ visualShapes.reserve(MAX_VISUAL_SHAPES);
float extraScaling = 1;//0.01;
btHashMap<btHashString, int> name2ShapeIndex;
@@ -565,7 +571,7 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
char filename[1024];
if (!f.findFile(relativeFileName,filename,1024))
{
- printf("File not found: %s\n", filename);
+ b3Warning("File not found: %s\n", filename);
return;
}
@@ -703,12 +709,12 @@ void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArra
int size=0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
{
- printf("Error: Cannot access file to determine size of %s\n", relativeFileName);
+ b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName);
} else
{
if (size)
{
- printf("Open DAE file of %d bytes\n",size);
+ //printf("Open DAE file of %d bytes\n",size);
Assimp::Importer importer;
//importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_COLORS);
diff --git a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h
index c65666f..2e939b5 100644
--- a/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h
+++ b/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h
@@ -21,7 +21,7 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
-#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
#include "ColladaGraphicsInstance.h"
diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
new file mode 100644
index 0000000..c042c42
--- /dev/null
+++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
@@ -0,0 +1,88 @@
+#include "b3ImportMeshUtility.h"
+
+#include <vector>
+#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
+#include "LinearMath/btVector3.h"
+#include "../ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h"
+#include "../../Utils/b3ResourcePath.h"
+#include "Bullet3Common/b3FileUtils.h"
+#include "../../ThirdPartyLibs/stb_image/stb_image.h"
+
+
+bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData)
+{
+
+ meshData.m_gfxShape = 0;
+ meshData.m_textureImage = 0;
+ meshData.m_textureHeight = 0;
+ meshData.m_textureWidth = 0;
+
+
+ char relativeFileName[1024];
+ if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
+ {
+ char pathPrefix[1024];
+
+ b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
+ btVector3 shift(0,0,0);
+
+ std::vector<tinyobj::shape_t> shapes;
+ std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix);
+
+ GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
+
+ int textureIndex = -1;
+ //try to load some texture
+ for (int i=0;i<shapes.size();i++)
+ {
+ const tinyobj::shape_t& shape = shapes[i];
+ if (shape.material.diffuse_texname.length()>0)
+ {
+
+ int width,height,n;
+ const char* filename = shape.material.diffuse_texname.c_str();
+ unsigned char* image=0;
+
+ const char* prefix[]={ pathPrefix,"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
+ int numprefix = sizeof(prefix)/sizeof(const char*);
+
+ for (int i=0;!image && i<numprefix;i++)
+ {
+ char relativeFileName[1024];
+ sprintf(relativeFileName,"%s%s",prefix[i],filename);
+ char relativeFileName2[1024];
+ if (b3ResourcePath::findResourcePath(relativeFileName, relativeFileName2, 1024))
+ {
+ image = stbi_load(relativeFileName, &width, &height, &n, 3);
+ meshData.m_textureImage = image;
+ if (image)
+ {
+ meshData.m_textureWidth = width;
+ meshData.m_textureHeight = height;
+ } else
+ {
+ meshData.m_textureWidth = 0;
+ meshData.m_textureHeight = 0;
+ }
+
+ } else
+ {
+ b3Warning("not found [%s]\n",relativeFileName);
+ }
+ }
+ }
+
+ }
+ meshData.m_gfxShape = gfxShape;
+ return true;
+
+ }
+ else
+ {
+ b3Warning("Cannot find %s\n", fileName.c_str());
+ }
+
+ return false;
+}
+
+
diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h
new file mode 100644
index 0000000..7bf6af8
--- /dev/null
+++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h
@@ -0,0 +1,25 @@
+#ifndef B3_IMPORT_MESH_UTILITY_H
+#define B3_IMPORT_MESH_UTILITY_H
+
+#include <string>
+
+struct b3ImportMeshData
+{
+ struct GLInstanceGraphicsShape* m_gfxShape;
+
+ unsigned char* m_textureImage;//in 3 component 8-bit RGB data
+ int m_textureWidth;
+ int m_textureHeight;
+};
+
+class b3ImportMeshUtility
+{
+public:
+
+static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData);
+
+};
+
+
+#endif //B3_IMPORT_MESH_UTILITY_H
+
diff --git a/examples/Importers/ImportObjDemo/ImportObjExample.cpp b/examples/Importers/ImportObjDemo/ImportObjExample.cpp
index db6ef97..2fc11af 100644
--- a/examples/Importers/ImportObjDemo/ImportObjExample.cpp
+++ b/examples/Importers/ImportObjDemo/ImportObjExample.cpp
@@ -12,13 +12,16 @@
#include "stb_image/stb_image.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
-
+#include "../ImportMeshUtility/b3ImportMeshUtility.h"
class ImportObjSetup : public CommonRigidBodyBase
{
+ std::string m_fileName;
+
+
public:
- ImportObjSetup(struct GUIHelperInterface* helper);
+ ImportObjSetup(struct GUIHelperInterface* helper, const char* fileName);
virtual ~ImportObjSetup();
virtual void initPhysics();
@@ -34,10 +37,16 @@ public:
};
-ImportObjSetup::ImportObjSetup(struct GUIHelperInterface* helper)
+ImportObjSetup::ImportObjSetup(struct GUIHelperInterface* helper, const char* fileName)
:CommonRigidBodyBase(helper)
{
-
+ if (fileName)
+ {
+ m_fileName = fileName;
+ } else
+ {
+ m_fileName = "cube.obj";//"sponza_closed.obj";//sphere8.obj";
+ }
}
ImportObjSetup::~ImportObjSetup()
@@ -47,7 +56,31 @@ ImportObjSetup::~ImportObjSetup()
-
+int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterface* renderer)
+{
+ int shapeId = -1;
+
+ b3ImportMeshData meshData;
+ if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
+ {
+ int textureIndex = -1;
+
+ if (meshData.m_textureImage)
+ {
+ textureIndex = renderer->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight);
+ }
+
+ shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
+ meshData.m_gfxShape->m_numvertices,
+ &meshData.m_gfxShape->m_indices->at(0),
+ meshData.m_gfxShape->m_numIndices,
+ B3_GL_TRIANGLES,
+ textureIndex);
+ delete meshData.m_gfxShape;
+ delete meshData.m_textureImage;
+ }
+ return shapeId;
+}
@@ -59,91 +92,25 @@ void ImportObjSetup::initPhysics()
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
- const char* fileName = "cube.obj";//sphere8.obj";//sponza_closed.obj";//sphere8.obj";
- char relativeFileName[1024];
- if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
- {
- char pathPrefix[1024];
-
- b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
-
-
-
- btVector3 shift(0,0,0);
- btVector3 scaling(1,1,1);
-// int index=10;
-
- {
-
- std::vector<tinyobj::shape_t> shapes;
- std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix);
+ btTransform trans;
+ trans.setIdentity();
+ trans.setRotation(btQuaternion(btVector3(1,0,0),SIMD_HALF_PI));
+ btVector3 position = trans.getOrigin();
+ btQuaternion orn = trans.getRotation();
- GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
+ btVector3 scaling(1,1,1);
+ btVector3 color(1,1,1);
- btTransform trans;
- trans.setIdentity();
- trans.setRotation(btQuaternion(btVector3(1,0,0),SIMD_HALF_PI));
-
- btVector3 position = trans.getOrigin();
- btQuaternion orn = trans.getRotation();
-
- btVector3 color(1,1,1);
-
- int textureIndex = -1;
- //try to load some texture
- for (int i=0;i<shapes.size();i++)
- {
- const tinyobj::shape_t& shape = shapes[i];
- if (shape.material.diffuse_texname.length()>0)
- {
-
- int width,height,n;
- const char* filename = shape.material.diffuse_texname.c_str();
- const unsigned char* image=0;
-
- const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
- int numprefix = sizeof(prefix)/sizeof(const char*);
-
- for (int i=0;!image && i<numprefix;i++)
- {
- char relativeFileName[1024];
- sprintf(relativeFileName,"%s%s",prefix[i],filename);
- image = stbi_load(relativeFileName, &width, &height, &n, 0);
- }
-
- if (image)
- {
- textureIndex = m_guiHelper->getRenderInterface()->registerTexture(image,width,height);
- if (textureIndex>=0)
- {
- break;
- }
- }
-
- }
-
- }
-
- if (1)
- {
-
- }
-
- int shapeId = m_guiHelper->getRenderInterface()->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices,B3_GL_TRIANGLES,textureIndex);
-
- //int id =
- m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
-
-
- }}
- else
- {
- b3Warning("Cannot find %s\n", fileName);
- }
+ int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface());
+ if (shapeId>=0)
+ {
+ //int id =
+ m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+ }
}
CommonExampleInterface* ImportObjCreateFunc(struct CommonExampleOptions& options)
{
- return new ImportObjSetup(options.m_guiHelper);
+ return new ImportObjSetup(options.m_guiHelper, options.m_fileName);
}
diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
index 4988fb8..d90bedf 100644
--- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
+++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
@@ -1,6 +1,6 @@
#include "LoadMeshFromObj.h"
-#include"Wavefront/tiny_obj_loader.h"
-#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
#include <stdio.h> //fopen
#include "Bullet3Common/b3AlignedObjectArray.h"
#include <string>
diff --git a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
index c1b417e..4504fb7 100644
--- a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
+++ b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
@@ -1,14 +1,14 @@
#include "Wavefront2GLInstanceGraphicsShape.h"
-#include "../OpenGLWindow/GLInstancingRenderer.h"
-#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "../../OpenGLWindow/GLInstancingRenderer.h"
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
#include "btBulletDynamicsCommon.h"
-#include "../OpenGLWindow/SimpleOpenGL3App.h"
+#include "../../OpenGLWindow/SimpleOpenGL3App.h"
#include "Wavefront2GLInstanceGraphicsShape.h"
-#include "../OpenGLWindow/GLInstancingRenderer.h"
-#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "../../OpenGLWindow/GLInstancingRenderer.h"
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
-GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes)
+GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes, bool flatShading)
{
b3AlignedObjectArray<GLInstanceVertex>* vertices = new b3AlignedObjectArray<GLInstanceVertex>;
@@ -33,6 +33,10 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
int vtxBaseIndex = vertices->size();
+ if (f<0 && f>=shape.mesh.indices.size())
+ {
+ continue;
+ }
GLInstanceVertex vtx0;
vtx0.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f]*3+0];
@@ -40,10 +44,22 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
vtx0.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f]*3+2];
vtx0.xyzw[3] = 0.f;
- if (shape.mesh.texcoords.size())
+
+ if (shape.mesh.texcoords.size() )
{
- vtx0.uv[0] = shape.mesh.texcoords[shape.mesh.indices[f]*2+0];
- vtx0.uv[1] = shape.mesh.texcoords[shape.mesh.indices[f]*2+1];
+ int uv0Index = shape.mesh.indices[f]*2+0;
+ int uv1Index = shape.mesh.indices[f]*2+1;
+ if (uv0Index>=0 && uv1Index>=0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
+ {
+ vtx0.uv[0] = shape.mesh.texcoords[uv0Index];
+ vtx0.uv[1] = shape.mesh.texcoords[uv1Index];
+ } else
+ {
+ b3Warning("obj texture coordinate out-of-range!");
+ vtx0.uv[0] = 0;
+ vtx0.uv[1] = 0;
+ }
+
} else
{
vtx0.uv[0] = 0.5;
@@ -58,8 +74,18 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
if (shape.mesh.texcoords.size())
{
- vtx1.uv[0] = shape.mesh.texcoords[shape.mesh.indices[f+1]*2+0];
- vtx1.uv[1] = shape.mesh.texcoords[shape.mesh.indices[f+1]*2+1];
+ int uv0Index = shape.mesh.indices[f+1]*2+0;
+ int uv1Index = shape.mesh.indices[f+1]*2+1;
+ if (uv0Index>=0 && uv1Index>=0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
+ {
+ vtx1.uv[0] = shape.mesh.texcoords[uv0Index];
+ vtx1.uv[1] = shape.mesh.texcoords[uv1Index];
+ } else
+ {
+ b3Warning("obj texture coordinate out-of-range!");
+ vtx1.uv[0] = 0;
+ vtx1.uv[1] = 0;
+ }
} else
{
vtx1.uv[0] = 0.5f;
@@ -73,8 +99,18 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
vtx2.xyzw[3] = 0.f;
if (shape.mesh.texcoords.size())
{
- vtx2.uv[0] = shape.mesh.texcoords[shape.mesh.indices[f+2]*2+0];
- vtx2.uv[1] = shape.mesh.texcoords[shape.mesh.indices[f+2]*2+1];
+ int uv0Index = shape.mesh.indices[f+2]*2+0;
+ int uv1Index = shape.mesh.indices[f+2]*2+1;
+ if (uv0Index>=0 && uv1Index>=0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
+ {
+ vtx2.uv[0] = shape.mesh.texcoords[uv0Index];
+ vtx2.uv[1] = shape.mesh.texcoords[uv1Index];
+ } else
+ {
+ b3Warning("obj texture coordinate out-of-range!");
+ vtx2.uv[0] = 0;
+ vtx2.uv[1] = 0;
+ }
} else
{
vtx2.uv[0] = 0.5;
@@ -82,29 +118,59 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
}
+
btVector3 v0(vtx0.xyzw[0],vtx0.xyzw[1],vtx0.xyzw[2]);
btVector3 v1(vtx1.xyzw[0],vtx1.xyzw[1],vtx1.xyzw[2]);
btVector3 v2(vtx2.xyzw[0],vtx2.xyzw[1],vtx2.xyzw[2]);
- normal = (v1-v0).cross(v2-v0);
- btScalar len2 = normal.length2();
- //skip degenerate triangles
- if (len2 > SIMD_EPSILON)
+ unsigned int maxIndex = 0;
+ maxIndex = b3Max(maxIndex,shape.mesh.indices[f]*3+0);
+ maxIndex = b3Max(maxIndex,shape.mesh.indices[f]*3+1);
+ maxIndex = b3Max(maxIndex,shape.mesh.indices[f]*3+2);
+ maxIndex = b3Max(maxIndex,shape.mesh.indices[f+1]*3+0);
+ maxIndex = b3Max(maxIndex,shape.mesh.indices[f+1]*3+1);
+ maxIndex = b3Max(maxIndex,shape.mesh.indices[f+1]*3+2);
+ maxIndex = b3Max(maxIndex,shape.mesh.indices[f+2]*3+0);
+ maxIndex = b3Max(maxIndex,shape.mesh.indices[f+2]*3+1);
+ maxIndex = b3Max(maxIndex,shape.mesh.indices[f+2]*3+2);
+ bool hasNormals = (shape.mesh.normals.size() && maxIndex<shape.mesh.normals.size() );
+
+ if (flatShading || !hasNormals)
{
- normal.normalize();
+ normal = (v1-v0).cross(v2-v0);
+ btScalar len2 = normal.length2();
+ //skip degenerate triangles
+ if (len2 > SIMD_EPSILON)
+ {
+ normal.normalize();
+ } else
+ {
+ normal.setValue(0,0,0);
+ }
+ vtx0.normal[0] = normal[0];
+ vtx0.normal[1] = normal[1];
+ vtx0.normal[2] = normal[2];
+ vtx1.normal[0] = normal[0];
+ vtx1.normal[1] = normal[1];
+ vtx1.normal[2] = normal[2];
+ vtx2.normal[0] = normal[0];
+ vtx2.normal[1] = normal[1];
+ vtx2.normal[2] = normal[2];
} else
{
- normal.setValue(0,0,0);
+
+ vtx0.normal[0] = shape.mesh.normals[shape.mesh.indices[f]*3+0];
+ vtx0.normal[1] = shape.mesh.normals[shape.mesh.indices[f]*3+1];
+ vtx0.normal[2] = shape.mesh.normals[shape.mesh.indices[f]*3+2]; //shape.mesh.indices[f+1]*3+0
+ vtx1.normal[0] = shape.mesh.normals[shape.mesh.indices[f+1]*3+0];
+ vtx1.normal[1] = shape.mesh.normals[shape.mesh.indices[f+1]*3+1];
+ vtx1.normal[2] = shape.mesh.normals[shape.mesh.indices[f+1]*3+2];
+ vtx2.normal[0] = shape.mesh.normals[shape.mesh.indices[f+2]*3+0];
+ vtx2.normal[1] = shape.mesh.normals[shape.mesh.indices[f+2]*3+1];
+ vtx2.normal[2] = shape.mesh.normals[shape.mesh.indices[f+2]*3+2];
+
+
}
- vtx0.normal[0] = normal[0];
- vtx0.normal[1] = normal[1];
- vtx0.normal[2] = normal[2];
- vtx1.normal[0] = normal[0];
- vtx1.normal[1] = normal[1];
- vtx1.normal[2] = normal[2];
- vtx2.normal[0] = normal[0];
- vtx2.normal[1] = normal[1];
- vtx2.normal[2] = normal[2];
vertices->push_back(vtx0);
vertices->push_back(vtx1);
vertices->push_back(vtx2);
diff --git a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
index c93d1c4..3df100a 100644
--- a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
+++ b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
@@ -1,9 +1,9 @@
#ifndef WAVEFRONT2GRAPHICS_H
#define WAVEFRONT2GRAPHICS_H
-#include"Wavefront/tiny_obj_loader.h"
+#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include <vector>
-struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes);
+struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes, bool flatShading=false);
#endif //WAVEFRONT2GRAPHICS_H
diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
similarity index 64%
copy from examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
copy to examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
index cfedc3a..6e84379 100644
--- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
+++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
@@ -1,5 +1,5 @@
-#include "ImportURDFSetup.h"
+#include "ImportSDFSetup.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
@@ -11,13 +11,10 @@
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../../Utils/b3ResourcePath.h"
-#ifdef ENABLE_ROS_URDF
-#include "ROSURDFImporter.h"
-#endif
-#include "BulletUrdfImporter.h"
+#include "../ImportURDFDemo/BulletUrdfImporter.h"
-#include "URDF2Bullet.h"
+#include "../ImportURDFDemo/URDF2Bullet.h"
//#include "urdf_samples.h"
@@ -28,20 +25,22 @@
#include "../CommonInterfaces/CommonMultiBodyBase.h"
-#include "MyMultiBodyCreator.h"
+#include "../ImportURDFDemo/MyMultiBodyCreator.h"
-class ImportUrdfSetup : public CommonMultiBodyBase
+class ImportSDFSetup : public CommonMultiBodyBase
{
char m_fileName[1024];
- struct ImportUrdfInternalData* m_data;
+ struct ImportSDFInternalData* m_data;
bool m_useMultiBody;
+
+ //todo(erwincoumans) we need a name memory for each model
btAlignedObjectArray<std::string* > m_nameMemory;
public:
- ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
- virtual ~ImportUrdfSetup();
+ ImportSDFSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
+ virtual ~ImportSDFSetup();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
@@ -59,14 +58,14 @@ public:
};
-btAlignedObjectArray<std::string> gFileNameArray;
+static btAlignedObjectArray<std::string> gFileNameArray;
#define MAX_NUM_MOTORS 1024
-struct ImportUrdfInternalData
+struct ImportSDFInternalData
{
- ImportUrdfInternalData()
+ ImportSDFInternalData()
:m_numMotors(0)
{
for (int i=0;i<MAX_NUM_MOTORS;i++)
@@ -84,18 +83,20 @@ struct ImportUrdfInternalData
};
-ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
+ImportSDFSetup::ImportSDFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper)
{
- m_data = new ImportUrdfInternalData;
+ m_data = new ImportSDFInternalData;
+ (void)option;
- if (option==1)
- {
+// if (option==1)
+// {
m_useMultiBody = true;
- } else
- {
- m_useMultiBody = false;
- }
+//
+// } else
+// {
+// m_useMultiBody = false;
+// }
static int count = 0;
if (fileName)
@@ -109,7 +110,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
//load additional urdf file names from file
- FILE* f = fopen("urdf_files.txt","r");
+ FILE* f = fopen("sdf_files.txt","r");
if (f)
{
int result;
@@ -118,7 +119,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
do
{
result = fscanf(f,"%s",fileName);
- b3Printf("urdf_files.txt entry %s",fileName);
+ b3Printf("sdf_files.txt entry %s",fileName);
if (result==1)
{
gFileNameArray.push_back(fileName);
@@ -130,7 +131,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
if (gFileNameArray.size()==0)
{
- gFileNameArray.push_back("r2d2.urdf");
+ gFileNameArray.push_back("two_cubes.sdf");
}
@@ -144,7 +145,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
}
}
-ImportUrdfSetup::~ImportUrdfSetup()
+ImportSDFSetup::~ImportSDFSetup()
{
for (int i=0;i<m_nameMemory.size();i++)
{
@@ -163,7 +164,7 @@ static btVector4 colors[4] =
};
-btVector3 selectColor()
+static btVector3 selectColor()
{
static int curColor = 0;
@@ -173,7 +174,7 @@ btVector3 selectColor()
return color;
}
-void ImportUrdfSetup::setFileName(const char* urdfFileName)
+void ImportSDFSetup::setFileName(const char* urdfFileName)
{
memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
}
@@ -181,7 +182,7 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName)
-void ImportUrdfSetup::initPhysics()
+void ImportSDFSetup::initPhysics()
{
int upAxis = 2;
@@ -203,31 +204,10 @@ void ImportUrdfSetup::initPhysics()
m_dynamicsWorld->setGravity(gravity);
+ BulletURDFImporter u2b(m_guiHelper, 0);
+
+ bool loadOk = u2b.loadSDF(m_fileName);
- //now print the tree using the new interface
- URDFImporterInterface* bla=0;
-
- static bool newURDF = true;
- if (newURDF)
- {
- b3Printf("using new URDF\n");
- bla = new BulletURDFImporter(m_guiHelper);
- }
-#ifdef USE_ROS_URDF
- else
- {
- b3Printf("using ROS URDF\n");
- bla = new ROSURDFImporter(m_guiHelper);
- }
- newURDF = !newURDF;
-#endif//USE_ROS_URDF
- URDFImporterInterface& u2b = *bla;
- bool loadOk = u2b.loadURDF(m_fileName);
-
-#ifdef TEST_MULTIBODY_SERIALIZATION
- //test to serialize a multibody to disk or shared memory, with base, link and joint names
- btSerializer* s = new btDefaultSerializer;
-#endif //TEST_MULTIBODY_SERIALIZATION
if (loadOk)
{
@@ -235,13 +215,14 @@ void ImportUrdfSetup::initPhysics()
//u2b.printTree();
- btTransform identityTrans;
- identityTrans.setIdentity();
-
-
+ btTransform rootTrans;
+ rootTrans.setIdentity();
+
+ for (int m =0; m<u2b.getNumModels();m++)
{
-
+ u2b.activateModel(m);
+
btMultiBody* mb = 0;
@@ -250,9 +231,11 @@ void ImportUrdfSetup::initPhysics()
b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
- ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
+ u2b.getRootTransformInWorld(rootTrans);
+ ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),CUF_USE_SDF);
mb = creation.getBulletMultiBody();
-
+
+
if (m_useMultiBody && mb )
{
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
@@ -305,50 +288,10 @@ void ImportUrdfSetup::initPhysics()
}
}
- } else
- {
- if (1)
- {
- //create motors for each generic joint
- int num6Dof = creation.getNum6DofConstraints();
- for (int i=0;i<num6Dof;i++)
- {
- btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
- if (c->getUserConstraintPtr())
- {
- GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
- if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) ||
- (jointInfo->m_urdfJointType ==URDFPrismaticJoint) ||
- (jointInfo->m_urdfJointType ==URDFContinuousJoint))
- {
- int urdfLinkIndex = jointInfo->m_urdfIndex;
- std::string jointName = u2b.getJointName(urdfLinkIndex);
- char motorName[1024];
- sprintf(motorName,"%s q'", jointName.c_str());
- btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
-
- *motorVel = 0.f;
- SliderParams slider(motorName,motorVel);
- slider.m_minVal=-4;
- slider.m_maxVal=4;
- m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
- m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c;
- bool motorOn = true;
- c->enableMotor(jointInfo->m_jointAxisIndex,motorOn);
- c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000);
- c->setTargetVelocity(jointInfo->m_jointAxisIndex,0);
-
- m_data->m_numMotors++;
- }
- }
- }
- }
-
- }
+ }
}
- //the btMultiBody support is work-in-progress :-)
-
+
for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++)
{
m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
@@ -380,21 +323,10 @@ void ImportUrdfSetup::initPhysics()
m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
}
-#ifdef TEST_MULTIBODY_SERIALIZATION
- m_dynamicsWorld->serialize(s);
- b3ResourcePath p;
- char resourcePath[1024];
- if (p.findResourcePath("r2d2_multibody.bullet",resourcePath,1024))
- {
- FILE* f = fopen(resourcePath,"wb");
- fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
- fclose(f);
- }
-#endif//TEST_MULTIBODY_SERIALIZATION
}
-void ImportUrdfSetup::stepSimulation(float deltaTime)
+void ImportSDFSetup::stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
{
@@ -417,8 +349,8 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
}
}
-class CommonExampleInterface* ImportURDFCreateFunc(struct CommonExampleOptions& options)
+class CommonExampleInterface* ImportSDFCreateFunc(struct CommonExampleOptions& options)
{
-
- return new ImportUrdfSetup(options.m_guiHelper, options.m_option,options.m_fileName);
+
+ return new ImportSDFSetup(options.m_guiHelper, options.m_option,options.m_fileName);
}
diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.h b/examples/Importers/ImportSDFDemo/ImportSDFSetup.h
new file mode 100644
index 0000000..eebd416
--- /dev/null
+++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.h
@@ -0,0 +1,8 @@
+#ifndef IMPORT_SDF_SETUP_H
+#define IMPORT_SDF_SETUP_H
+
+
+class CommonExampleInterface* ImportSDFCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif //IMPORT_SDF_SETUP_H
diff --git a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp
index b7f25c3..c05138e 100644
--- a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp
+++ b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp
@@ -13,9 +13,12 @@
class ImportSTLSetup : public CommonRigidBodyBase
{
+
+ const char* m_fileName;
+ btVector3 m_scaling;
public:
- ImportSTLSetup(struct GUIHelperInterface* helper);
+ ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName);
virtual ~ImportSTLSetup();
virtual void initPhysics();
@@ -31,10 +34,19 @@ public:
};
-ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper)
-:CommonRigidBodyBase(helper)
+ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName)
+:CommonRigidBodyBase(helper),
+m_scaling(btVector3(10,10,10))
{
-
+ if (fileName)
+ {
+ m_fileName = fileName;
+ m_scaling = btVector3(0.01,0.01,0.01);
+ } else
+ {
+ m_fileName = "l_finger_tip.stl";
+
+ }
}
ImportSTLSetup::~ImportSTLSetup()
@@ -51,17 +63,16 @@ void ImportSTLSetup::initPhysics()
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
- const char* fileName = "l_finger_tip.stl";
+
char relativeFileName[1024];
- if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
+ if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024))
{
- b3Warning("Cannot find file %s\n", fileName);
+ b3Warning("Cannot find file %s\n", m_fileName);
return;
}
btVector3 shift(0,0,0);
- btVector3 scaling(10,10,10);
// int index=10;
{
@@ -81,12 +92,12 @@ void ImportSTLSetup::initPhysics()
- m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+ m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,m_scaling);
}
}
class CommonExampleInterface* ImportSTLCreateFunc(struct CommonExampleOptions& options)
{
- return new ImportSTLSetup(options.m_guiHelper);
+ return new ImportSTLSetup(options.m_guiHelper, options.m_fileName);
}
diff --git a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
index a71bd81..65bdd33 100644
--- a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
+++ b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
@@ -2,7 +2,7 @@
#ifndef LOAD_MESH_FROM_STL_H
#define LOAD_MESH_FROM_STL_H
-#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
#include <stdio.h> //fopen
#include "Bullet3Common/b3AlignedObjectArray.h"
@@ -24,17 +24,17 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
int size=0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
{
- printf("Error: Cannot access file to determine size of %s\n", relativeFileName);
+ b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName);
} else
{
if (size)
{
- printf("Open STL file of %d bytes\n",size);
+ //b3Warning("Open STL file of %d bytes\n",size);
char* memoryBuffer = new char[size+1];
int actualBytesRead = fread(memoryBuffer,1,size,file);
if (actualBytesRead!=size)
{
- printf("Error reading from file %s",relativeFileName);
+ b3Warning("Error reading from file %s",relativeFileName);
} else
{
int numTriangles = *(int*)&memoryBuffer[80];
@@ -66,21 +66,18 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
for (int i=0;i<numTriangles;i++)
{
char* curPtr = &memoryBuffer[84+i*50];
- MySTLTriangle* tri = (MySTLTriangle*) curPtr;
+ MySTLTriangle tmp;
+ memcpy(&tmp,curPtr,sizeof(MySTLTriangle));
GLInstanceVertex v0,v1,v2;
- if (i==numTriangles-2)
- {
- printf("!\n");
- }
v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
for (int v=0;v<3;v++)
{
- v0.xyzw[v] = tri->vertex0[v];
- v1.xyzw[v] = tri->vertex1[v];
- v2.xyzw[v] = tri->vertex2[v];
- v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v];
+ v0.xyzw[v] = tmp.vertex0[v];
+ v1.xyzw[v] = tmp.vertex1[v];
+ v2.xyzw[v] = tmp.vertex2[v];
+ v0.normal[v] = v1.normal[v] = v2.normal[v] = tmp.normal[v];
}
v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index 549c36b..1f16758 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -13,33 +13,47 @@ subject to the following restrictions:
#include "BulletUrdfImporter.h"
-
-
+#include "../../CommonInterfaces/CommonRenderInterface.h"
+#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include "URDFImporterInterface.h"
#include "btBulletCollisionCommon.h"
#include "../ImportObjDemo/LoadMeshFromObj.h"
#include "../ImportSTLDemo/LoadMeshFromSTL.h"
#include "../ImportColladaDemo/LoadMeshFromCollada.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
-#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
#include "Bullet3Common/b3FileUtils.h"
#include <string>
#include "../../Utils/b3ResourcePath.h"
+#include "../ImportMeshUtility/b3ImportMeshUtility.h"
+static btScalar gUrdfDefaultCollisionMargin = 0.001;
#include <iostream>
#include <fstream>
#include "UrdfParser.h"
+struct MyTexture
+{
+ int m_width;
+ int m_height;
+ unsigned char* textureData;
+};
+
struct BulletURDFInternalData
{
UrdfParser m_urdfParser;
struct GUIHelperInterface* m_guiHelper;
char m_pathPrefix[1024];
+ int m_bodyId;
btHashMap<btHashInt,btVector4> m_linkColors;
+ btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
+
+ LinkVisualShapesConverter* m_customVisualShapesConverter;
};
+
void BulletURDFImporter::printTree()
{
// btAssert(0);
@@ -55,13 +69,13 @@ enum MyFileType
-BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper)
+BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
{
m_data = new BulletURDFInternalData;
m_data->m_guiHelper = helper;
m_data->m_pathPrefix[0]=0;
-
+ m_data->m_customVisualShapesConverter = customConverter;
}
@@ -96,8 +110,8 @@ struct BulletErrorLogger : public ErrorLogger
bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
{
- m_data->m_linkColors.clear();
-
+ if (strlen(fileName)==0)
+ return false;
//int argc=0;
char relativeFileName[1024];
@@ -121,7 +135,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
std::fstream xml_file(relativeFileName, std::fstream::in);
- while ( xml_file.good() )
+ while ( xml_file.good())
{
std::string line;
std::getline( xml_file, line);
@@ -131,16 +145,83 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
}
BulletErrorLogger loggie;
+ m_data->m_urdfParser.setParseSDF(false);
bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase);
return result;
}
+int BulletURDFImporter::getNumModels() const
+{
+ return m_data->m_urdfParser.getNumModels();
+}
+
+void BulletURDFImporter::activateModel(int modelIndex)
+{
+ m_data->m_urdfParser.activateModel(modelIndex);
+}
+
+
+bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
+{
+
+ //int argc=0;
+ char relativeFileName[1024];
+
+ b3FileUtils fu;
+
+ //bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
+ bool fileFound = b3ResourcePath::findResourcePath(fileName,relativeFileName,1024);
+
+ std::string xml_string;
+ m_data->m_pathPrefix[0] = 0;
+
+ if (!fileFound){
+ std::cerr << "SDF file not found" << std::endl;
+ return false;
+ } else
+ {
+
+ int maxPathLen = 1024;
+ fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
+
+
+ std::fstream xml_file(relativeFileName, std::fstream::in);
+ while ( xml_file.good() )
+ {
+ std::string line;
+ std::getline( xml_file, line);
+ xml_string += (line + "\n");
+ }
+ xml_file.close();
+ }
+
+ BulletErrorLogger loggie;
+ //todo: quick test to see if we can re-use the URDF parser for SDF or not
+ m_data->m_urdfParser.setParseSDF(true);
+ bool result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie);
+
+ return result;
+}
+
+
const char* BulletURDFImporter::getPathPrefix()
{
return m_data->m_pathPrefix;
}
+
+void BulletURDFImporter::setBodyUniqueId(int bodyId)
+{
+ m_data->m_bodyId =bodyId;
+}
+
+
+int BulletURDFImporter::getBodyUniqueId() const
+{
+ return m_data->m_bodyId;
+}
+
BulletURDFImporter::~BulletURDFImporter()
{
@@ -174,16 +255,6 @@ void BulletURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray
}
}
-bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
-{
- btVector4* rgbaPtr = m_data->m_linkColors[linkIndex];
- if (rgbaPtr)
- {
- colorRGBA = *rgbaPtr;
- return true;
- }
- return false;
-}
std::string BulletURDFImporter::getLinkName(int linkIndex) const
{
@@ -219,34 +290,87 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
//the link->m_inertia is NOT necessarily aligned with the inertial frame
//so an additional transform might need to be computed
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
+
+
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
- mass = link->m_inertia.m_mass;
- inertialFrame = link->m_inertia.m_linkLocalFrame;
- localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
- link->m_inertia.m_izz);
+ btMatrix3x3 linkInertiaBasis;
+ btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ;
+ if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
+ {
+ linkMass = 0.f;
+ principalInertiaX = 0.f;
+ principalInertiaY = 0.f;
+ principalInertiaZ = 0.f;
+ linkInertiaBasis.setIdentity();
+ }
+ else
+ {
+ linkMass = link->m_inertia.m_mass;
+ if (link->m_inertia.m_ixy == 0.0 &&
+ link->m_inertia.m_ixz == 0.0 &&
+ link->m_inertia.m_iyz == 0.0)
+ {
+ principalInertiaX = link->m_inertia.m_ixx;
+ principalInertiaY = link->m_inertia.m_iyy;
+ principalInertiaZ = link->m_inertia.m_izz;
+ linkInertiaBasis.setIdentity();
+ }
+ else
+ {
+ principalInertiaX = link->m_inertia.m_ixx;
+ btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
+ link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
+ link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
+ btScalar threshold = 1.0e-6;
+ int numIterations = 30;
+ inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations);
+ principalInertiaX = inertiaTensor[0][0];
+ principalInertiaY = inertiaTensor[1][1];
+ principalInertiaZ = inertiaTensor[2][2];
+ }
+ }
+ mass = linkMass;
+ if (principalInertiaX < 0 ||
+ principalInertiaX > (principalInertiaY + principalInertiaZ) ||
+ principalInertiaY < 0 ||
+ principalInertiaY > (principalInertiaX + principalInertiaZ) ||
+ principalInertiaZ < 0 ||
+ principalInertiaZ > (principalInertiaX + principalInertiaY))
+ {
+ b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str());
+ principalInertiaX = 0.f;
+ principalInertiaY = 0.f;
+ principalInertiaZ = 0.f;
+ linkInertiaBasis.setIdentity();
+ }
+ localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ);
+ inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
+ inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis()*linkInertiaBasis);
}
else
- {
- mass = 1.f;
- localInertiaDiagonal.setValue(1,1,1);
- inertialFrame.setIdentity();
- }
+ {
+ mass = 1.f;
+ localInertiaDiagonal.setValue(1,1,1);
+ inertialFrame.setIdentity();
+ }
}
-bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
+bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
{
jointLowerLimit = 0.f;
jointUpperLimit = 0.f;
-
+ jointDamping = 0.f;
+ jointFriction = 0.f;
+
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
-
+ linkTransformInWorld = link->m_linkTransformInWorld;
if (link->m_parentJoint)
{
@@ -256,6 +380,9 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
jointAxisInJointSpace = pj->m_localJointAxis;
jointLowerLimit = pj->m_lowerLimit;
jointUpperLimit = pj->m_upperLimit;
+ jointDamping = pj->m_jointDamping;
+ jointFriction = pj->m_jointFriction;
+
return true;
} else
{
@@ -268,604 +395,690 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
}
-
-
-void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
+bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const
{
+ rootTransformInWorld = m_data->m_urdfParser.getModel().m_rootTransformInWorld;
+ return true;
+}
-
- GLInstanceGraphicsShape* glmesh = 0;
+static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
+{
+ btCompoundShape* compound = new btCompoundShape();
+ compound->setMargin(gUrdfDefaultCollisionMargin);
- btConvexShape* convexColShape = 0;
+ btTransform identity;
+ identity.setIdentity();
- switch (visual->m_geometry.m_type)
+ for (int s = 0; s<(int)shapes.size(); s++)
{
- case URDF_GEOM_CYLINDER:
+ btConvexHullShape* convexHull = new btConvexHullShape();
+ convexHull->setMargin(gUrdfDefaultCollisionMargin);
+ tinyobj::shape_t& shape = shapes[s];
+ int faceCount = shape.mesh.indices.size();
+
+ for (int f = 0; f<faceCount; f += 3)
{
- printf("processing a cylinder\n");
- btAlignedObjectArray<btVector3> vertices;
-
- //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
- int numSteps = 32;
- for (int i = 0; i<numSteps; i++)
- {
- btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
- btScalar cylLength = visual->m_geometry.m_cylinderLength;
-
- btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
- vertices.push_back(vert);
- vert[2] = -cylLength / 2.;
- vertices.push_back(vert);
- }
+ btVector3 pt;
+ pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
+ shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
+ shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
+
+ convexHull->addPoint(pt*geomScale,false);
- btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
- cylZShape->setMargin(0.001);
- convexColShape = cylZShape;
- break;
+ pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
+ shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
+ shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
+ convexHull->addPoint(pt*geomScale, false);
+
+ pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
+ shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
+ shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
+ convexHull->addPoint(pt*geomScale, false);
}
- case URDF_GEOM_BOX:
- {
- printf("processing a box\n");
+
+ convexHull->recalcLocalAabb();
+ convexHull->optimizeConvexHull();
+ compound->addChildShape(identity,convexHull);
+ }
+
+ return compound;
+}
+
+btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
+{
+ btCollisionShape* shape = 0;
+
+ switch (collision->m_geometry.m_type)
+ {
+ case URDF_GEOM_CYLINDER:
+ {
+ btScalar cylRadius = collision->m_geometry.m_cylinderRadius;
+ btScalar cylLength = collision->m_geometry.m_cylinderLength;
- btVector3 extents = visual->m_geometry.m_boxSize;
+ btAlignedObjectArray<btVector3> vertices;
+ //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
+ int numSteps = 32;
+ for (int i=0;i<numSteps;i++)
+ {
+
+ btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i)/numSteps)),cylRadius*btCos(SIMD_2_PI*(float(i)/numSteps)),cylLength/2.);
+ vertices.push_back(vert);
+ vert[2] = -cylLength/2.;
+ vertices.push_back(vert);
+
+ }
+ btConvexHullShape* convexHull = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+ convexHull->setMargin(gUrdfDefaultCollisionMargin);
+ convexHull->initializePolyhedralFeatures();
+ convexHull->optimizeConvexHull();
+ //btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+
+ //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+ //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+
+
+ shape = convexHull;
+ break;
+ }
+ case URDF_GEOM_BOX:
+ {
+ btVector3 extents = collision->m_geometry.m_boxSize;
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
- convexColShape = boxShape;
- convexColShape->setMargin(0.001);
- break;
- }
- case URDF_GEOM_SPHERE:
- {
- printf("processing a sphere\n");
- btScalar radius = visual->m_geometry.m_sphereRadius;
+ shape = boxShape;
+ shape ->setMargin(gUrdfDefaultCollisionMargin);
+ break;
+ }
+ case URDF_GEOM_SPHERE:
+ {
+
+ btScalar radius = collision->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
- convexColShape = sphereShape;
- convexColShape->setMargin(0.001);
- break;
+ shape = sphereShape;
+ shape ->setMargin(gUrdfDefaultCollisionMargin);
+ break;
- break;
- }
- case URDF_GEOM_MESH:
- {
- if (visual->m_name.length())
+ break;
+ }
+ case URDF_GEOM_MESH:
+ {
+ if (collision->m_name.length())
{
- printf("visual->name=%s\n", visual->m_name.c_str());
+ //b3Printf("collision->name=%s\n",collision->m_name.c_str());
}
- if (1)//visual->m_geometry)
+ if (1)
{
- if (visual->m_geometry.m_meshFileName.length())
+ if (collision->m_geometry.m_meshFileName.length())
{
- const char* filename = visual->m_geometry.m_meshFileName.c_str();
- printf("mesh->filename=%s\n", filename);
+ const char* filename = collision->m_geometry.m_meshFileName.c_str();
+ //b3Printf("mesh->filename=%s\n",filename);
char fullPath[1024];
int fileType = 0;
-
+ sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
+ b3FileUtils::toLower(fullPath);
char tmpPathPrefix[1024];
- std::string xml_string;
int maxPathLen = 1024;
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
-
- char visualPathPrefix[1024];
- sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
+
+ char collisionPathPrefix[1024];
+ sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
- sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
- b3FileUtils::toLower(fullPath);
- if (strstr(fullPath, ".dae"))
+
+ if (strstr(fullPath,".dae"))
{
fileType = FILE_COLLADA;
}
- if (strstr(fullPath, ".stl"))
+ if (strstr(fullPath,".stl"))
{
fileType = FILE_STL;
}
if (strstr(fullPath,".obj"))
- {
- fileType = FILE_OBJ;
- }
-
+ {
+ fileType = FILE_OBJ;
+ }
- sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
- FILE* f = fopen(fullPath, "rb");
+ sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
+ FILE* f = fopen(fullPath,"rb");
if (f)
{
fclose(f);
+ GLInstanceGraphicsShape* glmesh = 0;
+
-
-
switch (fileType)
{
case FILE_OBJ:
{
- glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+ if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
+ {
+ glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
+ }
+ else
+ {
+ std::vector<tinyobj::shape_t> shapes;
+ std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
+ //create a convex hull for each shape, and store it in a btCompoundShape
+
+ shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
+ return shape;
+ }
break;
}
-
case FILE_STL:
- {
- glmesh = LoadMeshFromSTL(fullPath);
+ {
+ glmesh = LoadMeshFromSTL(fullPath);
break;
- }
+ }
case FILE_COLLADA:
- {
-
- btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
- btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
- btTransform upAxisTrans; upAxisTrans.setIdentity();
- float unitMeterScaling = 1;
- int upAxis = 2;
-
- LoadMeshFromCollada(fullPath,
- visualShapes,
- visualShapeInstances,
- upAxisTrans,
- unitMeterScaling,
- upAxis);
-
- glmesh = new GLInstanceGraphicsShape;
- // int index = 0;
- glmesh->m_indices = new b3AlignedObjectArray<int>();
- glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
-
- for (int i = 0; i<visualShapeInstances.size(); i++)
{
- ColladaGraphicsInstance* instance = &visualShapeInstances[i];
- GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
-
- b3AlignedObjectArray<GLInstanceVertex> verts;
- verts.resize(gfxShape->m_vertices->size());
-
- int baseIndex = glmesh->m_vertices->size();
-
- for (int i = 0; i<gfxShape->m_vertices->size(); i++)
+
+ btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
+ btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
+ btTransform upAxisTrans;upAxisTrans.setIdentity();
+ float unitMeterScaling=1;
+ int upAxis = 2;
+ LoadMeshFromCollada(fullPath,
+ visualShapes,
+ visualShapeInstances,
+ upAxisTrans,
+ unitMeterScaling,
+ upAxis );
+
+ glmesh = new GLInstanceGraphicsShape;
+ // int index = 0;
+ glmesh->m_indices = new b3AlignedObjectArray<int>();
+ glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+ for (int i=0;i<visualShapeInstances.size();i++)
{
- verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
- verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
- verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
- verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
- verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
- verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
- verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
- verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
- verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
+ ColladaGraphicsInstance* instance = &visualShapeInstances[i];
+ GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
+
+ b3AlignedObjectArray<GLInstanceVertex> verts;
+ verts.resize(gfxShape->m_vertices->size());
- }
+ int baseIndex = glmesh->m_vertices->size();
- int curNumIndices = glmesh->m_indices->size();
- int additionalIndices = gfxShape->m_indices->size();
- glmesh->m_indices->resize(curNumIndices + additionalIndices);
- for (int k = 0; k<additionalIndices; k++)
- {
- glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
- }
+ for (int i=0;i<gfxShape->m_vertices->size();i++)
+ {
+ verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
+ verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
+ verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
+ verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
+ verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
+ verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
+ verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
+ verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
+ verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
- //compensate upAxisTrans and unitMeterScaling here
- btMatrix4x4 upAxisMat;
- upAxisMat.setIdentity();
-// upAxisMat.setPureRotation(upAxisTrans.getRotation());
- btMatrix4x4 unitMeterScalingMat;
- unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
- btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
- //btMatrix4x4 worldMat = instance->m_worldTransform;
- int curNumVertices = glmesh->m_vertices->size();
- int additionalVertices = verts.size();
- glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
+ }
- for (int v = 0; v<verts.size(); v++)
- {
- btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
- pos = worldMat*pos;
- verts[v].xyzw[0] = float(pos[0]);
- verts[v].xyzw[1] = float(pos[1]);
- verts[v].xyzw[2] = float(pos[2]);
- glmesh->m_vertices->push_back(verts[v]);
+ int curNumIndices = glmesh->m_indices->size();
+ int additionalIndices = gfxShape->m_indices->size();
+ glmesh->m_indices->resize(curNumIndices+additionalIndices);
+ for (int k=0;k<additionalIndices;k++)
+ {
+ glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex;
+ }
+
+ //compensate upAxisTrans and unitMeterScaling here
+ btMatrix4x4 upAxisMat;
+ upAxisMat.setIdentity();
+ //upAxisMat.setPureRotation(upAxisTrans.getRotation());
+ btMatrix4x4 unitMeterScalingMat;
+ unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
+ btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat;
+ //btMatrix4x4 worldMat = instance->m_worldTransform;
+ int curNumVertices = glmesh->m_vertices->size();
+ int additionalVertices = verts.size();
+ glmesh->m_vertices->reserve(curNumVertices+additionalVertices);
+
+ for(int v=0;v<verts.size();v++)
+ {
+ btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
+ pos = worldMat*pos;
+ verts[v].xyzw[0] = float(pos[0]);
+ verts[v].xyzw[1] = float(pos[1]);
+ verts[v].xyzw[2] = float(pos[2]);
+ glmesh->m_vertices->push_back(verts[v]);
+ }
}
- }
- glmesh->m_numIndices = glmesh->m_indices->size();
- glmesh->m_numvertices = glmesh->m_vertices->size();
- //glmesh = LoadMeshFromCollada(fullPath);
+ glmesh->m_numIndices = glmesh->m_indices->size();
+ glmesh->m_numvertices = glmesh->m_vertices->size();
+ //glmesh = LoadMeshFromCollada(fullPath);
- break;
- }
+ break;
+ }
default:
- {
- printf("Error: unsupported file type for Visual mesh: %s\n", fullPath);
- btAssert(0);
- }
+ {
+ b3Warning("Unsupported file type in Collision: %s\n",fullPath);
+ btAssert(0);
+ }
}
-
+
if (glmesh && (glmesh->m_numvertices>0))
{
- }
- else
+ //b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
+ //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
+ //convex->setUserIndex(shapeId);
+ btAlignedObjectArray<btVector3> convertedVerts;
+ convertedVerts.reserve(glmesh->m_numvertices);
+ for (int i=0;i<glmesh->m_numvertices;i++)
+ {
+ convertedVerts.push_back(btVector3(
+ glmesh->m_vertices->at(i).xyzw[0]*collision->m_geometry.m_meshScale[0],
+ glmesh->m_vertices->at(i).xyzw[1]*collision->m_geometry.m_meshScale[1],
+ glmesh->m_vertices->at(i).xyzw[2]*collision->m_geometry.m_meshScale[2]));
+ }
+
+ if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
+ {
+ btTriangleMesh* meshInterface = new btTriangleMesh();
+ for (int i=0;i<glmesh->m_numIndices/3;i++)
+ {
+ float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
+ float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
+ float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
+ meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
+ btVector3(v1[0],v1[1],v1[2]),
+ btVector3(v2[0],v2[1],v2[2]));
+ }
+
+ btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
+ shape = trimesh;
+ } else
+ {
+ btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
+ convexHull->optimizeConvexHull();
+ //convexHull->initializePolyhedralFeatures();
+ convexHull->setMargin(gUrdfDefaultCollisionMargin);
+ shape = convexHull;
+ }
+ } else
{
- printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
+ b3Warning("issue extracting mesh from STL file %s\n", fullPath);
}
- }
- else
+ delete glmesh;
+
+ } else
{
- printf("mesh geometry not found %s\n", fullPath);
+ b3Warning("mesh geometry not found %s\n",fullPath);
}
-
-
+
}
}
+
+ break;
+ }
+ default:
+ {
+ b3Warning("Error: unknown visual geometry type\n");
+ }
+ }
+ return shape;
+}
- break;
- }
- default:
- {
- printf("Error: unknown visual geometry type\n");
- }
- }
- //if we have a convex, tesselate into localVertices/localIndices
- if (convexColShape)
- {
- btShapeHull* hull = new btShapeHull(convexColShape);
- hull->buildHull(0.0);
- {
- // int strideInBytes = 9*sizeof(float);
- int numVertices = hull->numVertices();
- int numIndices = hull->numIndices();
+static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture>& texturesOut)
+{
-
- glmesh = new GLInstanceGraphicsShape;
- // int index = 0;
- glmesh->m_indices = new b3AlignedObjectArray<int>();
- glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+ GLInstanceGraphicsShape* glmesh = 0;
+ btConvexShape* convexColShape = 0;
- for (int i = 0; i < numVertices; i++)
+ switch (visual->m_geometry.m_type)
+ {
+ case URDF_GEOM_CYLINDER:
+ {
+ btAlignedObjectArray<btVector3> vertices;
+
+ //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
+ int numSteps = 32;
+ for (int i = 0; i<numSteps; i++)
{
- GLInstanceVertex vtx;
- btVector3 pos = hull->getVertexPointer()[i];
- vtx.xyzw[0] = pos.x();
- vtx.xyzw[1] = pos.y();
- vtx.xyzw[2] = pos.z();
- vtx.xyzw[3] = 1.f;
- pos.normalize();
- vtx.normal[0] = pos.x();
- vtx.normal[1] = pos.y();
- vtx.normal[2] = pos.z();
- vtx.uv[0] = 0.5f;
- vtx.uv[1] = 0.5f;
- glmesh->m_vertices->push_back(vtx);
- }
- btAlignedObjectArray<int> indices;
- for (int i = 0; i < numIndices; i++)
- {
- glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
+ btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
+ btScalar cylLength = visual->m_geometry.m_cylinderLength;
+
+ btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
+ vertices.push_back(vert);
+ vert[2] = -cylLength / 2.;
+ vertices.push_back(vert);
}
-
- glmesh->m_numvertices = glmesh->m_vertices->size();
- glmesh->m_numIndices = glmesh->m_indices->size();
- }
- delete convexColShape;
- convexColShape = 0;
-
- }
-
- if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
- {
-
- int baseIndex = verticesOut.size();
-
-
- for (int i = 0; i < glmesh->m_indices->size(); i++)
- {
- indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
+ btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+ cylZShape->setMargin(gUrdfDefaultCollisionMargin);
+ convexColShape = cylZShape;
+ break;
}
-
- for (int i = 0; i < glmesh->m_vertices->size(); i++)
+ case URDF_GEOM_BOX:
{
- GLInstanceVertex& v = glmesh->m_vertices->at(i);
- btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
- btVector3 vt = visualTransform*vert;
- v.xyzw[0] = vt[0];
- v.xyzw[1] = vt[1];
- v.xyzw[2] = vt[2];
- btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
- triNormal = visualTransform.getBasis()*triNormal;
- v.normal[0] = triNormal[0];
- v.normal[1] = triNormal[1];
- v.normal[2] = triNormal[2];
- verticesOut.push_back(v);
- }
- }
-}
-
-
-
-
-btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
-{
- btCollisionShape* shape = 0;
-
- switch (collision->m_geometry.m_type)
- {
- case URDF_GEOM_CYLINDER:
- {
- printf("processing a cylinder\n");
- btScalar cylRadius = collision->m_geometry.m_cylinderRadius;
- btScalar cylLength = collision->m_geometry.m_cylinderLength;
- btAlignedObjectArray<btVector3> vertices;
- //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
- int numSteps = 32;
- for (int i=0;i<numSteps;i++)
- {
-
- btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i)/numSteps)),cylRadius*btCos(SIMD_2_PI*(float(i)/numSteps)),cylLength/2.);
- vertices.push_back(vert);
- vert[2] = -cylLength/2.;
- vertices.push_back(vert);
-
- }
- btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
- cylZShape->setMargin(0.001);
- cylZShape->initializePolyhedralFeatures();
- //btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
-
- //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
- //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
-
-
- shape = cylZShape;
- break;
- }
- case URDF_GEOM_BOX:
- {
- printf("processing a box\n");
- btVector3 extents = collision->m_geometry.m_boxSize;
+ btVector3 extents = visual->m_geometry.m_boxSize;
+
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
- shape = boxShape;
- shape ->setMargin(0.001);
- break;
- }
- case URDF_GEOM_SPHERE:
- {
- printf("processing a sphere\n");
-
- btScalar radius = collision->m_geometry.m_sphereRadius;
+ convexColShape = boxShape;
+ convexColShape->setMargin(gUrdfDefaultCollisionMargin);
+ break;
+ }
+ case URDF_GEOM_SPHERE:
+ {
+ btScalar radius = visual->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
- shape = sphereShape;
- shape ->setMargin(0.001);
- break;
+ convexColShape = sphereShape;
+ convexColShape->setMargin(gUrdfDefaultCollisionMargin);
+ break;
- break;
- }
- case URDF_GEOM_MESH:
- {
- if (collision->m_name.length())
+ break;
+ }
+ case URDF_GEOM_MESH:
+ {
+ if (visual->m_name.length())
{
- printf("collision->name=%s\n",collision->m_name.c_str());
+ //b3Printf("visual->name=%s\n", visual->m_name.c_str());
}
- if (1)
+ if (1)//visual->m_geometry)
{
- if (collision->m_geometry.m_meshFileName.length())
+ if (visual->m_geometry.m_meshFileName.length())
{
- const char* filename = collision->m_geometry.m_meshFileName.c_str();
- printf("mesh->filename=%s\n",filename);
+ const char* filename = visual->m_geometry.m_meshFileName.c_str();
+ //b3Printf("mesh->filename=%s\n", filename);
char fullPath[1024];
int fileType = 0;
- sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
- b3FileUtils::toLower(fullPath);
+
char tmpPathPrefix[1024];
+ std::string xml_string;
int maxPathLen = 1024;
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
-
- char collisionPathPrefix[1024];
- sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
-
+
+ char visualPathPrefix[1024];
+ sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
- if (strstr(fullPath,".dae"))
+ sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
+ b3FileUtils::toLower(fullPath);
+ if (strstr(fullPath, ".dae"))
{
fileType = FILE_COLLADA;
}
- if (strstr(fullPath,".stl"))
+ if (strstr(fullPath, ".stl"))
{
fileType = FILE_STL;
}
if (strstr(fullPath,".obj"))
- {
- fileType = FILE_OBJ;
- }
+ {
+ fileType = FILE_OBJ;
+ }
- sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
- FILE* f = fopen(fullPath,"rb");
+
+ sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
+ FILE* f = fopen(fullPath, "rb");
if (f)
{
fclose(f);
- GLInstanceGraphicsShape* glmesh = 0;
-
switch (fileType)
{
case FILE_OBJ:
{
- glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix);
+// glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+
+ b3ImportMeshData meshData;
+ if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
+ {
+
+ if (meshData.m_textureImage)
+ {
+ MyTexture texData;
+ texData.m_width = meshData.m_textureWidth;
+ texData.m_height = meshData.m_textureHeight;
+ texData.textureData = meshData.m_textureImage;
+ texturesOut.push_back(texData);
+ }
+ glmesh = meshData.m_gfxShape;
+ }
+
break;
}
+
case FILE_STL:
- {
- glmesh = LoadMeshFromSTL(fullPath);
+ {
+ glmesh = LoadMeshFromSTL(fullPath);
break;
- }
+ }
case FILE_COLLADA:
+ {
+
+ btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
+ btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
+ btTransform upAxisTrans; upAxisTrans.setIdentity();
+ float unitMeterScaling = 1;
+ int upAxis = 2;
+
+ LoadMeshFromCollada(fullPath,
+ visualShapes,
+ visualShapeInstances,
+ upAxisTrans,
+ unitMeterScaling,
+ upAxis);
+
+ glmesh = new GLInstanceGraphicsShape;
+ // int index = 0;
+ glmesh->m_indices = new b3AlignedObjectArray<int>();
+ glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+ for (int i = 0; i<visualShapeInstances.size(); i++)
{
-
- btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
- btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
- btTransform upAxisTrans;upAxisTrans.setIdentity();
- float unitMeterScaling=1;
- int upAxis = 2;
- LoadMeshFromCollada(fullPath,
- visualShapes,
- visualShapeInstances,
- upAxisTrans,
- unitMeterScaling,
- upAxis );
-
- glmesh = new GLInstanceGraphicsShape;
- // int index = 0;
- glmesh->m_indices = new b3AlignedObjectArray<int>();
- glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+ ColladaGraphicsInstance* instance = &visualShapeInstances[i];
+ GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
- for (int i=0;i<visualShapeInstances.size();i++)
+ b3AlignedObjectArray<GLInstanceVertex> verts;
+ verts.resize(gfxShape->m_vertices->size());
+
+ int baseIndex = glmesh->m_vertices->size();
+
+ for (int i = 0; i<gfxShape->m_vertices->size(); i++)
{
- ColladaGraphicsInstance* instance = &visualShapeInstances[i];
- GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
-
- b3AlignedObjectArray<GLInstanceVertex> verts;
- verts.resize(gfxShape->m_vertices->size());
+ verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
+ verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
+ verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
+ verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
+ verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
+ verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
+ verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
+ verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
+ verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
- int baseIndex = glmesh->m_vertices->size();
+ }
- for (int i=0;i<gfxShape->m_vertices->size();i++)
- {
- verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
- verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
- verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
- verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
- verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
- verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
- verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
- verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
- verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
+ int curNumIndices = glmesh->m_indices->size();
+ int additionalIndices = gfxShape->m_indices->size();
+ glmesh->m_indices->resize(curNumIndices + additionalIndices);
+ for (int k = 0; k<additionalIndices; k++)
+ {
+ glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
+ }
- }
+ //compensate upAxisTrans and unitMeterScaling here
+ btMatrix4x4 upAxisMat;
+ upAxisMat.setIdentity();
+// upAxisMat.setPureRotation(upAxisTrans.getRotation());
+ btMatrix4x4 unitMeterScalingMat;
+ unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
+ btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
+ //btMatrix4x4 worldMat = instance->m_worldTransform;
+ int curNumVertices = glmesh->m_vertices->size();
+ int additionalVertices = verts.size();
+ glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
- int curNumIndices = glmesh->m_indices->size();
- int additionalIndices = gfxShape->m_indices->size();
- glmesh->m_indices->resize(curNumIndices+additionalIndices);
- for (int k=0;k<additionalIndices;k++)
- {
- glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex;
- }
-
- //compensate upAxisTrans and unitMeterScaling here
- btMatrix4x4 upAxisMat;
- upAxisMat.setIdentity();
- //upAxisMat.setPureRotation(upAxisTrans.getRotation());
- btMatrix4x4 unitMeterScalingMat;
- unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
- btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat;
- //btMatrix4x4 worldMat = instance->m_worldTransform;
- int curNumVertices = glmesh->m_vertices->size();
- int additionalVertices = verts.size();
- glmesh->m_vertices->reserve(curNumVertices+additionalVertices);
-
- for(int v=0;v<verts.size();v++)
- {
- btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
- pos = worldMat*pos;
- verts[v].xyzw[0] = float(pos[0]);
- verts[v].xyzw[1] = float(pos[1]);
- verts[v].xyzw[2] = float(pos[2]);
- glmesh->m_vertices->push_back(verts[v]);
- }
+ for (int v = 0; v<verts.size(); v++)
+ {
+ btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
+ pos = worldMat*pos;
+ verts[v].xyzw[0] = float(pos[0]);
+ verts[v].xyzw[1] = float(pos[1]);
+ verts[v].xyzw[2] = float(pos[2]);
+ glmesh->m_vertices->push_back(verts[v]);
}
- glmesh->m_numIndices = glmesh->m_indices->size();
- glmesh->m_numvertices = glmesh->m_vertices->size();
- //glmesh = LoadMeshFromCollada(fullPath);
-
- break;
}
+ glmesh->m_numIndices = glmesh->m_indices->size();
+ glmesh->m_numvertices = glmesh->m_vertices->size();
+ //glmesh = LoadMeshFromCollada(fullPath);
+
+ break;
+ }
default:
- {
- printf("Unsupported file type in Collision: %s\n",fullPath);
- btAssert(0);
- }
+ {
+ b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath);
+ btAssert(0);
+ }
}
-
- if (glmesh && (glmesh->m_numvertices>0))
+
+ if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0))
{
- printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
- //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
- //convex->setUserIndex(shapeId);
- btAlignedObjectArray<btVector3> convertedVerts;
- convertedVerts.reserve(glmesh->m_numvertices);
- for (int i=0;i<glmesh->m_numvertices;i++)
- {
- convertedVerts.push_back(btVector3(glmesh->m_vertices->at(i).xyzw[0],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2]));
- }
- //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
- btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
- //cylZShape->initializePolyhedralFeatures();
- //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
- //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
- cylZShape->setMargin(0.001);
- shape = cylZShape;
- } else
+ //apply the geometry scaling
+ for (int i=0;i<glmesh->m_vertices->size();i++)
+ {
+ glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
+ glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
+ glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
+ }
+
+ }
+ else
{
- printf("issue extracting mesh from STL file %s\n", fullPath);
+ b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
}
- } else
+ }
+ else
{
- printf("mesh geometry not found %s\n",fullPath);
+ b3Warning("mesh geometry not found %s\n", fullPath);
}
-
-
+
+
}
}
-
- break;
- }
- default:
- {
- printf("Error: unknown visual geometry type\n");
- }
- }
- return shape;
-}
+
+ break;
+ }
+ default:
+ {
+ b3Warning("Error: unknown visual geometry type\n");
+ }
+ }
+
+ //if we have a convex, tesselate into localVertices/localIndices
+ if ((glmesh==0) && convexColShape)
+ {
+ btShapeHull* hull = new btShapeHull(convexColShape);
+ hull->buildHull(0.0);
+ {
+ // int strideInBytes = 9*sizeof(float);
+ int numVertices = hull->numVertices();
+ int numIndices = hull->numIndices();
+
+
+ glmesh = new GLInstanceGraphicsShape;
+ // int index = 0;
+ glmesh->m_indices = new b3AlignedObjectArray<int>();
+ glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+
+ for (int i = 0; i < numVertices; i++)
+ {
+ GLInstanceVertex vtx;
+ btVector3 pos = hull->getVertexPointer()[i];
+ vtx.xyzw[0] = pos.x();
+ vtx.xyzw[1] = pos.y();
+ vtx.xyzw[2] = pos.z();
+ vtx.xyzw[3] = 1.f;
+ pos.normalize();
+ vtx.normal[0] = pos.x();
+ vtx.normal[1] = pos.y();
+ vtx.normal[2] = pos.z();
+ vtx.uv[0] = 0.5f;
+ vtx.uv[1] = 0.5f;
+ glmesh->m_vertices->push_back(vtx);
+ }
+
+ btAlignedObjectArray<int> indices;
+ for (int i = 0; i < numIndices; i++)
+ {
+ glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
+ }
+
+ glmesh->m_numvertices = glmesh->m_vertices->size();
+ glmesh->m_numIndices = glmesh->m_indices->size();
+ }
+ delete hull;
+ delete convexColShape;
+ convexColShape = 0;
+
+ }
+
+ if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
+ {
+
+ int baseIndex = verticesOut.size();
+ for (int i = 0; i < glmesh->m_indices->size(); i++)
+ {
+ indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
+ }
+
+ for (int i = 0; i < glmesh->m_vertices->size(); i++)
+ {
+ GLInstanceVertex& v = glmesh->m_vertices->at(i);
+ btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
+ btVector3 vt = visualTransform*vert;
+ v.xyzw[0] = vt[0];
+ v.xyzw[1] = vt[1];
+ v.xyzw[2] = vt[2];
+ btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
+ triNormal = visualTransform.getBasis()*triNormal;
+ v.normal[0] = triNormal[0];
+ v.normal[1] = triNormal[1];
+ v.normal[2] = triNormal[2];
+ verticesOut.push_back(v);
+ }
+ }
+ delete glmesh;
+
+}
+
-int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
+int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
- btAlignedObjectArray<GLInstanceVertex> vertices;
- btAlignedObjectArray<int> indices;
- btTransform startTrans; startTrans.setIdentity();
int graphicsIndex = -1;
-#if USE_ROS_URDF_PARSER
- for (int v = 0; v < (int)m_data->m_links[linkIndex]->visual_array.size(); v++)
- {
- const Visual* vis = m_data->m_links[linkIndex]->visual_array[v].get();
- btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);
- btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);
- btTransform childTrans;
- childTrans.setOrigin(childPos);
- childTrans.setRotation(childOrn);
-
- convertURDFToVisualShape(vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
-
- }
-#else
- const UrdfModel& model = m_data->m_urdfParser.getModel();
+ btAlignedObjectArray<GLInstanceVertex> vertices;
+ btAlignedObjectArray<int> indices;
+ btTransform startTrans; startTrans.setIdentity();
+ btAlignedObjectArray<MyTexture> textures;
+
+ const UrdfModel& model = m_data->m_urdfParser.getModel();
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
if (linkPtr)
{
const UrdfLink* link = *linkPtr;
-
+
for (int v = 0; v < link->m_visualArray.size();v++)
{
const UrdfVisual& vis = link->m_visualArray[v];
- btTransform childTrans = vis.m_linkLocalFrame;
+ btTransform childTrans = vis.m_linkLocalFrame;
btHashString matName(vis.m_materialName.c_str());
UrdfMaterial *const * matPtr = model.m_materials[matName];
if (matPtr)
@@ -874,45 +1087,92 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
}
- convertURDFToVisualShape(&vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
-
+ convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
+
+
+ }
+ }
+ if (vertices.size() && indices.size())
+ {
+// graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
+ //graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
+
+ //CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface();
+
+ if (1)
+ {
+ int textureIndex = -1;
+ if (textures.size())
+ {
+
+ textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height);
+ }
+ graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex);
}
}
-#endif
- if (vertices.size() && indices.size())
- {
- graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
- }
-
- return graphicsIndex;
-
+
+ //delete textures
+ for (int i=0;i<textures.size();i++)
+ {
+ free( textures[i].textureData);
+ }
+ return graphicsIndex;
+}
+
+
+bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
+{
+ const btVector4* rgbaPtr = m_data->m_linkColors[linkIndex];
+ if (rgbaPtr)
+ {
+ colorRGBA = *rgbaPtr;
+ return true;
+ }
+ return false;
+}
+
+bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const
+{
+ UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
+ if (linkPtr)
+ {
+ const UrdfLink* link = *linkPtr;
+ contactInfo = link->m_contactInfo;
+ return true;
+ }
+ return false;
+}
+
+void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int objectIndex) const
+{
+
+ if (m_data->m_customVisualShapesConverter)
+ {
+ const UrdfModel& model = m_data->m_urdfParser.getModel();
+ m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, objectIndex);
+ }
+
+}
+
+int BulletURDFImporter::getNumAllocatedCollisionShapes() const
+{
+ return m_data->m_allocatedCollisionShapes.size();
+}
+
+
+btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index)
+{
+ return m_data->m_allocatedCollisionShapes[index];
}
class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
btCompoundShape* compoundShape = new btCompoundShape();
- compoundShape->setMargin(0.001);
-#if USE_ROS_URDF_PARSER
- for (int v=0;v<(int)m_data->m_links[linkIndex]->collision_array.size();v++)
- {
- const Collision* col = m_data->m_links[linkIndex]->collision_array[v].get();
- btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix);
-
- if (childShape)
- {
- btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z);
- btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w);
- btTransform childTrans;
- childTrans.setOrigin(childPos);
- childTrans.setRotation(childOrn);
- compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
-
- }
- }
-#else
-
+ m_data->m_allocatedCollisionShapes.push_back(compoundShape);
+
+ compoundShape->setMargin(gUrdfDefaultCollisionMargin);
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
@@ -925,17 +1185,16 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
{
const UrdfCollision& col = link->m_collisionArray[v];
btCollisionShape* childShape = convertURDFToCollisionShape(&col ,pathPrefix);
+ m_data->m_allocatedCollisionShapes.push_back(childShape);
if (childShape)
{
btTransform childTrans = col.m_linkLocalFrame;
compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
- }
+ }
}
}
-
-#endif
return compoundShape;
}
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
index 2b9e447..127ed25 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
@@ -3,7 +3,10 @@
#include "URDFImporterInterface.h"
+#include "LinkVisualShapesConverter.h"
+
+///BulletURDFImporter can deal with URDF and (soon) SDF files
class BulletURDFImporter : public URDFImporterInterface
{
@@ -12,12 +15,18 @@ class BulletURDFImporter : public URDFImporterInterface
public:
- BulletURDFImporter(struct GUIHelperInterface* guiHelper);
+ BulletURDFImporter(struct GUIHelperInterface* guiHelper, LinkVisualShapesConverter* customConverter);
virtual ~BulletURDFImporter();
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
+ //warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
+ virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);
+ virtual int getNumModels() const;
+ virtual void activateModel(int modelIndex);
+ virtual void setBodyUniqueId(int bodyId);
+ virtual int getBodyUniqueId() const;
const char* getPathPrefix();
void printTree(); //for debugging
@@ -29,17 +38,29 @@ public:
virtual std::string getLinkName(int linkIndex) const;
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
-
+
+ virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const;
+
virtual std::string getJointName(int linkIndex) const;
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
- virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const;
+ virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
+
+ virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
- virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
+ virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
+ virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
+
+ ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
+
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
+
+ virtual int getNumAllocatedCollisionShapes() const;
+ virtual class btCollisionShape* getAllocatedCollisionShape(int index);
+
};
diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
index cfedc3a..d2fb767 100644
--- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
+++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
@@ -1,7 +1,7 @@
#include "ImportURDFSetup.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
-
+//#define TEST_MULTIBODY_SERIALIZATION 1
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "Bullet3Common/b3FileUtils.h"
@@ -11,9 +11,7 @@
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../../Utils/b3ResourcePath.h"
-#ifdef ENABLE_ROS_URDF
-#include "ROSURDFImporter.h"
-#endif
+
#include "BulletUrdfImporter.h"
@@ -38,7 +36,8 @@ class ImportUrdfSetup : public CommonMultiBodyBase
struct ImportUrdfInternalData* m_data;
bool m_useMultiBody;
btAlignedObjectArray<std::string* > m_nameMemory;
-
+ btScalar m_grav;
+ int m_upAxis;
public:
ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
virtual ~ImportUrdfSetup();
@@ -67,7 +66,8 @@ btAlignedObjectArray<std::string> gFileNameArray;
struct ImportUrdfInternalData
{
ImportUrdfInternalData()
- :m_numMotors(0)
+ :m_numMotors(0),
+ m_mb(0)
{
for (int i=0;i<MAX_NUM_MOTORS;i++)
{
@@ -76,16 +76,21 @@ struct ImportUrdfInternalData
}
}
+
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
int m_numMotors;
+ btMultiBody* m_mb;
+ btRigidBody* m_rb;
};
ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
- :CommonMultiBodyBase(helper)
+ :CommonMultiBodyBase(helper),
+ m_grav(0),
+ m_upAxis(2)
{
m_data = new ImportUrdfInternalData;
@@ -163,7 +168,7 @@ static btVector4 colors[4] =
};
-btVector3 selectColor()
+static btVector3 selectColor()
{
static int curColor = 0;
@@ -184,9 +189,9 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName)
void ImportUrdfSetup::initPhysics()
{
- int upAxis = 2;
- m_guiHelper->setUpAxis(upAxis);
-
+
+ m_guiHelper->setUpAxis(m_upAxis);
+
this->createEmptyDynamicsWorld();
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@@ -197,32 +202,19 @@ void ImportUrdfSetup::initPhysics()
);//+btIDebugDraw::DBG_DrawConstraintLimits);
- btVector3 gravity(0,0,0);
- gravity[upAxis]=-9.8;
-
- m_dynamicsWorld->setGravity(gravity);
-
+ if (m_guiHelper->getParameterInterface())
+ {
+ SliderParams slider("Gravity", &m_grav);
+ slider.m_minVal = -10;
+ slider.m_maxVal = 10;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
- //now print the tree using the new interface
- URDFImporterInterface* bla=0;
+ BulletURDFImporter u2b(m_guiHelper, 0);
- static bool newURDF = true;
- if (newURDF)
- {
- b3Printf("using new URDF\n");
- bla = new BulletURDFImporter(m_guiHelper);
- }
-#ifdef USE_ROS_URDF
- else
- {
- b3Printf("using ROS URDF\n");
- bla = new ROSURDFImporter(m_guiHelper);
- }
- newURDF = !newURDF;
-#endif//USE_ROS_URDF
- URDFImporterInterface& u2b = *bla;
- bool loadOk = u2b.loadURDF(m_fileName);
+
+ bool loadOk = u2b.loadURDF(m_fileName);
#ifdef TEST_MULTIBODY_SERIALIZATION
//test to serialize a multibody to disk or shared memory, with base, link and joint names
@@ -242,7 +234,6 @@ void ImportUrdfSetup::initPhysics()
{
- btMultiBody* mb = 0;
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
@@ -251,7 +242,13 @@ void ImportUrdfSetup::initPhysics()
MyMultiBodyCreator creation(m_guiHelper);
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
- mb = creation.getBulletMultiBody();
+ m_data->m_rb = creation.getRigidBody();
+ m_data->m_mb = creation.getBulletMultiBody();
+ btMultiBody* mb = m_data->m_mb;
+ for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
+ {
+ m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
+ }
if (m_useMultiBody && mb )
{
@@ -360,14 +357,15 @@ void ImportUrdfSetup::initPhysics()
if (createGround)
{
btVector3 groundHalfExtents(20,20,20);
- groundHalfExtents[upAxis]=1.f;
+ groundHalfExtents[m_upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
+ m_collisionShapes.push_back(box);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
- groundOrigin[upAxis]=-2.5;
+ groundOrigin[m_upAxis]=-2.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
//m_dynamicsWorld->removeRigidBody(body);
@@ -376,10 +374,10 @@ void ImportUrdfSetup::initPhysics()
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
- ///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
- m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
+
}
+
#ifdef TEST_MULTIBODY_SERIALIZATION
m_dynamicsWorld->serialize(s);
b3ResourcePath p;
@@ -398,6 +396,10 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
{
+ btVector3 gravity(0, 0, 0);
+ gravity[m_upAxis] = m_grav;
+ m_dynamicsWorld->setGravity(gravity);
+
for (int i=0;i<m_data->m_numMotors;i++)
{
if (m_data->m_jointMotors[i])
diff --git a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h
new file mode 100644
index 0000000..814838c
--- /dev/null
+++ b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h
@@ -0,0 +1,9 @@
+#ifndef LINK_VISUAL_SHAPES_CONVERTER_H
+#define LINK_VISUAL_SHAPES_CONVERTER_H
+
+struct LinkVisualShapesConverter
+{
+ virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0;
+};
+
+#endif //LINK_VISUAL_SHAPES_CONVERTER_H
diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
index dd36baa..3d34e53 100644
--- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
+++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
@@ -1,6 +1,6 @@
#include "MyMultiBodyCreator.h"
-#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
@@ -13,6 +13,7 @@
MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
:m_bulletMultiBody(0),
+ m_rigidBody(0),
m_guiHelper(guiHelper)
{
}
@@ -31,10 +32,10 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
{
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
rbci.m_startWorldTransform = initialWorldTrans;
- btRigidBody* body = new btRigidBody(rbci);
- body->forceActivationState(DISABLE_DEACTIVATION);
+ m_rigidBody = new btRigidBody(rbci);
+ m_rigidBody->forceActivationState(DISABLE_DEACTIVATION);
- return body;
+ return m_rigidBody;
}
class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)
diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
index 7334fbd..453f973 100644
--- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
+++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
@@ -24,6 +24,7 @@ class MyMultiBodyCreator : public MultiBodyCreationInterface
protected:
btMultiBody* m_bulletMultiBody;
+ btRigidBody* m_rigidBody;
struct GUIHelperInterface* m_guiHelper;
@@ -62,6 +63,10 @@ public:
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
btMultiBody* getBulletMultiBody();
+ btRigidBody* getRigidBody()
+ {
+ return m_rigidBody;
+ }
int getNum6DofConstraints() const
{
diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp b/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp
deleted file mode 100644
index 6b8fcc5..0000000
--- a/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp
+++ /dev/null
@@ -1,899 +0,0 @@
-/* Copyright (C) 2015 Google
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#include "ROSURDFImporter.h"
-
-
-#include "URDFImporterInterface.h"
-#include "btBulletCollisionCommon.h"
-#include "../ImportObjDemo/LoadMeshFromObj.h"
-#include "../ImportSTLDemo/LoadMeshFromSTL.h"
-#include "../ImportColladaDemo/LoadMeshFromCollada.h"
-#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
-#include "../CommonInterfaces/CommonGUIHelperInterface.h"
-#include "Bullet3Common/b3FileUtils.h"
-#include <string>
-
-#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
-#include <iostream>
-#include <fstream>
-using namespace urdf;
-
-void ROSconvertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut);
-btCollisionShape* ROSconvertURDFToCollisionShape(const Collision* visual, const char* pathPrefix);
-
-
-
-
-static void ROSprintTreeInternal(my_shared_ptr<const Link> link,int level = 0)
-{
- level+=2;
- int count = 0;
- for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
- {
- if (*child)
- {
- for(int j=0;j<level;j++) std::cout << " "; //indent
- std::cout << "child(" << (count++)+1 << "): " << (*child)->name << std::endl;
- // first grandchild
- ROSprintTreeInternal(*child,level);
- }
- else
- {
- for(int j=0;j<level;j++) std::cout << " "; //indent
- std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
- }
- }
-
-}
-
-
-
-
-struct ROSURDFInternalData
-{
- my_shared_ptr<ModelInterface> m_robot;
- std::vector<my_shared_ptr<Link> > m_links;
- struct GUIHelperInterface* m_guiHelper;
- char m_pathPrefix[1024];
-
-};
-
-void ROSURDFImporter::printTree()
-{
- ROSprintTreeInternal(m_data->m_robot->getRoot(),0);
-}
-
-enum MyFileType
-{
- FILE_STL=1,
- FILE_COLLADA=2,
- FILE_OBJ=3,
-};
-
-
-
-ROSURDFImporter::ROSURDFImporter(struct GUIHelperInterface* helper)
-{
- m_data = new ROSURDFInternalData;
- m_data->m_robot = 0;
- m_data->m_guiHelper = helper;
- m_data->m_pathPrefix[0]=0;
-
-
-
-}
-
-bool ROSURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
-{
-
-
-//int argc=0;
- char relativeFileName[1024];
-
- b3FileUtils fu;
-
- bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
-
- std::string xml_string;
- m_data->m_pathPrefix[0] = 0;
-
- if (!fileFound){
- std::cerr << "URDF file not found" << std::endl;
- return false;
- } else
- {
-
- int maxPathLen = 1024;
- fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
-
-
- std::fstream xml_file(relativeFileName, std::fstream::in);
- while ( xml_file.good() )
- {
- std::string line;
- std::getline( xml_file, line);
- xml_string += (line + "\n");
- }
- xml_file.close();
- }
-
- my_shared_ptr<ModelInterface> robot = parseURDF(xml_string);
- if (!robot){
- std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
- return false;
- }
- std::cout << "robot name is: " << robot->getName() << std::endl;
-
- // get info from parser
- std::cout << "Successfully Parsed URDF" << std::endl;
- // get root link
- my_shared_ptr<const Link> root_link=robot->getRoot();
- if (!root_link)
- {
- std::cout << "Failed to find root link in URDF" << std::endl;
- return false;
- }
-
-
- m_data->m_robot = robot;
-
- m_data->m_robot->getLinks(m_data->m_links);
-
- //initialize the 'index' of each link
- for (int i=0;i<(int)m_data->m_links.size();i++)
- {
- m_data->m_links[i]->m_link_index = i;
- }
-
- return true;
-}
-
-const char* ROSURDFImporter::getPathPrefix()
-{
- return m_data->m_pathPrefix;
-}
-
-
-ROSURDFImporter::~ROSURDFImporter()
-{
- delete m_data;
-}
-
-
-int ROSURDFImporter::getRootLinkIndex() const
-{
- if (m_data->m_links.size())
- {
- int rootLinkIndex = m_data->m_robot->getRoot()->m_link_index;
- // btAssert(m_links[0]->m_link_index == rootLinkIndex);
- return rootLinkIndex;
- }
- return -1;
-};
-
-void ROSURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
-{
- childLinkIndices.resize(0);
- int numChildren = m_data->m_links[linkIndex]->child_links.size();
-
- for (int i=0;i<numChildren;i++)
- {
- int childIndex =m_data->m_links[linkIndex]->child_links[i]->m_link_index;
- childLinkIndices.push_back(childIndex);
- }
-}
-
-std::string ROSURDFImporter::getLinkName(int linkIndex) const
-{
- std::string n = m_data->m_links[linkIndex]->name;
- return n;
-}
-
-std::string ROSURDFImporter::getJointName(int linkIndex) const
-{
- return m_data->m_links[linkIndex]->parent_joint->name;
-}
-
-
-void ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
-{
- if ((*m_data->m_links[linkIndex]).inertial)
- {
- mass = (*m_data->m_links[linkIndex]).inertial->mass;
- localInertiaDiagonal.setValue((*m_data->m_links[linkIndex]).inertial->ixx,(*m_data->m_links[linkIndex]).inertial->iyy,(*m_data->m_links[linkIndex]).inertial->izz);
- inertialFrame.setOrigin(btVector3((*m_data->m_links[linkIndex]).inertial->origin.position.x,(*m_data->m_links[linkIndex]).inertial->origin.position.y,(*m_data->m_links[linkIndex]).inertial->origin.position.z));
- inertialFrame.setRotation(btQuaternion((*m_data->m_links[linkIndex]).inertial->origin.rotation.x,(*m_data->m_links[linkIndex]).inertial->origin.rotation.y,(*m_data->m_links[linkIndex]).inertial->origin.rotation.z,(*m_data->m_links[linkIndex]).inertial->origin.rotation.w));
- } else
- {
- mass = 1.f;
- localInertiaDiagonal.setValue(1,1,1);
- inertialFrame.setIdentity();
- }
-}
-
-bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
-{
- jointLowerLimit = 0.f;
- jointUpperLimit = 0.f;
-
- if ((*m_data->m_links[urdfLinkIndex]).parent_joint)
- {
- my_shared_ptr<Joint> pj =(*m_data->m_links[urdfLinkIndex]).parent_joint;
-
- const urdf::Vector3 pos = pj->parent_to_joint_origin_transform.position;
- const urdf::Rotation orn = pj->parent_to_joint_origin_transform.rotation;
-
- jointAxisInJointSpace.setValue(pj->axis.x,pj->axis.y,pj->axis.z);
- parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
- parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));
-
- switch (pj->type)
- {
- case Joint::REVOLUTE:
- jointType = URDFRevoluteJoint;
- break;
- case Joint::FIXED:
- jointType = URDFFixedJoint;
- break;
- case Joint::PRISMATIC:
- jointType = URDFPrismaticJoint;
- break;
- case Joint::PLANAR:
- jointType = URDFPlanarJoint;
- break;
- case Joint::CONTINUOUS:
- jointType = URDFContinuousJoint;
- break;
- default:
- {
- printf("Error: unknown joint type %d\n", pj->type);
- btAssert(0);
- }
-
- };
-
- if (pj->limits)
- {
- jointLowerLimit = pj->limits.get()->lower;
- jointUpperLimit = pj->limits.get()->upper;
- }
- return true;
- } else
- {
- parent2joint.setIdentity();
- return false;
- }
-}
-
-
-
-void ROSconvertURDFToVisualShape(const Visual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
-{
-
-
- GLInstanceGraphicsShape* glmesh = 0;
-
- btConvexShape* convexColShape = 0;
-
- switch (visual->geometry->type)
- {
- case Geometry::CYLINDER:
- {
- printf("processing a cylinder\n");
- urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
- btAlignedObjectArray<btVector3> vertices;
-
- //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
- int numSteps = 32;
- for (int i = 0; i<numSteps; i++)
- {
-
- btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i) / numSteps)), cyl->radius*btCos(SIMD_2_PI*(float(i) / numSteps)), cyl->length / 2.);
- vertices.push_back(vert);
- vert[2] = -cyl->length / 2.;
- vertices.push_back(vert);
- }
-
- btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
- cylZShape->setMargin(0.001);
- convexColShape = cylZShape;
- break;
- }
- case Geometry::BOX:
- {
- printf("processing a box\n");
- urdf::Box* box = (urdf::Box*)visual->geometry.get();
- btVector3 extents(box->dim.x, box->dim.y, box->dim.z);
- btBoxShape* boxShape = new btBoxShape(extents*0.5f);
- //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
- convexColShape = boxShape;
- convexColShape->setMargin(0.001);
- break;
- }
- case Geometry::SPHERE:
- {
- printf("processing a sphere\n");
- urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
- btScalar radius = sphere->radius;
- btSphereShape* sphereShape = new btSphereShape(radius);
- convexColShape = sphereShape;
- convexColShape->setMargin(0.001);
- break;
-
- break;
- }
- case Geometry::MESH:
- {
- if (visual->name.length())
- {
- printf("visual->name=%s\n", visual->name.c_str());
- }
- if (visual->geometry)
- {
- const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get();
- if (mesh->filename.length())
- {
- const char* filename = mesh->filename.c_str();
- printf("mesh->filename=%s\n", filename);
- char fullPath[1024];
- int fileType = 0;
-
- char tmpPathPrefix[1024];
- std::string xml_string;
- int maxPathLen = 1024;
- b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
-
- char visualPathPrefix[1024];
- sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
-
-
- sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
- b3FileUtils::toLower(fullPath);
- if (strstr(fullPath, ".dae"))
- {
- fileType = FILE_COLLADA;
- }
- if (strstr(fullPath, ".stl"))
- {
- fileType = FILE_STL;
- }
- if (strstr(fullPath,".obj"))
- {
- fileType = FILE_OBJ;
- }
-
-
- sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
- FILE* f = fopen(fullPath, "rb");
- if (f)
- {
- fclose(f);
-
-
-
- switch (fileType)
- {
- case FILE_OBJ:
- {
- glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
- break;
- }
-
- case FILE_STL:
- {
- glmesh = LoadMeshFromSTL(fullPath);
- break;
- }
- case FILE_COLLADA:
- {
-
- btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
- btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
- btTransform upAxisTrans; upAxisTrans.setIdentity();
- float unitMeterScaling = 1;
- int upAxis = 2;
-
- LoadMeshFromCollada(fullPath,
- visualShapes,
- visualShapeInstances,
- upAxisTrans,
- unitMeterScaling,
- upAxis);
-
- glmesh = new GLInstanceGraphicsShape;
- //int index = 0;
- glmesh->m_indices = new b3AlignedObjectArray<int>();
- glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
-
- for (int i = 0; i<visualShapeInstances.size(); i++)
- {
- ColladaGraphicsInstance* instance = &visualShapeInstances[i];
- GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
-
- b3AlignedObjectArray<GLInstanceVertex> verts;
- verts.resize(gfxShape->m_vertices->size());
-
- int baseIndex = glmesh->m_vertices->size();
-
- for (int i = 0; i<gfxShape->m_vertices->size(); i++)
- {
- verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
- verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
- verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
- verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
- verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
- verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
- verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
- verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
- verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
-
- }
-
- int curNumIndices = glmesh->m_indices->size();
- int additionalIndices = gfxShape->m_indices->size();
- glmesh->m_indices->resize(curNumIndices + additionalIndices);
- for (int k = 0; k<additionalIndices; k++)
- {
- glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
- }
-
- //compensate upAxisTrans and unitMeterScaling here
- btMatrix4x4 upAxisMat;
- upAxisMat.setIdentity();
-// upAxisMat.setPureRotation(upAxisTrans.getRotation());
- btMatrix4x4 unitMeterScalingMat;
- unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
- btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
- //btMatrix4x4 worldMat = instance->m_worldTransform;
- int curNumVertices = glmesh->m_vertices->size();
- int additionalVertices = verts.size();
- glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
-
- for (int v = 0; v<verts.size(); v++)
- {
- btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
- pos = worldMat*pos;
- verts[v].xyzw[0] = float(pos[0]);
- verts[v].xyzw[1] = float(pos[1]);
- verts[v].xyzw[2] = float(pos[2]);
- glmesh->m_vertices->push_back(verts[v]);
- }
- }
- glmesh->m_numIndices = glmesh->m_indices->size();
- glmesh->m_numvertices = glmesh->m_vertices->size();
- //glmesh = LoadMeshFromCollada(fullPath);
-
- break;
- }
- default:
- {
- printf("Error: unsupported file type for Visual mesh: %s\n", fullPath);
- btAssert(0);
- }
- }
-
-
- if (glmesh && (glmesh->m_numvertices>0))
- {
- }
- else
- {
- printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
- }
-
- }
- else
- {
- printf("mesh geometry not found %s\n", fullPath);
- }
-
-
- }
- }
-
-
- break;
- }
- default:
- {
- printf("Error: unknown visual geometry type\n");
- }
- }
-
- //if we have a convex, tesselate into localVertices/localIndices
- if (convexColShape)
- {
- btShapeHull* hull = new btShapeHull(convexColShape);
- hull->buildHull(0.0);
- {
- // int strideInBytes = 9*sizeof(float);
- int numVertices = hull->numVertices();
- int numIndices = hull->numIndices();
-
-
- glmesh = new GLInstanceGraphicsShape;
- // int index = 0;
- glmesh->m_indices = new b3AlignedObjectArray<int>();
- glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
-
-
- for (int i = 0; i < numVertices; i++)
- {
- GLInstanceVertex vtx;
- btVector3 pos = hull->getVertexPointer()[i];
- vtx.xyzw[0] = pos.x();
- vtx.xyzw[1] = pos.y();
- vtx.xyzw[2] = pos.z();
- vtx.xyzw[3] = 1.f;
- pos.normalize();
- vtx.normal[0] = pos.x();
- vtx.normal[1] = pos.y();
- vtx.normal[2] = pos.z();
- vtx.uv[0] = 0.5f;
- vtx.uv[1] = 0.5f;
- glmesh->m_vertices->push_back(vtx);
- }
-
- btAlignedObjectArray<int> indices;
- for (int i = 0; i < numIndices; i++)
- {
- glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
- }
-
- glmesh->m_numvertices = glmesh->m_vertices->size();
- glmesh->m_numIndices = glmesh->m_indices->size();
- }
- delete convexColShape;
- convexColShape = 0;
-
- }
-
- if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
- {
-
- int baseIndex = verticesOut.size();
-
-
-
- for (int i = 0; i < glmesh->m_indices->size(); i++)
- {
- indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
- }
-
- for (int i = 0; i < glmesh->m_vertices->size(); i++)
- {
- GLInstanceVertex& v = glmesh->m_vertices->at(i);
- btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
- btVector3 vt = visualTransform*vert;
- v.xyzw[0] = vt[0];
- v.xyzw[1] = vt[1];
- v.xyzw[2] = vt[2];
- btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
- triNormal = visualTransform.getBasis()*triNormal;
- v.normal[0] = triNormal[0];
- v.normal[1] = triNormal[1];
- v.normal[2] = triNormal[2];
- verticesOut.push_back(v);
- }
- }
-}
-
-
-
-
-btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* urdfPathPrefix)
-{
- btCollisionShape* shape = 0;
-
- switch (visual->geometry->type)
- {
- case Geometry::CYLINDER:
- {
- printf("processing a cylinder\n");
- urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
-
- btAlignedObjectArray<btVector3> vertices;
- //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
- int numSteps = 32;
- for (int i=0;i<numSteps;i++)
- {
-
- btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.);
- vertices.push_back(vert);
- vert[2] = -cyl->length/2.;
- vertices.push_back(vert);
-
- }
- btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
- cylZShape->setMargin(0.001);
- cylZShape->initializePolyhedralFeatures();
- //btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
-
- //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
- //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
-
-
- shape = cylZShape;
- break;
- }
- case Geometry::BOX:
- {
- printf("processing a box\n");
- urdf::Box* box = (urdf::Box*)visual->geometry.get();
- btVector3 extents(box->dim.x,box->dim.y,box->dim.z);
- btBoxShape* boxShape = new btBoxShape(extents*0.5f);
- //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
- shape = boxShape;
- shape ->setMargin(0.001);
- break;
- }
- case Geometry::SPHERE:
- {
- printf("processing a sphere\n");
- urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
- btScalar radius = sphere->radius;
- btSphereShape* sphereShape = new btSphereShape(radius);
- shape = sphereShape;
- shape ->setMargin(0.001);
- break;
-
- break;
- }
- case Geometry::MESH:
- {
- if (visual->name.length())
- {
- printf("visual->name=%s\n",visual->name.c_str());
- }
- if (visual->geometry)
- {
- const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get();
- if (mesh->filename.length())
- {
- const char* filename = mesh->filename.c_str();
- printf("mesh->filename=%s\n",filename);
- char fullPath[1024];
- int fileType = 0;
- sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
- b3FileUtils::toLower(fullPath);
- char tmpPathPrefix[1024];
- int maxPathLen = 1024;
- b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
-
- char collisionPathPrefix[1024];
- sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
-
-
-
- if (strstr(fullPath,".dae"))
- {
- fileType = FILE_COLLADA;
- }
- if (strstr(fullPath,".stl"))
- {
- fileType = FILE_STL;
- }
- if (strstr(fullPath,".obj"))
- {
- fileType = FILE_OBJ;
- }
-
- sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
- FILE* f = fopen(fullPath,"rb");
- if (f)
- {
- fclose(f);
- GLInstanceGraphicsShape* glmesh = 0;
-
-
- switch (fileType)
- {
- case FILE_OBJ:
- {
- glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix);
- break;
- }
- case FILE_STL:
- {
- glmesh = LoadMeshFromSTL(fullPath);
- break;
- }
- case FILE_COLLADA:
- {
-
- btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
- btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
- btTransform upAxisTrans;upAxisTrans.setIdentity();
- float unitMeterScaling=1;
- int upAxis = 2;
- LoadMeshFromCollada(fullPath,
- visualShapes,
- visualShapeInstances,
- upAxisTrans,
- unitMeterScaling,
- upAxis );
-
- glmesh = new GLInstanceGraphicsShape;
- // int index = 0;
- glmesh->m_indices = new b3AlignedObjectArray<int>();
- glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
-
- for (int i=0;i<visualShapeInstances.size();i++)
- {
- ColladaGraphicsInstance* instance = &visualShapeInstances[i];
- GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
-
- b3AlignedObjectArray<GLInstanceVertex> verts;
- verts.resize(gfxShape->m_vertices->size());
-
- int baseIndex = glmesh->m_vertices->size();
-
- for (int i=0;i<gfxShape->m_vertices->size();i++)
- {
- verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
- verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
- verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
- verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
- verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
- verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
- verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
- verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
- verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
-
- }
-
- int curNumIndices = glmesh->m_indices->size();
- int additionalIndices = gfxShape->m_indices->size();
- glmesh->m_indices->resize(curNumIndices+additionalIndices);
- for (int k=0;k<additionalIndices;k++)
- {
- glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex;
- }
-
- //compensate upAxisTrans and unitMeterScaling here
- btMatrix4x4 upAxisMat;
- upAxisMat.setPureRotation(upAxisTrans.getRotation());
- btMatrix4x4 unitMeterScalingMat;
- unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
- btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat;
- //btMatrix4x4 worldMat = instance->m_worldTransform;
- int curNumVertices = glmesh->m_vertices->size();
- int additionalVertices = verts.size();
- glmesh->m_vertices->reserve(curNumVertices+additionalVertices);
-
- for(int v=0;v<verts.size();v++)
- {
- btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
- pos = worldMat*pos;
- verts[v].xyzw[0] = float(pos[0]);
- verts[v].xyzw[1] = float(pos[1]);
- verts[v].xyzw[2] = float(pos[2]);
- glmesh->m_vertices->push_back(verts[v]);
- }
- }
- glmesh->m_numIndices = glmesh->m_indices->size();
- glmesh->m_numvertices = glmesh->m_vertices->size();
- //glmesh = LoadMeshFromCollada(fullPath);
-
- break;
- }
- default:
- {
- printf("Unsupported file type in Collision: %s\n",fullPath);
- btAssert(0);
- }
- }
-
-
- if (glmesh && (glmesh->m_numvertices>0))
- {
- printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
- //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
- //convex->setUserIndex(shapeId);
- btAlignedObjectArray<btVector3> convertedVerts;
- convertedVerts.reserve(glmesh->m_numvertices);
- for (int i=0;i<glmesh->m_numvertices;i++)
- {
- convertedVerts.push_back(btVector3(glmesh->m_vertices->at(i).xyzw[0],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2]));
- }
- //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
- btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
- //cylZShape->initializePolyhedralFeatures();
- //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
- //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
- cylZShape->setMargin(0.001);
- shape = cylZShape;
- } else
- {
- printf("issue extracting mesh from STL file %s\n", fullPath);
- }
-
- } else
- {
- printf("mesh geometry not found %s\n",fullPath);
- }
-
-
- }
- }
-
-
- break;
- }
- default:
- {
- printf("Error: unknown visual geometry type\n");
- }
- }
- return shape;
-}
-
-
-
-
-int ROSURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
-{
- btAlignedObjectArray<GLInstanceVertex> vertices;
- btAlignedObjectArray<int> indices;
- btTransform startTrans; startTrans.setIdentity();
- int graphicsIndex = -1;
-
- for (int v = 0; v < (int)m_data->m_links[linkIndex]->visual_array.size(); v++)
- {
- const Visual* vis = m_data->m_links[linkIndex]->visual_array[v].get();
- btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);
- btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);
- btTransform childTrans;
- childTrans.setOrigin(childPos);
- childTrans.setRotation(childOrn);
-
- ROSconvertURDFToVisualShape(vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
-
- }
-
- if (vertices.size() && indices.size())
- {
- graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
- }
-
- return graphicsIndex;
-
-}
-
- class btCompoundShape* ROSURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
-{
-
- btCompoundShape* compoundShape = new btCompoundShape();
- compoundShape->setMargin(0.001);
-
- for (int v=0;v<(int)m_data->m_links[linkIndex]->collision_array.size();v++)
- {
- const Collision* col = m_data->m_links[linkIndex]->collision_array[v].get();
- btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix);
-
- if (childShape)
- {
- btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z);
- btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w);
- btTransform childTrans;
- childTrans.setOrigin(childPos);
- childTrans.setRotation(childOrn);
- compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
-
- }
- }
-
- return compoundShape;
-}
diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h
deleted file mode 100644
index 93e7bb1..0000000
--- a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#ifndef ROS_URDF_IMPORTER_H
-#define ROS_URDF_IMPORTER_H
-
-#include "URDFImporterInterface.h"
-
-
-class ROSURDFImporter : public URDFImporterInterface
-{
-
- struct ROSURDFInternalData* m_data;
-
-
-public:
-
- ROSURDFImporter(struct GUIHelperInterface* guiHelper);
-
- virtual ~ROSURDFImporter();
-
- // Note that forceFixedBase is ignored in this implementation.
- virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
-
- virtual const char* getPathPrefix();
-
- void printTree(); //for debugging
-
- virtual int getRootLinkIndex() const;
-
- virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
-
- virtual std::string getLinkName(int linkIndex) const;
-
- virtual std::string getJointName(int linkIndex) const;
-
- virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
-
- virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const;
-
- virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
-
- virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
-
-};
-
-
-#endif //ROS_URDF_IMPORTER_H
diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
index 6d87d2e..d9e58ef 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
@@ -1,4 +1,3 @@
-#include "URDFImporterInterface.h"
#include <stdio.h>
#include "LinearMath/btTransform.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
@@ -7,6 +6,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "URDF2Bullet.h"
#include "URDFImporterInterface.h"
#include "MultiBodyCreationInterface.h"
#include <string>
@@ -143,7 +143,12 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat
}
-void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
+void ConvertURDF2BulletInternal(
+ const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
+ URDF2BulletCachedData& cache, int urdfLinkIndex,
+ const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
+ bool createMultiBody, const char* pathPrefix,
+ int flags = 0)
{
//b3Printf("start converting/extracting data from URDF interface\n");
@@ -157,8 +162,6 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
btRigidBody* parentRigidBody = 0;
- //std::string name = u2b.getLinkName(urdfLinkIndex);
- //b3Printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
//b3Printf("mb link index = %d\n",mbLinkIndex);
btTransform parentLocalInertialFrame;
@@ -195,33 +198,55 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
btVector3 jointAxisInJointSpace;
btScalar jointLowerLimit;
btScalar jointUpperLimit;
+ btScalar jointDamping;
+ btScalar jointFriction;
- bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit);
-
-
- linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
-
- int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
+ bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
+ if (flags & CUF_USE_SDF)
+ {
+ parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace;
+ }
+ else
+ {
+ linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
+ }
+
+
btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
+
+ int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
+
+
+
if (compoundShape)
{
- btVector3 color = selectColor2();
+ btVector4 color = selectColor2();
+ u2b.getLinkColor(urdfLinkIndex,color);
/*
if (visual->material.get())
{
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
}
*/
- //btVector3 localInertiaDiagonal(0, 0, 0);
- //if (mass)
- //{
- // shape->calculateLocalInertia(mass, localInertiaDiagonal);
- //}
+ if (mass)
+ {
+ if (!(flags & CUF_USE_URDF_INERTIA))
+ {
+ compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
+ }
+ URDFLinkContactInfo contactInfo;
+ u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
+ //temporary inertia scaling until we load inertia from URDF
+ if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
+ {
+ localInertiaDiagonal*=contactInfo.m_inertiaScaling;
+ }
+ }
btRigidBody* linkRigidBody = 0;
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
@@ -231,12 +256,14 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
linkRigidBody = body;
- world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
+ world1->addRigidBody(body);
compoundShape->setUserIndex(graphicsIndex);
creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
+
+ //untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body);
} else
{
if (cache.m_bulletMultiBody==0)
@@ -258,6 +285,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
btTransform offsetInA,offsetInB;
offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
offsetInB = localInertialFrame.inverse();
+ btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
bool disableParentCollision = true;
switch (jointType)
@@ -267,19 +295,14 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
-
- //b3Printf("Fixed joint (btMultiBody)\n");
- btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
- rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin());
+ parentRotToThis, offsetInA.getOrigin(),-offsetInB.getOrigin());
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
-
-
} else
{
//b3Printf("Fixed joint\n");
- btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
+ btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
if (enableConstraints)
world1->addConstraint(dof6,true);
@@ -291,18 +314,23 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{
if (createMultiBody)
{
-
-
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
- offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
+ parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
+ cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
+ cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
-
+ if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) {
+ //std::string name = u2b.getLinkName(urdfLinkIndex);
+ //printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
+ btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
+ world1->addMultiBodyConstraint(con);
+ }
} else
{
- btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
+ btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
if (enableConstraints)
world1->addConstraint(dof6,true);
@@ -314,21 +342,25 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{
if (createMultiBody)
{
-
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
- offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
+ parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
-
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
- btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit);
- world1->addMultiBodyConstraint(con);
+ if (jointLowerLimit <= jointUpperLimit)
+ {
+ //std::string name = u2b.getLinkName(urdfLinkIndex);
+ //printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit);
+
+ btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
+ world1->addMultiBodyConstraint(con);
+ }
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
} else
{
- btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
+ btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
if (enableConstraints)
@@ -364,8 +396,9 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
col->setWorldTransform(tr);
-
- bool isDynamic = true;
+
+ //base and fixed? -> static, otherwise flag as dynamic
+ bool isDynamic = (mbLinkIndex<0 && cache.m_bulletMultiBody->hasFixedBase())? false : true;
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
@@ -374,10 +407,28 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
btVector4 color = selectColor2();//(0.0,0.0,0.5);
u2b.getLinkColor(urdfLinkIndex,color);
creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
-
- btScalar friction = 0.5f;
-
- col->setFriction(friction);
+
+ u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId());
+
+ URDFLinkContactInfo contactInfo;
+ u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
+
+ if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0)
+ {
+ col->setFriction(contactInfo.m_lateralFriction);
+ }
+ if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION)!=0)
+ {
+ col->setRollingFriction(contactInfo.m_rollingFriction);
+ }
+ if ((contactInfo.m_flags & URDF_CONTACT_HAS_SPINNING_FRICTION)!=0)
+ {
+ col->setSpinningFriction(contactInfo.m_spinningFriction);
+ }
+ if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING)!=0)
+ {
+ col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness,contactInfo.m_contactDamping);
+ }
if (mbLinkIndex>=0) //???? double-check +/- 1
{
@@ -387,6 +438,9 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
cache.m_bulletMultiBody->setBaseCollider(col);
}
}
+ } else
+ {
+ //u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape);
}
}
@@ -400,18 +454,22 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
{
int urdfChildLinkIndex = urdfChildIndices[i];
- ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix);
+ ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags);
}
}
-void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
+void ConvertURDF2Bullet(
+ const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
+ const btTransform& rootTransformInWorldSpace,
+ btMultiBodyDynamicsWorld* world1,
+ bool createMultiBody, const char* pathPrefix, int flags)
{
URDF2BulletCachedData cache;
InitURDF2BulletCache(u2b,cache);
int urdfLinkIndex = u2b.getRootLinkIndex();
- ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
+ ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags);
if (world1 && cache.m_bulletMultiBody)
{
@@ -419,7 +477,9 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
- mb->setBaseWorldTransform(rootTransformInWorldSpace);
+ btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
+
+ mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
mb->forwardKinematics(scratch_q,scratch_m);
diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h
index 0d93068..dcf05c6 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h
@@ -14,13 +14,19 @@ class MultiBodyCreationInterface;
-
-void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
- MultiBodyCreationInterface& creationCallback,
- const btTransform& rootTransformInWorldSpace,
+enum ConvertURDFFlags {
+ CUF_USE_SDF = 1,
+ // Use inertia values in URDF instead of recomputing them from collision shape.
+ CUF_USE_URDF_INERTIA = 2
+};
+
+void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
+ MultiBodyCreationInterface& creationCallback,
+ const btTransform& rootTransformInWorldSpace,
btMultiBodyDynamicsWorld* world,
- bool createMultiBody,
- const char* pathPrefix);
+ bool createMultiBody,
+ const char* pathPrefix,
+ int flags = 0);
#endif //_URDF2BULLET_H
diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
index 8fce7b3..ce688ec 100644
--- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
+++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
@@ -6,6 +6,7 @@
#include "LinearMath/btTransform.h"
#include "URDFJointTypes.h"
+
class URDFImporterInterface
{
@@ -17,6 +18,8 @@ public:
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)=0;
+ virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false;}
+
virtual const char* getPathPrefix()=0;
///return >=0 for the root link index, -1 if there is no root link
@@ -26,6 +29,9 @@ public:
virtual std::string getLinkName(int linkIndex) const =0;
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;}
+
+ ///this API will likely change, don't override it!
+ virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;}
virtual std::string getJointName(int linkIndex) const = 0;
@@ -35,12 +41,18 @@ public:
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const =0;
- virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
+ virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0;
+
+ virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0;
///quick hack: need to rethink the API/dependencies of this
- virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const = 0;
-
- virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
+ virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;}
+
+ virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { }
+ virtual void setBodyUniqueId(int bodyId) {}
+ virtual int getBodyUniqueId() const { return 0;}
+
+ virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
};
#endif //URDF_IMPORTER_INTERFACE_H
diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h
index 0c50d26..88912cb 100644
--- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h
+++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h
@@ -11,6 +11,46 @@ enum UrdfJointTypes
URDFPlanarJoint,
URDFFixedJoint,
};
+#include "LinearMath/btScalar.h"
+
+enum URDF_LinkContactFlags
+{
+ URDF_CONTACT_HAS_LATERAL_FRICTION=1,
+ URDF_CONTACT_HAS_INERTIA_SCALING=2,
+ URDF_CONTACT_HAS_CONTACT_CFM=4,
+ URDF_CONTACT_HAS_CONTACT_ERP=8,
+ URDF_CONTACT_HAS_STIFFNESS_DAMPING=16,
+ URDF_CONTACT_HAS_ROLLING_FRICTION=32,
+ URDF_CONTACT_HAS_SPINNING_FRICTION=64,
+
+};
+
+struct URDFLinkContactInfo
+{
+ btScalar m_lateralFriction;
+ btScalar m_rollingFriction;
+ btScalar m_spinningFriction;
+ btScalar m_inertiaScaling;
+ btScalar m_contactCfm;
+ btScalar m_contactErp;
+ btScalar m_contactStiffness;
+ btScalar m_contactDamping;
+
+ int m_flags;
+
+ URDFLinkContactInfo()
+ :m_lateralFriction(0.5),
+ m_rollingFriction(0),
+ m_spinningFriction(0),
+ m_inertiaScaling(1),
+ m_contactCfm(0),
+ m_contactErp(0),
+ m_contactStiffness(1e4),
+ m_contactDamping(1)
+ {
+ m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
+ }
+};
#endif //URDF_JOINT_TYPES_H
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index a40d864..817cb0c 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -1,15 +1,59 @@
#include "UrdfParser.h"
-#include "tinyxml/tinyxml.h"
+#include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
#include "urdfStringSplit.h"
#include "urdfLexicalCast.h"
UrdfParser::UrdfParser()
+:m_parseSDF(false),
+m_activeSdfModel(-1)
{
}
+
+
UrdfParser::~UrdfParser()
{
- //todo(erwincoumans) delete memory
+ cleanModel(&m_urdf2Model);
+
+ for (int i=0;i<m_tmpModels.size();i++)
+ {
+ cleanModel(m_tmpModels[i]);
+ }
+ m_sdfModels.clear();
+ m_tmpModels.clear();
+}
+
+void UrdfParser::cleanModel(UrdfModel* model)
+{
+ for (int i=0;i<model->m_materials.size();i++)
+ {
+ UrdfMaterial** matPtr = model->m_materials.getAtIndex(i);
+ if (matPtr)
+ {
+ UrdfMaterial* mat = *matPtr;
+ delete mat;
+ }
+ }
+
+ for (int i=0;i<model->m_links.size();i++)
+ {
+ UrdfLink** linkPtr = model->m_links.getAtIndex(i);
+ if (linkPtr)
+ {
+ UrdfLink* link = *linkPtr;
+ delete link;
+ }
+ }
+
+ for (int i=0;i<model->m_joints.size();i++)
+ {
+ UrdfJoint** jointPtr = model->m_joints.getAtIndex(i);
+ if (jointPtr)
+ {
+ UrdfJoint* joint = *jointPtr;
+ delete joint;
+ }
+ }
}
static bool parseVector4(btVector4& vec4, const std::string& vector_str)
@@ -17,7 +61,9 @@ static bool parseVector4(btVector4& vec4, const std::string& vector_str)
vec4.setZero();
btArray<std::string> pieces;
btArray<float> rgba;
- urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" "));
+ btAlignedObjectArray<std::string> strArray;
+ urdfIsAnyOf(" ", strArray);
+ urdfStringSplit(pieces, vector_str, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
@@ -33,12 +79,14 @@ static bool parseVector4(btVector4& vec4, const std::string& vector_str)
return true;
}
-static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger)
+static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger, bool lastThree = false)
{
vec3.setZero();
btArray<std::string> pieces;
btArray<float> rgba;
- urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" "));
+ btAlignedObjectArray<std::string> strArray;
+ urdfIsAnyOf(" ", strArray);
+ urdfStringSplit(pieces, vector_str, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
@@ -46,16 +94,22 @@ static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLo
rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
- if (rgba.size() != 3)
+ if (rgba.size() < 3)
{
logger->reportWarning("Couldn't parse vector3");
return false;
}
- vec3.setValue(rgba[0],rgba[1],rgba[2]);
+ if (lastThree) {
+ vec3.setValue(rgba[rgba.size()-3], rgba[rgba.size()-2], rgba[rgba.size()-1]);
+ }
+ else
+ {
+ vec3.setValue(rgba[0],rgba[1],rgba[2]);
+
+ }
return true;
}
-
bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, ErrorLogger* logger)
{
@@ -99,51 +153,91 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err
}
-bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger)
+bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false)
{
- tr.setIdentity();
-
- {
- const char* xyz_str = xml->Attribute("xyz");
- if (xyz_str)
- {
- parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
- }
- }
-
- {
- const char* rpy_str = xml->Attribute("rpy");
- if (rpy_str != NULL)
- {
- btVector3 rpy;
- if (parseVector3(rpy,std::string(rpy_str),logger))
- {
- double phi, the, psi;
- double roll = rpy[0];
- double pitch = rpy[1];
- double yaw = rpy[2];
-
- phi = roll / 2.0;
- the = pitch / 2.0;
- psi = yaw / 2.0;
-
- btQuaternion orn(
- sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
- cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
- cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
- cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
-
- orn.normalize();
- tr.setRotation(orn);
- }
- }
- }
- return true;
+ tr.setIdentity();
+
+ if (parseSDF)
+ {
+ parseVector3(tr.getOrigin(),std::string(xml->GetText()),logger);
+ }
+ else
+ {
+ const char* xyz_str = xml->Attribute("xyz");
+ if (xyz_str)
+ {
+ parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
+ }
+ }
+
+ if (parseSDF)
+ {
+ btVector3 rpy;
+ if (parseVector3(rpy,std::string(xml->GetText()),logger,true))
+ {
+ double phi, the, psi;
+ double roll = rpy[0];
+ double pitch = rpy[1];
+ double yaw = rpy[2];
+
+ phi = roll / 2.0;
+ the = pitch / 2.0;
+ psi = yaw / 2.0;
+
+ btQuaternion orn(
+ sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
+ cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
+ cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
+ cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
+
+ orn.normalize();
+ tr.setRotation(orn);
+ }
+ }
+ else
+ {
+ const char* rpy_str = xml->Attribute("rpy");
+ if (rpy_str != NULL)
+ {
+ btVector3 rpy;
+ if (parseVector3(rpy,std::string(rpy_str),logger))
+ {
+ double phi, the, psi;
+ double roll = rpy[0];
+ double pitch = rpy[1];
+ double yaw = rpy[2];
+
+ phi = roll / 2.0;
+ the = pitch / 2.0;
+ psi = yaw / 2.0;
+
+ btQuaternion orn(
+ sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
+ cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
+ cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
+ cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
+
+ orn.normalize();
+ tr.setRotation(orn);
+ }
+ }
+ }
+ return true;
}
+
bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorLogger* logger)
{
inertia.m_linkLocalFrame.setIdentity();
inertia.m_mass = 0.f;
+ if(m_parseSDF)
+ {
+ TiXmlElement* pose = config->FirstChildElement("pose");
+ if (pose)
+ {
+ parseTransform(inertia.m_linkLocalFrame, pose,logger,m_parseSDF);
+ }
+ }
+
// Origin
@@ -162,14 +256,19 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
logger->reportError("Inertial element must have a mass element");
return false;
}
- if (!mass_xml->Attribute("value"))
- {
- logger->reportError("Inertial: mass element must have value attribute");
- return false;
- }
+ if (m_parseSDF)
+ {
+ inertia.m_mass = urdfLexicalCast<double>(mass_xml->GetText());
+ } else
+ {
+ if (!mass_xml->Attribute("value"))
+ {
+ logger->reportError("Inertial: mass element must have value attribute");
+ return false;
+ }
- inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
-
+ inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
+ }
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
@@ -177,21 +276,45 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
logger->reportError("Inertial element must have inertia element");
return false;
}
- if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
- inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
- inertia_xml->Attribute("izz")))
- {
- logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
- return false;
- }
- inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
- inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
- inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
- inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
- inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
- inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
-
+ if (m_parseSDF)
+ {
+ TiXmlElement* ixx = inertia_xml->FirstChildElement("ixx");
+ TiXmlElement* ixy = inertia_xml->FirstChildElement("ixy");
+ TiXmlElement* ixz = inertia_xml->FirstChildElement("ixz");
+ TiXmlElement* iyy = inertia_xml->FirstChildElement("iyy");
+ TiXmlElement* iyz = inertia_xml->FirstChildElement("iyz");
+ TiXmlElement* izz = inertia_xml->FirstChildElement("izz");
+ if (ixx && ixy && ixz && iyy && iyz && izz)
+ {
+ inertia.m_ixx = urdfLexicalCast<double>(ixx->GetText());
+ inertia.m_ixy = urdfLexicalCast<double>(ixy->GetText());
+ inertia.m_ixz = urdfLexicalCast<double>(ixz->GetText());
+ inertia.m_iyy = urdfLexicalCast<double>(iyy->GetText());
+ inertia.m_iyz = urdfLexicalCast<double>(iyz->GetText());
+ inertia.m_izz = urdfLexicalCast<double>(izz->GetText());
+ } else
+ {
+ logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz child elements");
+ return false;
+ }
+ } else
+ {
+ if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
+ inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
+ inertia_xml->Attribute("izz")))
+ {
+ logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
+ return false;
+ }
+ inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
+ inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
+ inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
+ inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
+ inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
+ inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
+ }
return true;
+
}
bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger)
@@ -221,13 +344,27 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
else if (type_name == "box")
{
geom.m_type = URDF_GEOM_BOX;
- if (!shape->Attribute("size"))
- {
- logger->reportError("box requires a size attribute");
- } else
- {
- parseVector3(geom.m_boxSize,shape->Attribute("size"),logger);
- }
+ if (m_parseSDF)
+ {
+ TiXmlElement* size = shape->FirstChildElement("size");
+ if (0==size)
+ {
+ logger->reportError("box requires a size child element");
+ return false;
+ }
+ parseVector3(geom.m_boxSize,size->GetText(),logger);
+ }
+ else
+ {
+ if (!shape->Attribute("size"))
+ {
+ logger->reportError("box requires a size attribute");
+ return false;
+ } else
+ {
+ parseVector3(geom.m_boxSize,shape->Attribute("size"),logger);
+ }
+ }
}
else if (type_name == "cylinder")
{
@@ -246,27 +383,73 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
else if (type_name == "mesh")
{
geom.m_type = URDF_GEOM_MESH;
- if (!shape->Attribute("filename")) {
- logger->reportError("Mesh must contain a filename attribute");
- return false;
- }
-
- geom.m_meshFileName = shape->Attribute("filename");
-
- if (shape->Attribute("scale"))
- {
- parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger);
- } else
- {
+ if (m_parseSDF)
+ {
+ TiXmlElement* scale = shape->FirstChildElement("scale");
+ if (0==scale)
+ {
+ geom.m_meshScale.setValue(1,1,1);
+ }
+ else
+ {
+ parseVector3(geom.m_meshScale,scale->GetText(),logger);
+ }
+
+ TiXmlElement* filename = shape->FirstChildElement("uri");
+ geom.m_meshFileName = filename->GetText();
+ }
+ else
+ {
+ if (!shape->Attribute("filename")) {
+ logger->reportError("Mesh must contain a filename attribute");
+ return false;
+ }
+
+ geom.m_meshFileName = shape->Attribute("filename");
geom.m_meshScale.setValue(1,1,1);
- }
+ if (shape->Attribute("scale"))
+ {
+ if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger))
+ {
+ logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n");
+ std::string scalar_str = shape->Attribute("scale");
+ double scaleFactor = urdfLexicalCast<double>(scalar_str.c_str());
+ if (scaleFactor)
+ {
+ geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor);
+ }
+ }
+ } else
+ {
+ }
+ }
}
else
{
- logger->reportError("Unknown geometry type:");
- logger->reportError(type_name.c_str());
- return false;
+ if (this->m_parseSDF)
+ {
+ if (type_name == "plane")
+ {
+ geom.m_type = URDF_GEOM_PLANE;
+
+ TiXmlElement *n = shape->FirstChildElement("normal");
+ TiXmlElement *s = shape->FirstChildElement("size");
+
+ if ((0==n)||(0==s))
+ {
+ logger->reportError("Plane shape must have both normal and size attributes");
+ return false;
+ }
+
+ parseVector3(geom.m_planeNormal,n->GetText(),logger);
+ }
+ } else
+ {
+ logger->reportError("Unknown geometry type:");
+ logger->reportError(type_name.c_str());
+ return false;
+ }
}
return true;
@@ -278,6 +461,15 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
collision.m_linkLocalFrame.setIdentity();
+ if(m_parseSDF)
+ {
+ TiXmlElement* pose = config->FirstChildElement("pose");
+ if (pose)
+ {
+ parseTransform(collision.m_linkLocalFrame, pose,logger,m_parseSDF);
+ }
+ }
+
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
@@ -297,14 +489,25 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
if (name_char)
collision.m_name = name_char;
+ const char *concave_char = config->Attribute("concave");
+ if (concave_char)
+ collision.m_flags |= URDF_FORCE_CONCAVE_TRIMESH;
return true;
}
-bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
+bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
{
visual.m_linkLocalFrame.setIdentity();
-
+ if(m_parseSDF)
+ {
+ TiXmlElement* pose = config->FirstChildElement("pose");
+ if (pose)
+ {
+ parseTransform(visual.m_linkLocalFrame, pose,logger,m_parseSDF);
+ }
+ }
+
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
@@ -328,35 +531,56 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg
// Material
TiXmlElement *mat = config->FirstChildElement("material");
- if (mat)
+//todo(erwincoumans) skip materials in SDF for now (due to complexity)
+ if (mat)
{
- // get material name
- if (!mat->Attribute("name"))
- {
- logger->reportError("Visual material must contain a name attribute");
- return false;
- }
- visual.m_materialName = mat->Attribute("name");
-
- // try to parse material element in place
-
- TiXmlElement *t = mat->FirstChildElement("texture");
- TiXmlElement *c = mat->FirstChildElement("color");
- if (t||c)
- {
- if (parseMaterial(visual.m_localMaterial, mat,logger))
- {
- UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
- m_model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
- visual.m_hasLocalMaterial = true;
- }
- }
+ if (m_parseSDF)
+ {
+ UrdfMaterial* matPtr = new UrdfMaterial;
+ matPtr->m_name = "mat";
+ if (name_char)
+ matPtr->m_name = name_char;
+ TiXmlElement *diffuse = mat->FirstChildElement("diffuse");
+ if (diffuse) {
+ std::string diffuseText = diffuse->GetText();
+ btVector4 rgba(1,0,0,1);
+ parseVector4(rgba,diffuseText);
+ matPtr->m_rgbaColor = rgba;
+ model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
+ visual.m_materialName = matPtr->m_name;
+ visual.m_hasLocalMaterial = true;
+ }
+ }
+ else
+ {
+ // get material name
+ if (!mat->Attribute("name"))
+ {
+ logger->reportError("Visual material must contain a name attribute");
+ return false;
+ }
+ visual.m_materialName = mat->Attribute("name");
+
+ // try to parse material element in place
+
+ TiXmlElement *t = mat->FirstChildElement("texture");
+ TiXmlElement *c = mat->FirstChildElement("color");
+ if (t||c)
+ {
+ if (parseMaterial(visual.m_localMaterial, mat,logger))
+ {
+ UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
+ model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
+ visual.m_hasLocalMaterial = true;
+ }
+ }
+ }
}
return true;
}
-bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
+bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
{
const char* linkName = config->Attribute("name");
if (!linkName)
@@ -365,8 +589,160 @@ bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* lo
return false;
}
link.m_name = linkName;
+
+ if (m_parseSDF) {
+ TiXmlElement* pose = config->FirstChildElement("pose");
+ if (0==pose)
+ {
+ link.m_linkTransformInWorld.setIdentity();
+ }
+ else
+ {
+ parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF);
+ }
+ }
+
+ {
+ //optional 'contact' parameters
+ TiXmlElement* ci = config->FirstChildElement("contact");
+ if (ci)
+ {
+
+ TiXmlElement *damping_xml = ci->FirstChildElement("inertia_scaling");
+ if (damping_xml)
+ {
+ if (m_parseSDF)
+ {
+ link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->GetText());
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
+ } else
+ {
+ if (!damping_xml->Attribute("value"))
+ {
+ logger->reportError("Link/contact: damping element must have value attribute");
+ return false;
+ }
+
+ link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->Attribute("value"));
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
+
+ }
+ }
+ {
+ TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
+ if (friction_xml)
+ {
+ if (m_parseSDF)
+ {
+ link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->GetText());
+ } else
+ {
+ if (!friction_xml->Attribute("value"))
+ {
+ logger->reportError("Link/contact: lateral_friction element must have value attribute");
+ return false;
+ }
+
+ link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
+ }
+ }
+ }
+
+ {
+ TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction");
+ if (rolling_xml)
+ {
+ if (m_parseSDF)
+ {
+ link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->GetText());
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
+ } else
+ {
+ if (!rolling_xml->Attribute("value"))
+ {
+ logger->reportError("Link/contact: rolling friction element must have value attribute");
+ return false;
+ }
+
+ link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->Attribute("value"));
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
+
+ }
+ }
+ }
+ {
+ TiXmlElement *spinning_xml = ci->FirstChildElement("spinning_friction");
+ if (spinning_xml)
+ {
+ if (m_parseSDF)
+ {
+ link.m_contactInfo.m_spinningFriction = urdfLexicalCast<double>(spinning_xml->GetText());
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_SPINNING_FRICTION;
+ } else
+ {
+ if (!spinning_xml->Attribute("value"))
+ {
+ logger->reportError("Link/contact: spinning friction element must have value attribute");
+ return false;
+ }
+
+ link.m_contactInfo.m_spinningFriction = urdfLexicalCast<double>(spinning_xml->Attribute("value"));
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_SPINNING_FRICTION;
+
+ }
+ }
+ }
+ {
+
+ TiXmlElement *stiffness_xml = ci->FirstChildElement("stiffness");
+ if (stiffness_xml)
+ {
+ if (m_parseSDF)
+ {
+ link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->GetText());
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
+ } else
+ {
+ if (!stiffness_xml->Attribute("value"))
+ {
+ logger->reportError("Link/contact: stiffness element must have value attribute");
+ return false;
+ }
+
+ link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->Attribute("value"));
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
+
+ }
+ }
+ }
+ {
+
+ TiXmlElement *damping_xml = ci->FirstChildElement("damping");
+ if (damping_xml)
+ {
+ if (m_parseSDF)
+ {
+ link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->GetText());
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
+ } else
+ {
+ if (!damping_xml->Attribute("value"))
+ {
+ logger->reportError("Link/contact: damping element must have value attribute");
+ return false;
+ }
+
+ link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->Attribute("value"));
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
+
+ }
+ }
+ }
+ }
+ }
+
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
@@ -379,22 +755,33 @@ bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* lo
}
} else
{
- logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
- link.m_inertia.m_mass = 1.f;
- link.m_inertia.m_linkLocalFrame.setIdentity();
- link.m_inertia.m_ixx = 1.f;
- link.m_inertia.m_iyy = 1.f;
- link.m_inertia.m_izz= 1.f;
-
- logger->reportWarning(link.m_name.c_str());
+
+ if ((strlen(linkName)==5) && (strncmp(linkName, "world", 5))==0)
+ {
+ link.m_inertia.m_mass = 0.f;
+ link.m_inertia.m_linkLocalFrame.setIdentity();
+ link.m_inertia.m_ixx = 0.f;
+ link.m_inertia.m_iyy = 0.f;
+ link.m_inertia.m_izz= 0.f;
+ } else
+ {
+
+ logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
+ link.m_inertia.m_mass = 1.f;
+ link.m_inertia.m_linkLocalFrame.setIdentity();
+ link.m_inertia.m_ixx = 1.f;
+ link.m_inertia.m_iyy = 1.f;
+ link.m_inertia.m_izz= 1.f;
+ logger->reportWarning(link.m_name.c_str());
+ }
}
-
+
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
UrdfVisual visual;
- if (parseVisual(visual, vis_xml,logger))
+ if (parseVisual(model, visual, vis_xml,logger))
{
link.m_visualArray.push_back(visual);
}
@@ -429,39 +816,115 @@ bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* lo
bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger)
{
joint.m_lowerLimit = 0.f;
- joint.m_upperLimit = 0.f;
+ joint.m_upperLimit = -1.f;
joint.m_effortLimit = 0.f;
joint.m_velocityLimit = 0.f;
+ joint.m_jointDamping = 0.f;
+ joint.m_jointFriction = 0.f;
- const char* lower_str = config->Attribute("lower");
- if (lower_str)
- {
- joint.m_lowerLimit = urdfLexicalCast<double>(lower_str);
- }
-
- const char* upper_str = config->Attribute("upper");
- if (upper_str)
- {
- joint.m_upperLimit = urdfLexicalCast<double>(upper_str);
- }
-
-
- // Get joint effort limit
- const char* effort_str = config->Attribute("effort");
- if (effort_str)
- {
- joint.m_effortLimit = urdfLexicalCast<double>(effort_str);
- }
-
- // Get joint velocity limit
- const char* velocity_str = config->Attribute("velocity");
- if (velocity_str)
- {
- joint.m_velocityLimit = urdfLexicalCast<double>(velocity_str);
- }
+ if (m_parseSDF)
+ {
+ TiXmlElement *lower_xml = config->FirstChildElement("lower");
+ if (lower_xml) {
+ joint.m_lowerLimit = urdfLexicalCast<double>(lower_xml->GetText());
+ }
+
+ TiXmlElement *upper_xml = config->FirstChildElement("upper");
+ if (upper_xml) {
+ joint.m_upperLimit = urdfLexicalCast<double>(upper_xml->GetText());
+ }
+
+ TiXmlElement *effort_xml = config->FirstChildElement("effort");
+ if (effort_xml) {
+ joint.m_effortLimit = urdfLexicalCast<double>(effort_xml->GetText());
+ }
+
+ TiXmlElement *velocity_xml = config->FirstChildElement("velocity");
+ if (velocity_xml) {
+ joint.m_velocityLimit = urdfLexicalCast<double>(velocity_xml->GetText());
+ }
+ }
+ else
+ {
+ const char* lower_str = config->Attribute("lower");
+ if (lower_str)
+ {
+ joint.m_lowerLimit = urdfLexicalCast<double>(lower_str);
+ }
+
+ const char* upper_str = config->Attribute("upper");
+ if (upper_str)
+ {
+ joint.m_upperLimit = urdfLexicalCast<double>(upper_str);
+ }
+
+
+ // Get joint effort limit
+ const char* effort_str = config->Attribute("effort");
+ if (effort_str)
+ {
+ joint.m_effortLimit = urdfLexicalCast<double>(effort_str);
+ }
+
+ // Get joint velocity limit
+ const char* velocity_str = config->Attribute("velocity");
+ if (velocity_str)
+ {
+ joint.m_velocityLimit = urdfLexicalCast<double>(velocity_str);
+ }
+ }
return true;
}
+
+bool UrdfParser::parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger)
+{
+ joint.m_jointDamping = 0;
+ joint.m_jointFriction = 0;
+
+ if (m_parseSDF) {
+ TiXmlElement *damping_xml = config->FirstChildElement("damping");
+ if (damping_xml) {
+ joint.m_jointDamping = urdfLexicalCast<double>(damping_xml->GetText());
+ }
+
+ TiXmlElement *friction_xml = config->FirstChildElement("friction");
+ if (friction_xml) {
+ joint.m_jointFriction = urdfLexicalCast<double>(friction_xml->GetText());
+ }
+
+ if (damping_xml == NULL && friction_xml == NULL)
+ {
+ logger->reportError("joint dynamics element specified with no damping and no friction");
+ return false;
+ }
+ }
+ else
+ {
+ // Get joint damping
+ const char* damping_str = config->Attribute("damping");
+ if (damping_str)
+ {
+ joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
+ }
+
+ // Get joint friction
+ const char* friction_str = config->Attribute("friction");
+ if (friction_str)
+ {
+ joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
+ }
+
+ if (damping_str == NULL && friction_str == NULL)
+ {
+ logger->reportError("joint dynamics element specified with no damping and no friction");
+ return false;
+ }
+ }
+
+ return true;
+}
+
bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* logger)
{
@@ -492,34 +955,48 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
TiXmlElement *parent_xml = config->FirstChildElement("parent");
if (parent_xml)
{
- const char *pname = parent_xml->Attribute("link");
- if (!pname)
- {
- logger->reportError("no parent link name specified for Joint link. this might be the root?");
- logger->reportError(joint.m_name.c_str());
- return false;
- }
- else
- {
- joint.m_parentLinkName = std::string(pname);
- }
+ if (m_parseSDF)
+ {
+ joint.m_parentLinkName = std::string(parent_xml->GetText());
+ }
+ else
+ {
+ const char *pname = parent_xml->Attribute("link");
+ if (!pname)
+ {
+ logger->reportError("no parent link name specified for Joint link. this might be the root?");
+ logger->reportError(joint.m_name.c_str());
+ return false;
+ }
+ else
+ {
+ joint.m_parentLinkName = std::string(pname);
+ }
+ }
}
-
+
// Get Child Link
TiXmlElement *child_xml = config->FirstChildElement("child");
if (child_xml)
{
- const char *pname = child_xml->Attribute("link");
- if (!pname)
- {
- logger->reportError("no child link name specified for Joint link [%s].");
- logger->reportError(joint.m_name.c_str());
- return false;
- }
- else
- {
- joint.m_childLinkName = std::string(pname);
- }
+ if (m_parseSDF)
+ {
+ joint.m_childLinkName = std::string(child_xml->GetText());
+ }
+ else
+ {
+ const char *pname = child_xml->Attribute("link");
+ if (!pname)
+ {
+ logger->reportError("no child link name specified for Joint link [%s].");
+ logger->reportError(joint.m_name.c_str());
+ return false;
+ }
+ else
+ {
+ joint.m_childLinkName = std::string(pname);
+ }
+ }
}
// Get Joint type
@@ -553,98 +1030,164 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
return false;
}
- // Get Joint Axis
- if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
- {
- // axis
- TiXmlElement *axis_xml = config->FirstChildElement("axis");
- if (!axis_xml){
- logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
- logger->reportWarning(joint.m_name.c_str());
- joint.m_localJointAxis.setValue(1,0,0);
- }
- else{
- if (axis_xml->Attribute("xyz"))
- {
- if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger))
- {
- logger->reportError("Malformed axis element:");
- logger->reportError(joint.m_name.c_str());
- logger->reportError(" for joint:");
- logger->reportError(axis_xml->Attribute("xyz"));
- return false;
- }
- }
- }
- }
-
- // Get limit
- TiXmlElement *limit_xml = config->FirstChildElement("limit");
- if (limit_xml)
- {
- if (!parseJointLimits(joint, limit_xml,logger))
- {
- logger->reportError("Could not parse limit element for joint:");
- logger->reportError(joint.m_name.c_str());
- return false;
- }
- }
- else if (joint.m_type == URDFRevoluteJoint)
- {
- logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
- logger->reportError(joint.m_name.c_str());
- return false;
- }
- else if (joint.m_type == URDFPrismaticJoint)
- {
- logger->reportError("Joint is of type PRISMATIC without limits");
- logger->reportError( joint.m_name.c_str());
- return false;
- }
-
- joint.m_jointDamping = 0;
- joint.m_jointFriction = 0;
-
- // Get Dynamics
- TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
- if (prop_xml)
- {
-
- // Get joint damping
- const char* damping_str = prop_xml->Attribute("damping");
- if (damping_str)
- {
- joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
- }
-
- // Get joint friction
- const char* friction_str = prop_xml->Attribute("friction");
- if (friction_str)
- {
- joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
- }
-
- if (damping_str == NULL && friction_str == NULL)
- {
- logger->reportError("joint dynamics element specified with no damping and no friction");
- return false;
- }
- }
+ if (m_parseSDF)
+ {
+ if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
+ {
+ // axis
+ TiXmlElement *axis_xml = config->FirstChildElement("axis");
+ if (!axis_xml){
+ logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
+ logger->reportWarning(joint.m_name.c_str());
+ joint.m_localJointAxis.setValue(1,0,0);
+ }
+ else{
+ TiXmlElement *xyz_xml = axis_xml->FirstChildElement("xyz");
+ if (xyz_xml) {
+ if (!parseVector3(joint.m_localJointAxis,std::string(xyz_xml->GetText()),logger))
+ {
+ logger->reportError("Malformed axis element:");
+ logger->reportError(joint.m_name.c_str());
+ logger->reportError(" for joint:");
+ logger->reportError(xyz_xml->GetText());
+ return false;
+ }
+ }
+
+ TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit");
+ if (limit_xml)
+ {
+ if (joint.m_type != URDFContinuousJoint)
+ {
+ if (!parseJointLimits(joint, limit_xml,logger))
+ {
+ logger->reportError("Could not parse limit element for joint:");
+ logger->reportError(joint.m_name.c_str());
+ return false;
+ }
+ }
+ }
+ else if (joint.m_type == URDFRevoluteJoint)
+ {
+ logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
+ logger->reportError(joint.m_name.c_str());
+ return false;
+ }
+ else if (joint.m_type == URDFPrismaticJoint)
+ {
+ logger->reportError("Joint is of type PRISMATIC without limits");
+ logger->reportError( joint.m_name.c_str());
+ return false;
+ }
+
+ TiXmlElement *prop_xml = axis_xml->FirstChildElement("dynamics");
+ if (prop_xml)
+ {
+ if (!parseJointDynamics(joint, prop_xml,logger))
+ {
+ logger->reportError("Could not parse dynamics element for joint:");
+ logger->reportError(joint.m_name.c_str());
+ return false;
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ // Get Joint Axis
+ if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
+ {
+ // axis
+ TiXmlElement *axis_xml = config->FirstChildElement("axis");
+ if (!axis_xml){
+ logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
+ logger->reportWarning(joint.m_name.c_str());
+ joint.m_localJointAxis.setValue(1,0,0);
+ }
+ else{
+ if (axis_xml->Attribute("xyz"))
+ {
+ if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger))
+ {
+ logger->reportError("Malformed axis element:");
+ logger->reportError(joint.m_name.c_str());
+ logger->reportError(" for joint:");
+ logger->reportError(axis_xml->Attribute("xyz"));
+ return false;
+ }
+ }
+ }
+ }
+
+ // Get limit
+ TiXmlElement *limit_xml = config->FirstChildElement("limit");
+ if (limit_xml)
+ {
+ if (!parseJointLimits(joint, limit_xml,logger))
+ {
+ logger->reportError("Could not parse limit element for joint:");
+ logger->reportError(joint.m_name.c_str());
+ return false;
+ }
+ }
+ else if (joint.m_type == URDFRevoluteJoint)
+ {
+ logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
+ logger->reportError(joint.m_name.c_str());
+ return false;
+ }
+ else if (joint.m_type == URDFPrismaticJoint)
+ {
+ logger->reportError("Joint is of type PRISMATIC without limits");
+ logger->reportError( joint.m_name.c_str());
+ return false;
+ }
+
+ joint.m_jointDamping = 0;
+ joint.m_jointFriction = 0;
+
+ // Get Dynamics
+ TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
+ if (prop_xml)
+ {
+
+ // Get joint damping
+ const char* damping_str = prop_xml->Attribute("damping");
+ if (damping_str)
+ {
+ joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
+ }
+
+ // Get joint friction
+ const char* friction_str = prop_xml->Attribute("friction");
+ if (friction_str)
+ {
+ joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
+ }
+
+ if (damping_str == NULL && friction_str == NULL)
+ {
+ logger->reportError("joint dynamics element specified with no damping and no friction");
+ return false;
+ }
+ }
+ }
return true;
}
-bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
+bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
{
// every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations
btHashMap<btHashString,btHashString> parentLinkTree;
// loop through all joints, for every link, assign children links and children joints
- for (int i=0;i<m_model.m_joints.size();i++)
+ for (int i=0;i<model.m_joints.size();i++)
{
- UrdfJoint** jointPtr = m_model.m_joints.getAtIndex(i);
+ UrdfJoint** jointPtr = model.m_joints.getAtIndex(i);
if (jointPtr)
{
UrdfJoint* joint = *jointPtr;
@@ -657,7 +1200,7 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
return false;
}
- UrdfLink** childLinkPtr = m_model.m_links.find(joint->m_childLinkName.c_str());
+ UrdfLink** childLinkPtr = model.m_links.find(joint->m_childLinkName.c_str());
if (!childLinkPtr)
{
logger->reportError("Cannot find child link for joint ");
@@ -667,7 +1210,7 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
}
UrdfLink* childLink = *childLinkPtr;
- UrdfLink** parentLinkPtr = m_model.m_links.find(joint->m_parentLinkName.c_str());
+ UrdfLink** parentLinkPtr = model.m_links.find(joint->m_parentLinkName.c_str());
if (!parentLinkPtr)
{
logger->reportError("Cannot find parent link for a joint");
@@ -682,14 +1225,13 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
parentLink->m_childJoints.push_back(joint);
parentLink->m_childLinks.push_back(childLink);
parentLinkTree.insert(childLink->m_name.c_str(),parentLink->m_name.c_str());
-
}
}
//search for children that have no parent, those are 'root'
- for (int i=0;i<m_model.m_links.size();i++)
+ for (int i=0;i<model.m_links.size();i++)
{
- UrdfLink** linkPtr = m_model.m_links.getAtIndex(i);
+ UrdfLink** linkPtr = model.m_links.getAtIndex(i);
btAssert(linkPtr);
if (linkPtr)
{
@@ -698,18 +1240,18 @@ bool UrdfParser::initTreeAndRoot(ErrorLogger* logger)
if (!link->m_parentLink)
{
- m_model.m_rootLinks.push_back(link);
+ model.m_rootLinks.push_back(link);
}
}
}
- if (m_model.m_rootLinks.size()>1)
+ if (model.m_rootLinks.size()>1)
{
logger->reportWarning("URDF file with multiple root links found");
}
- if (m_model.m_rootLinks.size()==0)
+ if (model.m_rootLinks.size()==0)
{
logger->reportError("URDF without root link found");
return false;
@@ -744,7 +1286,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
logger->reportError("Expected a name for robot");
return false;
}
- m_model.m_name = name;
+ m_urdf2Model.m_name = name;
@@ -756,28 +1298,29 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
parseMaterial(*material, material_xml, logger);
- UrdfMaterial** mat =m_model.m_materials.find(material->m_name.c_str());
+ UrdfMaterial** mat =m_urdf2Model.m_materials.find(material->m_name.c_str());
if (mat)
{
logger->reportWarning("Duplicate material");
} else
{
- m_model.m_materials.insert(material->m_name.c_str(),material);
+ m_urdf2Model.m_materials.insert(material->m_name.c_str(),material);
}
}
- char msg[1024];
- sprintf(msg,"Num materials=%d", m_model.m_materials.size());
- logger->printMessage(msg);
+
+// char msg[1024];
+// sprintf(msg,"Num materials=%d", m_model.m_materials.size());
+// logger->printMessage(msg);
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
UrdfLink* link = new UrdfLink;
- if (parseLink(*link, link_xml,logger))
+ if (parseLink(m_urdf2Model,*link, link_xml,logger))
{
- if (m_model.m_links.find(link->m_name.c_str()))
+ if (m_urdf2Model.m_links.find(link->m_name.c_str()))
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
@@ -790,7 +1333,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
{
- UrdfMaterial** mat = m_model.m_materials.find(vis.m_materialName.c_str());
+ UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
@@ -802,7 +1345,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
}
- m_model.m_links.insert(link->m_name.c_str(),link);
+ m_urdf2Model.m_links.insert(link->m_name.c_str(),link);
}
} else
{
@@ -812,7 +1355,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
}
- if (m_model.m_links.size() == 0)
+ if (m_urdf2Model.m_links.size() == 0)
{
logger->reportWarning("No links found in URDF file");
return false;
@@ -825,7 +1368,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
if (parseJoint(*joint, joint_xml,logger))
{
- if (m_model.m_joints.find(joint->m_name.c_str()))
+ if (m_urdf2Model.m_joints.find(joint->m_name.c_str()))
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
@@ -833,7 +1376,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
else
{
- m_model.m_joints.insert(joint->m_name.c_str(),joint);
+ m_urdf2Model.m_joints.insert(joint->m_name.c_str(),joint);
}
}
else
@@ -843,7 +1386,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
}
- bool ok(initTreeAndRoot(logger));
+ bool ok(initTreeAndRoot(m_urdf2Model,logger));
if (!ok)
{
return false;
@@ -851,9 +1394,9 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
if (forceFixedBase)
{
- for (int i=0;i<m_model.m_rootLinks.size();i++)
+ for (int i=0;i<m_urdf2Model.m_rootLinks.size();i++)
{
- UrdfLink* link(m_model.m_rootLinks.at(i));
+ UrdfLink* link(m_urdf2Model.m_rootLinks.at(i));
link->m_inertia.m_mass = 0.0;
link->m_inertia.m_ixx = 0.0;
link->m_inertia.m_ixy = 0.0;
@@ -866,3 +1409,186 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
return true;
}
+
+void UrdfParser::activateModel(int modelIndex)
+{
+ m_activeSdfModel = modelIndex;
+}
+
+
+bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
+{
+
+ TiXmlDocument xml_doc;
+ xml_doc.Parse(sdfText);
+ if (xml_doc.Error())
+ {
+ logger->reportError(xml_doc.ErrorDesc());
+ xml_doc.ClearError();
+ return false;
+ }
+
+ TiXmlElement *sdf_xml = xml_doc.FirstChildElement("sdf");
+ if (!sdf_xml)
+ {
+ logger->reportError("expected an sdf element");
+ return false;
+ }
+
+ //apparently, SDF doesn't require a "world" element, optional? URDF does.
+ TiXmlElement *world_xml = sdf_xml->FirstChildElement("world");
+
+ TiXmlElement* robot_xml = 0;
+
+ if (!world_xml)
+ {
+ logger->reportWarning("expected a world element, continuing without it.");
+ robot_xml = sdf_xml->FirstChildElement("model");
+ } else
+ {
+ robot_xml = world_xml->FirstChildElement("model");
+ }
+
+ // Get all model (robot) elements
+ for (; robot_xml; robot_xml = robot_xml->NextSiblingElement("model"))
+ {
+ UrdfModel* localModel = new UrdfModel;
+ m_tmpModels.push_back(localModel);
+
+ TiXmlElement* stat = robot_xml->FirstChildElement("static");
+ if (0!=stat)
+ {
+ int val = int(atof(stat->GetText()));
+ if (val==1)
+ {
+ localModel->m_overrideFixedBase = true;
+ }
+ }
+
+
+ // Get robot name
+ const char *name = robot_xml->Attribute("name");
+ if (!name)
+ {
+ logger->reportError("Expected a name for robot");
+ return false;
+ }
+ localModel->m_name = name;
+
+ TiXmlElement* pose_xml = robot_xml->FirstChildElement("pose");
+ if (0==pose_xml)
+ {
+ localModel->m_rootTransformInWorld.setIdentity();
+ }
+ else
+ {
+ parseTransform(localModel->m_rootTransformInWorld,pose_xml,logger,m_parseSDF);
+ }
+
+ // Get all Material elements
+ for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
+ {
+ UrdfMaterial* material = new UrdfMaterial;
+
+ parseMaterial(*material, material_xml, logger);
+
+
+ UrdfMaterial** mat =localModel->m_materials.find(material->m_name.c_str());
+ if (mat)
+ {
+ logger->reportWarning("Duplicate material");
+ } else
+ {
+ localModel->m_materials.insert(material->m_name.c_str(),material);
+ }
+ }
+
+
+ // char msg[1024];
+ // sprintf(msg,"Num materials=%d", m_model.m_materials.size());
+ // logger->printMessage(msg);
+
+
+ for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
+ {
+ UrdfLink* link = new UrdfLink;
+
+ if (parseLink(*localModel, *link, link_xml,logger))
+ {
+ if (localModel->m_links.find(link->m_name.c_str()))
+ {
+ logger->reportError("Link name is not unique, link names in the same model have to be unique");
+ logger->reportError(link->m_name.c_str());
+ return false;
+ } else
+ {
+ //copy model material into link material, if link has no local material
+ for (int i=0;i<link->m_visualArray.size();i++)
+ {
+ UrdfVisual& vis = link->m_visualArray.at(i);
+ if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
+ {
+ UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
+ if (mat && *mat)
+ {
+ vis.m_localMaterial = **mat;
+ } else
+ {
+ //logger->reportError("Cannot find material with name:");
+ //logger->reportError(vis.m_materialName.c_str());
+ }
+ }
+ }
+
+ localModel->m_links.insert(link->m_name.c_str(),link);
+ }
+ } else
+ {
+ logger->reportError("failed to parse link");
+ delete link;
+ return false;
+ }
+
+ }
+ if (localModel->m_links.size() == 0)
+ {
+ logger->reportWarning("No links found in URDF file");
+ return false;
+ }
+
+ // Get all Joint elements
+ for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
+ {
+ UrdfJoint* joint = new UrdfJoint;
+
+ if (parseJoint(*joint, joint_xml,logger))
+ {
+ if (localModel->m_joints.find(joint->m_name.c_str()))
+ {
+ logger->reportError("joint '%s' is not unique.");
+ logger->reportError(joint->m_name.c_str());
+ return false;
+ }
+ else
+ {
+ localModel->m_joints.insert(joint->m_name.c_str(),joint);
+ }
+ }
+ else
+ {
+ logger->reportError("joint xml is not initialized correctly");
+ return false;
+ }
+ }
+
+ bool ok(initTreeAndRoot(*localModel,logger));
+ if (!ok)
+ {
+ return false;
+ }
+ m_sdfModels.push_back(localModel);
+ }
+
+ return true;
+}
+
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h
index cc0b788..9640f36 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.h
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.h
@@ -42,7 +42,9 @@ enum UrdfGeomTypes
URDF_GEOM_SPHERE=2,
URDF_GEOM_BOX,
URDF_GEOM_CYLINDER,
- URDF_GEOM_MESH
+ URDF_GEOM_MESH,
+ URDF_GEOM_PLANE,
+
};
@@ -57,6 +59,8 @@ struct UrdfGeometry
double m_cylinderRadius;
double m_cylinderLength;
+ btVector3 m_planeNormal;
+
std::string m_meshFileName;
btVector3 m_meshScale;
};
@@ -71,11 +75,22 @@ struct UrdfVisual
UrdfMaterial m_localMaterial;
};
+enum UrdfCollisionFlags
+{
+ URDF_FORCE_CONCAVE_TRIMESH=1,
+};
+
+
struct UrdfCollision
{
btTransform m_linkLocalFrame;
UrdfGeometry m_geometry;
std::string m_name;
+ int m_flags;
+ UrdfCollision()
+ :m_flags(0)
+ {
+ }
};
struct UrdfJoint;
@@ -84,6 +99,7 @@ struct UrdfLink
{
std::string m_name;
UrdfInertia m_inertia;
+ btTransform m_linkTransformInWorld;
btArray<UrdfVisual> m_visualArray;
btArray<UrdfCollision> m_collisionArray;
UrdfLink* m_parentLink;
@@ -94,6 +110,8 @@ struct UrdfLink
int m_linkIndex;
+ URDFLinkContactInfo m_contactInfo;
+
UrdfLink()
:m_parentLink(0),
m_parentJoint(0)
@@ -118,48 +136,123 @@ struct UrdfJoint
double m_jointDamping;
double m_jointFriction;
+ UrdfJoint()
+ :m_lowerLimit(0),
+ m_upperLimit(-1),
+ m_effortLimit(0),
+ m_velocityLimit(0),
+ m_jointDamping(0),
+ m_jointFriction(0)
+ {
+ }
};
struct UrdfModel
{
std::string m_name;
+ btTransform m_rootTransformInWorld;
btHashMap<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;
btArray<UrdfLink*> m_rootLinks;
+ bool m_overrideFixedBase;
+
+ UrdfModel()
+ :m_overrideFixedBase(false)
+ {
+ m_rootTransformInWorld.setIdentity();
+ }
};
class UrdfParser
{
protected:
+
+ UrdfModel m_urdf2Model;
+ btAlignedObjectArray<UrdfModel*> m_sdfModels;
+ btAlignedObjectArray<UrdfModel*> m_tmpModels;
+
+ bool m_parseSDF;
+ int m_activeSdfModel;
+
+
+ void cleanModel(UrdfModel* model);
bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
- bool parseVisual(UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
+ bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
bool parseCollision(UrdfCollision& collision, class TiXmlElement* config, ErrorLogger* logger);
- bool initTreeAndRoot(ErrorLogger* logger);
+ bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger);
bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
+ bool parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger);
- bool parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
- UrdfModel m_model;
+ bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
+
public:
UrdfParser();
virtual ~UrdfParser();
- bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
+ void setParseSDF(bool useSDF)
+ {
+ m_parseSDF = useSDF;
+ }
+ bool getParseSDF() const
+ {
+ return m_parseSDF;
+ }
+ bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
+ bool loadSDF(const char* sdfText, ErrorLogger* logger);
+
+ int getNumModels() const
+ {
+ //user should have loaded an SDF when calling this method
+ btAssert(m_parseSDF);
+ if (m_parseSDF)
+ {
+ return m_sdfModels.size();
+ }
+ return 0;
+ }
+
+ void activateModel(int modelIndex);
+
+ UrdfModel& getModelByIndex(int index)
+ {
+ //user should have loaded an SDF when calling this method
+ btAssert(m_parseSDF);
+
+ return *m_sdfModels[index];
+ }
- const UrdfModel& getModel() const
+ const UrdfModel& getModelByIndex(int index) const
+ {
+ //user should have loaded an SDF when calling this method
+ btAssert(m_parseSDF);
+
+ return *m_sdfModels[index];
+ }
+
+ const UrdfModel& getModel() const
{
- return m_model;
+ if (m_parseSDF)
+ {
+ return *m_sdfModels[m_activeSdfModel];
+ }
+
+ return m_urdf2Model;
}
UrdfModel& getModel()
{
- return m_model;
+ if (m_parseSDF)
+ {
+ return *m_sdfModels[m_activeSdfModel];
+ }
+ return m_urdf2Model;
}
};
diff --git a/examples/Importers/ImportURDFDemo/urdfStringSplit.cpp b/examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
index a5a1adf..9ed88e7 100644
--- a/examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
+++ b/examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
@@ -8,7 +8,7 @@
#include "urdfStringSplit.h"
-void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, btAlignedObjectArray<std::string> separators)
+void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators)
{
assert(separators.size()==1);
if (separators.size()==1)
@@ -20,10 +20,9 @@ void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::strin
urdfStrArrayFree(strArray);
}
}
- btAlignedObjectArray<std::string> urdfIsAnyOf(const char* seps)
+ void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray)
{
- btAlignedObjectArray<std::string> strArray;
-
+
int numSeps = strlen(seps);
for (int i=0;i<numSeps;i++)
{
@@ -32,8 +31,6 @@ void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::strin
sep2[0] = seps[i];
strArray.push_back(sep2);
}
- return strArray;
-
}
diff --git a/examples/Importers/ImportURDFDemo/urdfStringSplit.h b/examples/Importers/ImportURDFDemo/urdfStringSplit.h
index 6eaa42b..dd7af91 100644
--- a/examples/Importers/ImportURDFDemo/urdfStringSplit.h
+++ b/examples/Importers/ImportURDFDemo/urdfStringSplit.h
@@ -7,9 +7,9 @@
#include "LinearMath/btAlignedObjectArray.h"
-void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, btAlignedObjectArray<std::string> separators);
+void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators);
-btAlignedObjectArray<std::string> urdfIsAnyOf(const char* seps);
+void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray);
///The string split C code is by Lars Wirzenius
///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
diff --git a/examples/InverseDynamics/InverseDynamicsExample.cpp b/examples/InverseDynamics/InverseDynamicsExample.cpp
index e062e17..9eb9287 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.cpp
+++ b/examples/InverseDynamics/InverseDynamicsExample.cpp
@@ -23,6 +23,7 @@ subject to the following restrictions:
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
+
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "btBulletDynamicsCommon.h"
@@ -85,10 +86,10 @@ public:
virtual void resetCamera()
{
- float dist = 3.5;
- float pitch = -136;
- float yaw = 28;
- float targetPos[3]={0.47,0,-0.64};
+ float dist = 1.5;
+ float pitch = -80;
+ float yaw = 10;
+ float targetPos[3]={0,0,0};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
};
@@ -104,7 +105,7 @@ InverseDynamicsExample::InverseDynamicsExample(struct GUIHelperInterface* helper
InverseDynamicsExample::~InverseDynamicsExample()
{
- delete m_multiBody;
+
delete m_inverseModel;
delete m_timeSeriesCanvas;
}
@@ -129,13 +130,15 @@ void InverseDynamicsExample::initPhysics()
SliderParams slider("Kp",&kp);
slider.m_minVal=0;
slider.m_maxVal=2000;
- m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ if (m_guiHelper->getParameterInterface())
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Kd",&kd);
slider.m_minVal=0;
slider.m_maxVal=50;
- m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ if (m_guiHelper->getParameterInterface())
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
if (m_option == BT_ID_PROGRAMMATICALLY)
@@ -150,8 +153,11 @@ void InverseDynamicsExample::initPhysics()
{
case BT_ID_LOAD_URDF:
{
- BulletURDFImporter u2b(m_guiHelper);
- bool loadOk = u2b.loadURDF("kuka_lwr/kuka.urdf");
+
+
+
+ BulletURDFImporter u2b(m_guiHelper,0);
+ bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf");
if (loadOk)
{
int rootLinkIndex = u2b.getRootLinkIndex();
@@ -160,6 +166,10 @@ void InverseDynamicsExample::initPhysics()
btTransform identityTrans;
identityTrans.setIdentity();
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix());
+ for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
+ {
+ m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
+ }
m_multiBody = creation.getBulletMultiBody();
if (m_multiBody)
{
@@ -189,9 +199,11 @@ void InverseDynamicsExample::initPhysics()
if(m_multiBody) {
{
- m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,512,230, "Joint Space Trajectory");
- m_timeSeriesCanvas ->setupTimeSeries(3,100, 0);
-
+ if (m_guiHelper->getAppInterface() && m_guiHelper->getParameterInterface())
+ {
+ m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,512,230, "Joint Space Trajectory");
+ m_timeSeriesCanvas ->setupTimeSeries(3,100, 0);
+ }
}
// construct inverse model
@@ -203,27 +215,34 @@ void InverseDynamicsExample::initPhysics()
}
// add joint target controls
qd.resize(m_multiBody->getNumDofs());
+
qd_name.resize(m_multiBody->getNumDofs());
q_name.resize(m_multiBody->getNumDofs());
- for(std::size_t dof=0;dof<qd.size();dof++) {
- qd[dof] = 0;
- char tmp[25];
- sprintf(tmp,"q_desired[%zu]",dof);
- qd_name[dof] = tmp;
- SliderParams slider(qd_name[dof].c_str(),&qd[dof]);
- slider.m_minVal=-3.14;
- slider.m_maxVal=3.14;
- sprintf(tmp,"q[%zu]",dof);
- q_name[dof] = tmp;
- m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
- btVector4 color = sJointCurveColors[dof&7];
- m_timeSeriesCanvas->addDataSource(q_name[dof].c_str(), color[0]*255,color[1]*255,color[2]*255);
- }
-
+
+ if (m_timeSeriesCanvas && m_guiHelper->getParameterInterface())
+ {
+ for(std::size_t dof=0;dof<qd.size();dof++)
+ {
+ qd[dof] = 0;
+ char tmp[25];
+ sprintf(tmp,"q_desired[%u]",dof);
+ qd_name[dof] = tmp;
+ SliderParams slider(qd_name[dof].c_str(),&qd[dof]);
+ slider.m_minVal=-3.14;
+ slider.m_maxVal=3.14;
+
+ sprintf(tmp,"q[%u]",dof);
+ q_name[dof] = tmp;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ btVector4 color = sJointCurveColors[dof&7];
+ m_timeSeriesCanvas->addDataSource(q_name[dof].c_str(), color[0]*255,color[1]*255,color[2]*255);
+
+ }
+ }
}
-
+ m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
@@ -246,7 +265,8 @@ void InverseDynamicsExample::stepSimulation(float deltaTime)
const btScalar qd_dot=0;
const btScalar qd_ddot=0;
- m_timeSeriesCanvas->insertDataAtCurrentTime(q[dof],dof,true);
+ if (m_timeSeriesCanvas)
+ m_timeSeriesCanvas->insertDataAtCurrentTime(q[dof],dof,true);
// pd_control is either desired joint torque for pd control,
// or the feedback contribution to nu
@@ -320,6 +340,21 @@ void InverseDynamicsExample::stepSimulation(float deltaTime)
// todo(thomas) check that this is correct:
// want to advance by 10ms, with 1ms timesteps
m_dynamicsWorld->stepSimulation(1e-3,0);//,1e-3);
+ btAlignedObjectArray<btQuaternion> scratch_q;
+ btAlignedObjectArray<btVector3> scratch_m;
+ m_multiBody->forwardKinematics(scratch_q, scratch_m);
+ for (int i = 0; i < m_multiBody->getNumLinks(); i++)
+ {
+ //btVector3 pos = m_multiBody->getLink(i).m_cachedWorldTransform.getOrigin();
+ btTransform tr = m_multiBody->getLink(i).m_cachedWorldTransform;
+ btVector3 pos = tr.getOrigin() - quatRotate(tr.getRotation(), m_multiBody->getLink(i).m_dVector);
+ btVector3 localAxis = m_multiBody->getLink(i).m_axes[0].m_topVec;
+ //printf("link %d: %f,%f,%f, local axis:%f,%f,%f\n", i, pos.x(), pos.y(), pos.z(), localAxis.x(), localAxis.y(), localAxis.z());
+
+
+
+
+ }
}
}
@@ -330,3 +365,4 @@ CommonExampleInterface* InverseDynamicsExampleCreateFunc(CommonExampleOptions
+B3_STANDALONE_EXAMPLE(InverseDynamicsExampleCreateFunc)
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/InverseDynamics/InverseDynamicsExample.h
index 13c37a4..06df04b 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/InverseDynamics/InverseDynamicsExample.h
@@ -18,8 +18,8 @@ subject to the following restrictions:
enum btInverseDynamicsExampleOptions
{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
+ BT_ID_LOAD_URDF=0,
+ BT_ID_PROGRAMMATICALLY=1
};
class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
diff --git a/examples/InverseDynamics/premake4.lua b/examples/InverseDynamics/premake4.lua
new file mode 100644
index 0000000..75105e7
--- /dev/null
+++ b/examples/InverseDynamics/premake4.lua
@@ -0,0 +1,221 @@
+
+project "App_InverseDynamicsExample"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+includedirs {"../../src"}
+
+links {
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","BulletDynamics","BulletCollision", "LinearMath"
+}
+
+language "C++"
+
+files {
+ "**.cpp",
+ "**.h",
+ "../StandaloneMain/main_console_single_example.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.h",
+ "../RenderingExamples/TimeSeriesCanvas.cpp",
+ "../RenderingExamples/TimeSeriesFontData.cpp",
+ "../MultiBody/InvertedPendulumPDControl.cpp",
+ "../ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../ThirdPartyLibs/stb_image/stb_image.cpp",
+}
+
+
+project "App_InverseDynamicsExampleGui"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
+}
+
+ initOpenGL()
+ initGlew()
+
+
+language "C++"
+
+files {
+ "InverseDynamicsExample.cpp",
+ "*.h",
+ "../StandaloneMain/main_opengl_single_example.cpp",
+ "../ExampleBrowser/OpenGLGuiHelper.cpp",
+ "../ExampleBrowser/GL_ShapeDrawer.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.h",
+ "../RenderingExamples/TimeSeriesCanvas.cpp",
+ "../RenderingExamples/TimeSeriesFontData.cpp",
+ "../MultiBody/InvertedPendulumPDControl.cpp",
+ "../ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../ThirdPartyLibs/stb_image/stb_image.cpp",
+ "../Utils/b3Clock.cpp",
+}
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+end
+
+
+
+project "App_InverseDynamicsExampleGuiWithSoftwareRenderer"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
+}
+
+ initOpenGL()
+ initGlew()
+
+
+language "C++"
+
+files {
+ "InverseDynamicsExample.cpp",
+ "*.h",
+ "../StandaloneMain/main_sw_tinyrenderer_single_example.cpp",
+ "../ExampleBrowser/OpenGLGuiHelper.cpp",
+ "../ExampleBrowser/GL_ShapeDrawer.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../TinyRenderer/geometry.cpp",
+ "../TinyRenderer/model.cpp",
+ "../TinyRenderer/tgaimage.cpp",
+ "../TinyRenderer/our_gl.cpp",
+ "../TinyRenderer/TinyRenderer.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.h",
+ "../RenderingExamples/TimeSeriesCanvas.cpp",
+ "../RenderingExamples/TimeSeriesFontData.cpp",
+ "../MultiBody/InvertedPendulumPDControl.cpp",
+ "../ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../ThirdPartyLibs/stb_image/stb_image.cpp",
+}
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+end
+
+
+project "App_InverseDynamicsExampleTinyRenderer"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics","BulletDynamics","BulletCollision", "LinearMath", "Bullet3Common"
+}
+
+language "C++"
+
+files {
+ "InverseDynamicsExample.cpp",
+ "*.h",
+ "../StandaloneMain/main_tinyrenderer_single_example.cpp",
+ "../OpenGLWindow/SimpleCamera.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../TinyRenderer/geometry.cpp",
+ "../TinyRenderer/model.cpp",
+ "../TinyRenderer/tgaimage.cpp",
+ "../TinyRenderer/our_gl.cpp",
+ "../TinyRenderer/TinyRenderer.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.cpp",
+ "../Utils/b3ResourcePath.h",
+ "../RenderingExamples/TimeSeriesCanvas.cpp",
+ "../RenderingExamples/TimeSeriesFontData.cpp",
+ "../MultiBody/InvertedPendulumPDControl.cpp",
+ "../ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../ThirdPartyLibs/stb_image/stb_image.cpp",
+}
+
diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp
new file mode 100644
index 0000000..3888267
--- /dev/null
+++ b/examples/InverseKinematics/InverseKinematicsExample.cpp
@@ -0,0 +1,385 @@
+#include "InverseKinematicsExample.h"
+
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3Transform.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../OpenGLWindow/OpenGLInclude.h"
+
+#include "BussIK/Node.h"
+#include "BussIK/Tree.h"
+#include "BussIK/Jacobian.h"
+#include "BussIK/VectorRn.h"
+
+#define RADIAN(X) ((X)*RadiansToDegrees)
+
+#define MAX_NUM_NODE 1000
+#define MAX_NUM_THETA 1000
+#define MAX_NUM_EFFECT 100
+
+double T = 0;
+VectorR3 targetaa[MAX_NUM_EFFECT];
+
+
+
+// Make slowdown factor larger to make the simulation take larger, less frequent steps
+// Make the constant factor in Tstep larger to make time pass more quickly
+//const int SlowdownFactor = 40;
+const int SlowdownFactor = 0; // Make higher to take larger steps less frequently
+const int SleepsPerStep=SlowdownFactor;
+int SleepCounter=0;
+//const double Tstep = 0.0005*(double)SlowdownFactor; // Time step
+
+
+int AxesList; /* list to hold the axes */
+int AxesOn; /* ON or OFF */
+
+float Scale, Scale2; /* scaling factors */
+
+
+
+int JointLimitsOn;
+int RestPositionOn;
+int UseJacobianTargets1;
+
+
+int numIteration = 1;
+double error = 0.0;
+double errorDLS = 0.0;
+double errorSDLS = 0.0;
+double sumError = 0.0;
+double sumErrorDLS = 0.0;
+double sumErrorSDLS = 0.0;
+
+#ifdef _DYNAMIC
+bool initMaxDist = true;
+extern double Excess[];
+extern double dsnorm[];
+#endif
+
+
+
+
+void Reset(Tree &tree, Jacobian* m_ikJacobian)
+{
+ AxesOn = false;
+
+ Scale = 1.0;
+ Scale2 = 0.0; /* because add 1. to it in Display() */
+
+ JointLimitsOn = true;
+ RestPositionOn = false;
+ UseJacobianTargets1 = false;
+
+
+ tree.Init();
+ tree.Compute();
+ m_ikJacobian->Reset();
+
+}
+
+// Update target positions
+
+void UpdateTargets( double T2, Tree & treeY) {
+ double T = T2 / 5.;
+ targetaa[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T));
+}
+
+
+// Does a single update (on one kind of tree)
+void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) {
+
+ if ( SleepCounter==0 ) {
+ T += Tstep;
+ UpdateTargets( T , treeY);
+ }
+
+ if ( UseJacobianTargets1 ) {
+ jacob->SetJtargetActive();
+ }
+ else {
+ jacob->SetJendActive();
+ }
+ jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors
+
+ // Calculate the change in theta values
+ switch (ikMethod) {
+ case IK_JACOB_TRANS:
+ jacob->CalcDeltaThetasTranspose(); // Jacobian transpose method
+ break;
+ case IK_DLS:
+ jacob->CalcDeltaThetasDLS(); // Damped least squares method
+ break;
+ case IK_DLS_SVD:
+ jacob->CalcDeltaThetasDLSwithSVD();
+ break;
+ case IK_PURE_PSEUDO:
+ jacob->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
+ break;
+ case IK_SDLS:
+ jacob->CalcDeltaThetasSDLS(); // Selectively damped least squares method
+ break;
+ default:
+ jacob->ZeroDeltaThetas();
+ break;
+ }
+
+ if ( SleepCounter==0 ) {
+ jacob->UpdateThetas(); // Apply the change in the theta values
+ jacob->UpdatedSClampValue(targetaa);
+ SleepCounter = SleepsPerStep;
+ }
+ else {
+ SleepCounter--;
+ }
+
+
+}
+
+
+
+
+
+
+
+
+///quick demo showing the right-handed coordinate system and positive rotations around each axis
+class InverseKinematicsExample : public CommonExampleInterface
+{
+ CommonGraphicsApp* m_app;
+ int m_ikMethod;
+ Tree m_ikTree;
+ b3AlignedObjectArray<Node*> m_ikNodes;
+ Jacobian* m_ikJacobian;
+
+ float m_x;
+ float m_y;
+ float m_z;
+ b3AlignedObjectArray<int> m_movingInstances;
+ int m_targetInstance;
+ enum
+ {
+ numCubesX = 20,
+ numCubesY = 20
+ };
+public:
+
+ InverseKinematicsExample(CommonGraphicsApp* app, int option)
+ :m_app(app),
+ m_x(0),
+ m_y(0),
+ m_z(0),
+ m_targetInstance(-1),
+ m_ikMethod(option)
+ {
+ m_app->setUpAxis(2);
+
+ {
+ b3Vector3 extents=b3MakeVector3(100,100,100);
+ extents[m_app->getUpAxis()]=1;
+
+ int xres = 20;
+ int yres = 20;
+
+ b3Vector4 color0=b3MakeVector4(0.4, 0.4, 0.4,1);
+ b3Vector4 color1=b3MakeVector4(0.6, 0.6, 0.6,1);
+ m_app->registerGrid(xres, yres, color0, color1);
+ }
+
+ ///create some graphics proxy for the tracking target
+ ///the endeffector tries to track it using Inverse Kinematics
+ {
+ int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM);
+ b3Vector3 pos = b3MakeVector3(1,1,1);
+ pos[app->getUpAxis()] = 1;
+ b3Quaternion orn(0, 0, 0, 1);
+ b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
+ b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
+ m_targetInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling);
+ }
+ m_app->m_renderer->writeTransforms();
+ }
+ virtual ~InverseKinematicsExample()
+ {
+ m_app->m_renderer->enableBlend(false);
+ }
+
+
+ virtual void physicsDebugDraw(int debugDrawMode)
+ {
+
+ }
+ virtual void initPhysics()
+ {
+ BuildKukaIIWAShape();
+ m_ikJacobian = new Jacobian(&m_ikTree);
+ Reset(m_ikTree,m_ikJacobian);
+ }
+ virtual void exitPhysics()
+ {
+ delete m_ikJacobian;
+ m_ikJacobian = 0;
+ }
+
+ void BuildKukaIIWAShape();
+
+ void getLocalTransform(const Node* node, b3Transform& act)
+ {
+ b3Vector3 axis = b3MakeVector3(node->v.x, node->v.y, node->v.z);
+ b3Quaternion rot(0, 0, 0, 1);
+ if (axis.length())
+ {
+ rot = b3Quaternion (axis, node->theta);
+ }
+ act.setIdentity();
+ act.setRotation(rot);
+ act.setOrigin(b3MakeVector3(node->r.x, node->r.y, node->r.z));
+ }
+ void MyDrawTree(Node* node, const b3Transform& tr)
+ {
+ b3Vector3 lineColor = b3MakeVector3(0, 0, 0);
+ int lineWidth = 2;
+ if (node != 0) {
+ // glPushMatrix();
+ b3Vector3 pos = b3MakeVector3(tr.getOrigin().x, tr.getOrigin().y, tr.getOrigin().z);
+ b3Vector3 color = b3MakeVector3(0, 1, 0);
+ int pointSize = 10;
+ m_app->m_renderer->drawPoint(pos, color, pointSize);
+
+ m_app->m_renderer->drawLine(pos, pos+ 0.05*tr.getBasis().getColumn(0), b3MakeVector3(1,0,0), lineWidth);
+ m_app->m_renderer->drawLine(pos, pos + 0.05*tr.getBasis().getColumn(1), b3MakeVector3(0, 1, 0), lineWidth);
+ m_app->m_renderer->drawLine(pos, pos + 0.05*tr.getBasis().getColumn(2), b3MakeVector3(0, 0, 1), lineWidth);
+
+ b3Vector3 axisLocal = b3MakeVector3(node->v.x, node->v.y, node->v.z);
+ b3Vector3 axisWorld = tr.getBasis()*axisLocal;
+
+ m_app->m_renderer->drawLine(pos, pos + 0.1*axisWorld, b3MakeVector3(.2, 0.2, 0.7), 5);
+
+ //node->DrawNode(node == root); // Recursively draw node and update ModelView matrix
+ if (node->left) {
+ b3Transform act;
+ getLocalTransform(node->left, act);
+
+ b3Transform trl = tr*act;
+ m_app->m_renderer->drawLine(tr.getOrigin(), trl.getOrigin(), lineColor, lineWidth);
+ MyDrawTree(node->left, trl); // Draw tree of children recursively
+ }
+ // glPopMatrix();
+ if (node->right) {
+ b3Transform act;
+ getLocalTransform(node->right, act);
+ b3Transform trr = tr*act;
+ m_app->m_renderer->drawLine(tr.getOrigin(), trr.getOrigin(), lineColor, lineWidth);
+ MyDrawTree(node->right,trr); // Draw right siblings recursively
+ }
+ }
+
+ }
+ virtual void stepSimulation(float deltaTime)
+ {
+ DoUpdateStep(deltaTime, m_ikTree, m_ikJacobian, m_ikMethod);
+ }
+ virtual void renderScene()
+ {
+
+
+ b3Transform act;
+ getLocalTransform(m_ikTree.GetRoot(), act);
+ MyDrawTree(m_ikTree.GetRoot(), act);
+
+ b3Vector3 pos = b3MakeVector3(targetaa[0].x, targetaa[0].y, targetaa[0].z);
+ b3Quaternion orn(0, 0, 0, 1);
+
+ m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstance);
+ m_app->m_renderer->writeTransforms();
+ m_app->m_renderer->renderScene();
+ }
+
+
+ virtual void physicsDebugDraw()
+ {
+
+ }
+ virtual bool mouseMoveCallback(float x,float y)
+ {
+ return false;
+ }
+ virtual bool mouseButtonCallback(int button, int state, float x, float y)
+ {
+ return false;
+ }
+ virtual bool keyboardCallback(int key, int state)
+ {
+ return false;
+ }
+
+ virtual void resetCamera()
+ {
+ float dist = 1.3;
+ float pitch = 120;
+ float yaw = 13;
+ float targetPos[3]={-0.35,0.14,0.25};
+ if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
+ {
+
+ m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
+ m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
+ m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
+ m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
+ }
+ }
+
+};
+
+void InverseKinematicsExample::BuildKukaIIWAShape()
+{
+ const VectorR3& unitx = VectorR3::UnitX;
+ const VectorR3& unity = VectorR3::UnitY;
+ const VectorR3& unitz = VectorR3::UnitZ;
+ const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0);
+ const VectorR3& zero = VectorR3::Zero;
+
+ float minTheta = -4 * PI;
+ float maxTheta = 4 * PI;
+
+ m_ikNodes.resize(8);//7DOF+additional endeffector
+
+ m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.));
+ m_ikTree.InsertRoot(m_ikNodes[0]);
+
+ m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.));
+ m_ikTree.InsertLeftChild(m_ikNodes[0], m_ikNodes[1]);
+
+ m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+ m_ikTree.InsertLeftChild(m_ikNodes[1], m_ikNodes[2]);
+
+ m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+ m_ikTree.InsertLeftChild(m_ikNodes[2], m_ikNodes[3]);
+
+ m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+ m_ikTree.InsertLeftChild(m_ikNodes[3], m_ikNodes[4]);
+
+ m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+ m_ikTree.InsertLeftChild(m_ikNodes[4], m_ikNodes[5]);
+
+ m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
+ m_ikTree.InsertLeftChild(m_ikNodes[5], m_ikNodes[6]);
+
+ m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR);
+ m_ikTree.InsertLeftChild(m_ikNodes[6], m_ikNodes[7]);
+
+}
+
+
+class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options)
+{
+ return new InverseKinematicsExample(options.m_guiHelper->getAppInterface(), options.m_option);
+}
+
+
+
+
+
diff --git a/examples/InverseKinematics/InverseKinematicsExample.h b/examples/InverseKinematics/InverseKinematicsExample.h
new file mode 100644
index 0000000..845925e
--- /dev/null
+++ b/examples/InverseKinematics/InverseKinematicsExample.h
@@ -0,0 +1,8 @@
+#ifndef INVERSE_KINEMATICSEXAMPLE_H
+#define INVERSE_KINEMATICSEXAMPLE_H
+
+enum Method {IK_JACOB_TRANS=0, IK_PURE_PSEUDO, IK_DLS, IK_SDLS , IK_DLS_SVD};
+
+class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options);
+
+#endif //INVERSE_KINEMATICSEXAMPLE_H
diff --git a/examples/LuaDemo/LuaPhysicsSetup.cpp b/examples/LuaDemo/LuaPhysicsSetup.cpp
index bb55e54..e462f53 100644
--- a/examples/LuaDemo/LuaPhysicsSetup.cpp
+++ b/examples/LuaDemo/LuaPhysicsSetup.cpp
@@ -2,7 +2,7 @@
#include "../CommonInterfaces/CommonMultiBodyBase.h"
-#include "../Importers/ImportURDFDemo/MyURDFImporter.h"
+#include "../Importers/ImportURDFDemo/BulletURDFImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
@@ -39,7 +39,12 @@ extern "C" {
}
-const char* sLuaFileName = "init_urdf.lua";//init_physics.lua";
+const char* sLuaFileName = "init_physics.lua";
+static int upaxis = 1;
+
+//const char* sLuaFileName = "init_urdf.lua";
+//static int upaxis = 2;
+
static const float scaling=0.35f;
static LuaPhysicsSetup* sLuaDemo = 0;
@@ -69,7 +74,10 @@ LuaPhysicsSetup::~LuaPhysicsSetup()
static int gCreateDefaultDynamicsWorld(lua_State *L)
{
sLuaDemo->createEmptyDynamicsWorld();
- sLuaDemo->m_dynamicsWorld->setGravity(btVector3(0,0,-10));
+ btVector3 grav(0,0,0);
+ grav[upaxis] = -10;
+
+ sLuaDemo->m_dynamicsWorld->setGravity(grav);
sLuaDemo->m_guiHelper->createPhysicsDebugDrawer(sLuaDemo->m_dynamicsWorld);
lua_pushlightuserdata (L, sLuaDemo->m_dynamicsWorld);
return 1;
@@ -211,8 +219,8 @@ static int gLoadMultiBodyFromUrdf(lua_State *L)
return 0;
}
const char* fileName = lua_tostring(L,2);
-
- MyURDFImporter u2b(sLuaDemo->m_guiHelper);
+#if 1
+ BulletURDFImporter u2b(sLuaDemo->m_guiHelper);
bool loadOk = u2b.loadURDF(fileName);
if (loadOk)
{
@@ -240,6 +248,7 @@ static int gLoadMultiBodyFromUrdf(lua_State *L)
{
b3Printf("can't find %s",fileName);
}
+#endif
}
return 0;
@@ -377,7 +386,7 @@ static void report_errors(lua_State *L, int status)
void LuaPhysicsSetup::initPhysics()
{
- m_guiHelper->setUpAxis(2);
+ m_guiHelper->setUpAxis(upaxis);
const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
int numPrefixes = sizeof(prefix)/sizeof(const char*);
char relativeFileName[1024];
diff --git a/examples/MultiBody/MultiBodySoftContact.cpp b/examples/MultiBody/MultiBodySoftContact.cpp
new file mode 100644
index 0000000..b7ce1c9
--- /dev/null
+++ b/examples/MultiBody/MultiBodySoftContact.cpp
@@ -0,0 +1,173 @@
+
+#include "MultiBodySoftContact.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+static btScalar radius(0.2);
+
+struct MultiBodySoftContact : public CommonMultiBodyBase
+{
+ btMultiBody* m_multiBody;
+ btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
+
+ bool m_once;
+public:
+
+ MultiBodySoftContact(struct GUIHelperInterface* helper);
+ virtual ~MultiBodySoftContact();
+
+ virtual void initPhysics();
+
+ virtual void stepSimulation(float deltaTime);
+
+ virtual void resetCamera()
+ {
+ float dist = 5;
+ float pitch = 270;
+ float yaw = 21;
+ float targetPos[3]={0,0,0};
+ m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+ }
+
+
+};
+
+
+
+MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper)
+:CommonMultiBodyBase(helper),
+m_once(true)
+{
+}
+
+MultiBodySoftContact::~MultiBodySoftContact()
+{
+
+}
+
+
+
+
+void MultiBodySoftContact::initPhysics()
+{
+
+ int upAxis = 2;
+
+ m_guiHelper->setUpAxis(upAxis);
+
+ btVector4 colors[4] =
+ {
+ btVector4(1,0,0,1),
+ btVector4(0,1,0,1),
+ btVector4(0,1,1,1),
+ btVector4(1,1,0,1),
+ };
+ int curColor = 0;
+
+
+
+
+ this->createEmptyDynamicsWorld();
+ m_dynamicsWorld->setGravity(btVector3(0,0,-10));
+
+ m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+ m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+ //btIDebugDraw::DBG_DrawConstraints
+ +btIDebugDraw::DBG_DrawWireframe
+ +btIDebugDraw::DBG_DrawContactPoints
+ +btIDebugDraw::DBG_DrawAabb
+ );//+btIDebugDraw::DBG_DrawConstraintLimits);
+
+
+
+ //create a static ground object
+ if (1)
+ {
+ btVector3 groundHalfExtents(50,50,50);
+ btBoxShape* box = new btBoxShape(groundHalfExtents);
+ box->initializePolyhedralFeatures();
+
+ m_guiHelper->createCollisionShapeGraphicsObject(box);
+ btTransform start; start.setIdentity();
+ btVector3 groundOrigin(0,0,-50.5);
+ start.setOrigin(groundOrigin);
+ // start.setRotation(groundOrn);
+ btRigidBody* body = createRigidBody(0,start,box);
+
+ //setContactStiffnessAndDamping will enable compliant rigid body contact
+ body->setContactStiffnessAndDamping(300,10);
+ btVector4 color = colors[curColor];
+ curColor++;
+ curColor&=3;
+ m_guiHelper->createRigidBodyGraphicsObject(body,color);
+
+ }
+
+ {
+ btCollisionShape* childShape = new btSphereShape(btScalar(0.5));
+ m_guiHelper->createCollisionShapeGraphicsObject(childShape);
+
+ btScalar mass = 1;
+ btVector3 baseInertiaDiag;
+ bool isFixed = (mass == 0);
+ childShape->calculateLocalInertia(mass,baseInertiaDiag);
+ btMultiBody *pMultiBody = new btMultiBody(0, 1, baseInertiaDiag, false, false);
+ btTransform startTrans;
+ startTrans.setIdentity();
+ startTrans.setOrigin(btVector3(0,0,3));
+
+ pMultiBody->setBaseWorldTransform(startTrans);
+
+ btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
+ col->setCollisionShape(childShape);
+ pMultiBody->setBaseCollider(col);
+ bool isDynamic = (mass > 0 && !isFixed);
+ short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
+ short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+ m_dynamicsWorld->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
+
+
+ pMultiBody->finalizeMultiDof();
+
+ m_dynamicsWorld->addMultiBody(pMultiBody);
+
+ btAlignedObjectArray<btQuaternion> scratch_q;
+ btAlignedObjectArray<btVector3> scratch_m;
+ pMultiBody->forwardKinematics(scratch_q,scratch_m);
+ btAlignedObjectArray<btQuaternion> world_to_local;
+ btAlignedObjectArray<btVector3> local_origin;
+ pMultiBody->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
+ }
+ m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+
+
+}
+
+void MultiBodySoftContact::stepSimulation(float deltaTime)
+{
+
+ if (0)//m_once)
+ {
+ m_once=false;
+ m_multiBody->addJointTorque(0, 10.0);
+
+ btScalar torque = m_multiBody->getJointTorque(0);
+ b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
+ }
+
+ m_dynamicsWorld->stepSimulation(deltaTime);
+
+
+
+}
+
+
+class CommonExampleInterface* MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options)
+{
+ return new MultiBodySoftContact(options.m_guiHelper);
+}
diff --git a/examples/MultiBody/MultiBodySoftContact.h b/examples/MultiBody/MultiBodySoftContact.h
new file mode 100644
index 0000000..e501cc9
--- /dev/null
+++ b/examples/MultiBody/MultiBodySoftContact.h
@@ -0,0 +1,7 @@
+#ifndef MULTI_BODY_SOFT_CONTACT_H
+#define MULTI_BODY_SOFT_CONTACT_H
+
+class CommonExampleInterface* MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options);
+
+#endif //MULTI_BODY_SOFT_CONTACT_H
+
diff --git a/examples/MultiBody/MultiDofDemo.cpp b/examples/MultiBody/MultiDofDemo.cpp
index f4dc2f6..33719a2 100644
--- a/examples/MultiBody/MultiDofDemo.cpp
+++ b/examples/MultiBody/MultiDofDemo.cpp
@@ -10,6 +10,8 @@
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "../OpenGLWindow/GLInstancingRenderer.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"
@@ -134,7 +136,8 @@ void MultiDofDemo::initPhysics()
bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool multibodyOnly = false;
bool canSleep = true;
- bool selfCollide = false;
+ bool selfCollide = false;
+ bool multibodyConstraint = true;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
@@ -225,7 +228,7 @@ void MultiDofDemo::initPhysics()
startTransform.setOrigin(btVector3(
btScalar(0.0),
- -0.95,
+ 0.0,
btScalar(0.0)));
@@ -236,7 +239,19 @@ void MultiDofDemo::initPhysics()
m_dynamicsWorld->addRigidBody(body);//,1,1+2);
-
+ if (multibodyConstraint) {
+ btVector3 pointInA = -linkHalfExtents;
+ btVector3 pointInB = halfExtents;
+ btMatrix3x3 frameInA;
+ btMatrix3x3 frameInB;
+ frameInA.setIdentity();
+ frameInB.setIdentity();
+ btVector3 jointAxis(1.0,0.0,0.0);
+ btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis);
+ //btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB);
+ p2p->setMaxAppliedImpulse(2.0);
+ m_dynamicsWorld->addMultiBodyConstraint(p2p);
+ }
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
@@ -294,7 +309,7 @@ btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
- pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
+ pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
pMultiBody->finalizeMultiDof();
diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp
index 8647b87..9ff47b8 100644
--- a/examples/MultiBody/TestJointTorqueSetup.cpp
+++ b/examples/MultiBody/TestJointTorqueSetup.cpp
@@ -367,9 +367,8 @@ void TestJointTorqueSetup::initPhysics()
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
- b3ResourcePath p;
char resourcePath[1024];
- if (p.findResourcePath("multibody.bullet",resourcePath,1024))
+ if (b3ResourcePath::findResourcePath("multibody.bullet",resourcePath,1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
diff --git a/examples/MultiThreading/MultiThreadingExample.cpp b/examples/MultiThreading/MultiThreadingExample.cpp
index 0cf47c5..1969912 100644
--- a/examples/MultiThreading/MultiThreadingExample.cpp
+++ b/examples/MultiThreading/MultiThreadingExample.cpp
@@ -68,6 +68,8 @@ struct SampleJob1 : public SampleJobInterface
m_jobId(jobId)
{
}
+ virtual ~SampleJob1() {}
+
virtual void executeJob(int threadIndex)
{
printf("start SampleJob1 %d using threadIndex %d\n",m_jobId,threadIndex);
@@ -117,7 +119,7 @@ struct SampleThreadLocalStorage
void SampleThreadFunc(void* userPtr,void* lsMemory)
{
- printf("thread started\n");
+ printf("SampleThreadFunc thread started\n");
SampleThreadLocalStorage* localStorage = (SampleThreadLocalStorage*) lsMemory;
diff --git a/examples/MultiThreading/b3PosixThreadSupport.cpp b/examples/MultiThreading/b3PosixThreadSupport.cpp
index 74cdfab..9a00d48 100644
--- a/examples/MultiThreading/b3PosixThreadSupport.cpp
+++ b/examples/MultiThreading/b3PosixThreadSupport.cpp
@@ -44,8 +44,6 @@ b3PosixThreadSupport::~b3PosixThreadSupport()
#define NAMED_SEMAPHORES
#endif
-// this semaphore will signal, if and how many threads are finished with their work
-static sem_t* mainSemaphore=0;
static sem_t* createSem(const char* baseName)
{
@@ -53,7 +51,7 @@ static sem_t* createSem(const char* baseName)
#ifdef NAMED_SEMAPHORES
/// Named semaphore begin
char name[32];
- snprintf(name, 32, "/%s-%d-%4.4d", baseName, getpid(), semCount++);
+ snprintf(name, 32, "/%8.s-%4.d-%4.4d", baseName, getpid(), semCount++);
sem_t* tempSem = sem_open(name, O_CREAT, 0600, 0);
if (tempSem != reinterpret_cast<sem_t *>(SEM_FAILED))
@@ -100,12 +98,12 @@ static void *threadFunction(void *argument)
b3Assert(status->m_status);
status->m_userThreadFunc(userPtr,status->m_lsMemory);
status->m_status = 2;
- checkPThreadFunction(sem_post(mainSemaphore));
+ checkPThreadFunction(sem_post(status->m_mainSemaphore));
status->threadUsed++;
} else {
//exit Thread
status->m_status = 3;
- checkPThreadFunction(sem_post(mainSemaphore));
+ checkPThreadFunction(sem_post(status->m_mainSemaphore));
printf("Thread with taskId %i exiting\n",status->m_taskId);
break;
}
@@ -160,13 +158,14 @@ bool b3PosixThreadSupport::isTaskCompleted(int *puiArgument0, int *puiArgument1,
b3Assert(m_activeThreadStatus.size());
// wait for any of the threads to finish
- int result = sem_trywait(mainSemaphore);
+ int result = sem_trywait(m_mainSemaphore);
if (result==0)
{
// get at least one thread which has finished
- size_t last = -1;
-
- for(size_t t=0; t < size_t(m_activeThreadStatus.size()); ++t) {
+ int last = -1;
+ int status = -1;
+ for(int t=0; t < int (m_activeThreadStatus.size()); ++t) {
+ status = m_activeThreadStatus[t].m_status;
if(2 == m_activeThreadStatus[t].m_status) {
last = t;
break;
@@ -200,7 +199,7 @@ void b3PosixThreadSupport::waitForResponse( int *puiArgument0, int *puiArgument
b3Assert(m_activeThreadStatus.size());
// wait for any of the threads to finish
- checkPThreadFunction(sem_wait(mainSemaphore));
+ checkPThreadFunction(sem_wait(m_mainSemaphore));
// get at least one thread which has finished
size_t last = -1;
@@ -231,7 +230,7 @@ void b3PosixThreadSupport::startThreads(ThreadConstructionInfo& threadConstructi
printf("%s creating %i threads.\n", __FUNCTION__, threadConstructionInfo.m_numThreads);
m_activeThreadStatus.resize(threadConstructionInfo.m_numThreads);
- mainSemaphore = createSem("main");
+ m_mainSemaphore = createSem("main");
//checkPThreadFunction(sem_wait(mainSemaphore));
for (int i=0;i < threadConstructionInfo.m_numThreads;i++)
@@ -249,6 +248,7 @@ void b3PosixThreadSupport::startThreads(ThreadConstructionInfo& threadConstructi
spuStatus.m_taskId = i;
spuStatus.m_commandId = 0;
spuStatus.m_status = 0;
+ spuStatus.m_mainSemaphore = m_mainSemaphore;
spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
spuStatus.threadUsed = 0;
@@ -271,7 +271,7 @@ void b3PosixThreadSupport::stopThreads()
spuStatus.m_userPtr = 0;
checkPThreadFunction(sem_post(spuStatus.startSemaphore));
- checkPThreadFunction(sem_wait(mainSemaphore));
+ checkPThreadFunction(sem_wait(m_mainSemaphore));
printf("destroy semaphore\n");
destroySem(spuStatus.startSemaphore);
@@ -280,7 +280,7 @@ void b3PosixThreadSupport::stopThreads()
}
printf("destroy main semaphore\n");
- destroySem(mainSemaphore);
+ destroySem(m_mainSemaphore);
printf("main semaphore destroyed\n");
m_activeThreadStatus.clear();
}
diff --git a/examples/MultiThreading/b3PosixThreadSupport.h b/examples/MultiThreading/b3PosixThreadSupport.h
index ae23dd6..3f54d4a 100644
--- a/examples/MultiThreading/b3PosixThreadSupport.h
+++ b/examples/MultiThreading/b3PosixThreadSupport.h
@@ -57,14 +57,22 @@ public:
void* m_userPtr; //for taskDesc etc
void* m_lsMemory; //initialized using PosixLocalStoreMemorySetupFunc
- pthread_t thread;
- sem_t* startSemaphore;
+ pthread_t thread;
+ //each tread will wait until this signal to start its work
+ sem_t* startSemaphore;
+ // this is a copy of m_mainSemaphore,
+ //each tread will signal once it is finished with its work
+ sem_t* m_mainSemaphore;
unsigned long threadUsed;
};
private:
b3AlignedObjectArray<b3ThreadStatus> m_activeThreadStatus;
+
+ // m_mainSemaphoresemaphore will signal, if and how many threads are finished with their work
+ sem_t* m_mainSemaphore;
+
public:
///Setup and initialize SPU/CELL/Libspe2
diff --git a/examples/MultiThreading/b3Win32ThreadSupport.cpp b/examples/MultiThreading/b3Win32ThreadSupport.cpp
index 508b893..199533b 100644
--- a/examples/MultiThreading/b3Win32ThreadSupport.cpp
+++ b/examples/MultiThreading/b3Win32ThreadSupport.cpp
@@ -223,7 +223,8 @@ bool b3Win32ThreadSupport::isTaskCompleted(int *puiArgument0, int *puiArgument1,
void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadConstructionInfo)
{
-
+ static int uniqueId = 0;
+ uniqueId++;
m_activeThreadStatus.resize(threadConstructionInfo.m_numThreads);
m_completeHandles.resize(threadConstructionInfo.m_numThreads);
@@ -244,18 +245,40 @@ void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threa
threadStatus.m_userPtr=0;
- sprintf(threadStatus.m_eventStartHandleName,"eventStart%s%d",threadConstructionInfo.m_uniqueName,i);
+ sprintf(threadStatus.m_eventStartHandleName,"es%.8s%d%d",threadConstructionInfo.m_uniqueName,uniqueId,i);
threadStatus.m_eventStartHandle = CreateEventA (0,false,false,threadStatus.m_eventStartHandleName);
- sprintf(threadStatus.m_eventCompletetHandleName,"eventComplete%s%d",threadConstructionInfo.m_uniqueName,i);
+ sprintf(threadStatus.m_eventCompletetHandleName,"ec%.8s%d%d",threadConstructionInfo.m_uniqueName,uniqueId,i);
threadStatus.m_eventCompletetHandle = CreateEventA (0,false,false,threadStatus.m_eventCompletetHandleName);
m_completeHandles[i] = threadStatus.m_eventCompletetHandle;
HANDLE handle = CreateThread(lpThreadAttributes,dwStackSize,lpStartAddress,lpParameter, dwCreationFlags,lpThreadId);
- //SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST);
- SetThreadPriority(handle,THREAD_PRIORITY_TIME_CRITICAL);
+ switch(threadConstructionInfo.m_priority)
+ {
+ case 0:
+ {
+ SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST);
+ break;
+ }
+ case 1:
+ {
+ SetThreadPriority(handle,THREAD_PRIORITY_TIME_CRITICAL);
+ break;
+ }
+ case 2:
+ {
+ SetThreadPriority(handle,THREAD_PRIORITY_BELOW_NORMAL);
+ break;
+ }
+
+ default:
+ {
+
+ }
+ }
+
SetThreadAffinityMask(handle, 1<<i);
threadStatus.m_taskId = i;
@@ -265,7 +288,7 @@ void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threa
threadStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
- printf("started thread %d with threadHandle %p\n",i,handle);
+ printf("started %s thread %d with threadHandle %p\n",threadConstructionInfo.m_uniqueName,i,handle);
}
diff --git a/examples/MultiThreading/b3Win32ThreadSupport.h b/examples/MultiThreading/b3Win32ThreadSupport.h
index b94722c..df9f3d7 100644
--- a/examples/MultiThreading/b3Win32ThreadSupport.h
+++ b/examples/MultiThreading/b3Win32ThreadSupport.h
@@ -74,7 +74,8 @@ public:
m_userThreadFunc(userThreadFunc),
m_lsMemoryFunc(lsMemoryFunc),
m_numThreads(numThreads),
- m_threadStackSize(threadStackSize)
+ m_threadStackSize(threadStackSize),
+ m_priority(0)
{
}
@@ -84,6 +85,7 @@ public:
b3Win32lsMemorySetupFunc m_lsMemoryFunc;
int m_numThreads;
int m_threadStackSize;
+ int m_priority;
};
diff --git a/examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp b/examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp
index 80e4e8e..d2ee979 100644
--- a/examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp
+++ b/examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp
@@ -356,7 +356,7 @@ b3Vector3 GpuRigidBodyDemo::getRayTo(int x,int y)
unsigned char* GpuRigidBodyDemo::loadImage(const char* fileName, int& width, int& height, int& n)
{
- unsigned char *data = stbi_load(fileName, &width, &height, &n, 0);
+ unsigned char *data = stbi_load(fileName, &width, &height, &n, 3);
return data;
}
diff --git a/examples/OpenGLWindow/CMakeLists.txt b/examples/OpenGLWindow/CMakeLists.txt
index 1e95d87..f7f1db5 100644
--- a/examples/OpenGLWindow/CMakeLists.txt
+++ b/examples/OpenGLWindow/CMakeLists.txt
@@ -59,7 +59,7 @@ if (BUILD_SHARED_LIBS)
else()
set (CMAKE_THREAD_PREFER_PTHREAD TRUE)
FIND_PACKAGE(Threads)
- target_link_libraries(OpenGLWindow ${CMAKE_THREAD_LIBS_INIT})
+ target_link_libraries(OpenGLWindow dl ${CMAKE_THREAD_LIBS_INIT})
endif()
endif()
diff --git a/examples/OpenGLWindow/GLInstanceGraphicsShape.h b/examples/OpenGLWindow/GLInstanceGraphicsShape.h
index 84aef93..d37eb83 100644
--- a/examples/OpenGLWindow/GLInstanceGraphicsShape.h
+++ b/examples/OpenGLWindow/GLInstanceGraphicsShape.h
@@ -16,6 +16,19 @@ struct GLInstanceGraphicsShape
b3AlignedObjectArray<int>* m_indices;
int m_numIndices;
float m_scaling[4];
+
+ GLInstanceGraphicsShape()
+ :m_vertices(0),
+ m_indices(0)
+ {
+
+ }
+
+ virtual ~GLInstanceGraphicsShape()
+ {
+ delete m_vertices;
+ delete m_indices;
+ }
};
#endif //GL_INSTANCE_GRAPHICS_SHAPE_H
diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp
index 8d2bac6..718a9fc 100644
--- a/examples/OpenGLWindow/GLInstancingRenderer.cpp
+++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp
@@ -16,10 +16,10 @@ subject to the following restrictions:
///todo: make this configurable in the gui
-bool useShadowMap=true;//false;//true;
-int shadowMapWidth=8192;
-int shadowMapHeight=8192;
-float shadowMapWorldSize=10;
+bool useShadowMap = true;// true;//false;//true;
+int shadowMapWidth= 2048;
+int shadowMapHeight= 2048;
+float shadowMapWorldSize=5;
#define MAX_POINTS_IN_BATCH 1024
#define MAX_LINES_IN_BATCH 1024
@@ -75,7 +75,7 @@ float shadowMapWorldSize=10;
static InternalDataRenderer* sData2;
GLint lineWidthRange[2]={1,1};
-static b3Vector3 gLightPos=b3MakeVector3(-5,200,-40);
+static b3Vector3 gLightPos=b3MakeVector3(-5,12,-4);
struct b3GraphicsInstance
{
@@ -134,29 +134,39 @@ extern int gShapeIndex;
-
+struct InternalTextureHandle
+{
+ GLuint m_glTexture;
+ int m_width;
+ int m_height;
+};
struct InternalDataRenderer : public GLInstanceRendererInternalData
{
- SimpleCamera m_defaultCamera;
-
+ SimpleCamera m_defaultCamera1;
+ CommonCameraInterface* m_activeCamera;
+
GLfloat m_projectionMatrix[16];
GLfloat m_viewMatrix[16];
GLuint m_defaultTexturehandle;
- b3AlignedObjectArray<GLuint> m_textureHandles;
+ b3AlignedObjectArray<InternalTextureHandle> m_textureHandles;
GLRenderToTexture* m_shadowMap;
GLuint m_shadowTexture;
+ GLuint m_renderFrameBuffer;
+
InternalDataRenderer() :
m_shadowMap(0),
- m_shadowTexture(0)
+ m_shadowTexture(0),
+ m_renderFrameBuffer(0),
+ m_activeCamera(&m_defaultCamera1)
{
//clear to zero to make it obvious if the matrix is used uninitialized
for (int i=0;i<16;i++)
@@ -253,6 +263,8 @@ GLInstancingRenderer::GLInstancingRenderer(int maxNumObjectCapacity, int maxShap
void GLInstancingRenderer::removeAllInstances()
{
+ m_data->m_totalNumInstances = 0;
+
for (int i=0;i<m_graphicsInstances.size();i++)
{
if (m_graphicsInstances[i]->m_index_vbo)
@@ -268,6 +280,7 @@ void GLInstancingRenderer::removeAllInstances()
m_graphicsInstances.clear();
}
+
GLInstancingRenderer::~GLInstancingRenderer()
{
delete m_data->m_shadowMap;
@@ -315,6 +328,20 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi
*/
}
+
+void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int srcIndex, float* position, float* orientation)
+{
+ b3Assert(srcIndex<m_data->m_totalNumInstances);
+ b3Assert(srcIndex>=0);
+ position[0] = m_data->m_instance_positions_ptr[srcIndex*4+0];
+ position[1] = m_data->m_instance_positions_ptr[srcIndex*4+1];
+ position[2] = m_data->m_instance_positions_ptr[srcIndex*4+2];
+
+ orientation[0] = m_data->m_instance_quaternion_ptr[srcIndex*4+0];
+ orientation[1] = m_data->m_instance_quaternion_ptr[srcIndex*4+1];
+ orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2];
+ orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3];
+}
void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex)
{
m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]);
@@ -332,12 +359,24 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int srcIn
m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3];
}
+void GLInstancingRenderer::writeSingleInstanceScaleToCPU(float* scale, int srcIndex)
+{
+ m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0];
+ m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1];
+ m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
+}
+void GLInstancingRenderer::writeSingleInstanceScaleToCPU(double* scale, int srcIndex)
+{
+ m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0];
+ m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1];
+ m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
+}
void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, float* orientation, int objectIndex)
{
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
- glFlush();
+ //glFlush();
char* orgBase = (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
//b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
@@ -367,36 +406,87 @@ void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, fl
orientations [objectIndex*4+3] = orientation[3];
glUnmapBuffer( GL_ARRAY_BUFFER);
- glFlush();
+ //glFlush();
}
void GLInstancingRenderer::writeTransforms()
{
- b3Assert(glGetError() ==GL_NO_ERROR);
-
-
- glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
- glFlush();
-
- b3Assert(glGetError() ==GL_NO_ERROR);
+ {
+ B3_PROFILE("b3Assert(glGetError() 1");
+ b3Assert(glGetError() ==GL_NO_ERROR);
+ }
+ {
+ B3_PROFILE("glBindBuffer");
+ glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
+ }
+ {
+ B3_PROFILE("glFlush()");
+ //without the flush, the glBufferSubData can spike to really slow (seconds slow)
+ glFlush();
+ }
- char* orgBase = (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
- if (orgBase)
{
+ B3_PROFILE("b3Assert(glGetError() 2");
+ b3Assert(glGetError() ==GL_NO_ERROR);
+ }
+
+#ifdef B3_DEBUG
+ {
+
+ //B3_PROFILE("m_data->m_totalNumInstances == totalNumInstances");
int totalNumInstances= 0;
-
for (int k=0;k<m_graphicsInstances.size();k++)
{
b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
totalNumInstances+=gfxObj->m_numGraphicsInstances;
}
+ b3Assert(m_data->m_totalNumInstances == totalNumInstances);
+ }
+#endif//B3_DEBUG
- m_data->m_totalNumInstances = totalNumInstances;
+ int POSITION_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4);
+ int ORIENTATION_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4);
+ int COLOR_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4);
+// int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);
+
+#if 1
+ {
+ // printf("m_data->m_totalNumInstances = %d\n", m_data->m_totalNumInstances);
+ {
+ B3_PROFILE("glBufferSubData pos");
+ glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes,m_data->m_totalNumInstances*sizeof(float)*4,
+ &m_data->m_instance_positions_ptr[0]);
+ }
+ {
+ B3_PROFILE("glBufferSubData orn");
+ glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE,m_data->m_totalNumInstances*sizeof(float)*4,
+ &m_data->m_instance_quaternion_ptr[0]);
+ }
+ {
+ B3_PROFILE("glBufferSubData color");
+ glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+ POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE, m_data->m_totalNumInstances*sizeof(float)*4,
+ &m_data->m_instance_colors_ptr[0]);
+ }
+ {
+ B3_PROFILE("glBufferSubData scale");
+ glBufferSubData( GL_ARRAY_BUFFER, m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE,m_data->m_totalNumInstances*sizeof(float)*3,
+ &m_data->m_instance_scale_ptr[0]);
+ }
+ }
+#else
+
+ char* orgBase = (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
+ if (orgBase)
+ {
+
+
+
+
for (int k=0;k<m_graphicsInstances.size();k++)
{
@@ -405,10 +495,7 @@ void GLInstancingRenderer::writeTransforms()
- int POSITION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
- int ORIENTATION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
- int COLOR_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
- // int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);
+
char* base = orgBase;
@@ -456,10 +543,19 @@ void GLInstancingRenderer::writeTransforms()
glUnmapBuffer( GL_ARRAY_BUFFER);
//if this glFinish is removed, the animation is not always working/blocks
//@todo: figure out why
- glFlush();
- glBindBuffer(GL_ARRAY_BUFFER, 0);//m_data->m_vbo);
+ //glFlush();
- b3Assert(glGetError() ==GL_NO_ERROR);
+#endif
+
+ {
+ B3_PROFILE("glBindBuffer 2");
+ glBindBuffer(GL_ARRAY_BUFFER, 0);//m_data->m_vbo);
+ }
+
+ {
+ B3_PROFILE("b3Assert(glGetError() 4");
+ b3Assert(glGetError() ==GL_NO_ERROR);
+ }
}
@@ -518,26 +614,72 @@ int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float*
int GLInstancingRenderer::registerTexture(const unsigned char* texels, int width, int height)
{
b3Assert(glGetError() ==GL_NO_ERROR);
-
+ glActiveTexture(GL_TEXTURE0);
int textureIndex = m_data->m_textureHandles.size();
- const GLubyte* image= (const GLubyte*)texels;
+ const GLubyte* image= (const GLubyte*)texels;
GLuint textureHandle;
glGenTextures(1,(GLuint*)&textureHandle);
glBindTexture(GL_TEXTURE_2D,textureHandle);
b3Assert(glGetError() ==GL_NO_ERROR);
- glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width,height,0,GL_RGB,GL_UNSIGNED_BYTE,image);
+ InternalTextureHandle h;
+ h.m_glTexture = textureHandle;
+ h.m_width = width;
+ h.m_height = height;
+ m_data->m_textureHandles.push_back(h);
+ updateTexture(textureIndex, texels);
+ return textureIndex;
+}
- b3Assert(glGetError() ==GL_NO_ERROR);
- glGenerateMipmap(GL_TEXTURE_2D);
- b3Assert(glGetError() ==GL_NO_ERROR);
+void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned char* texels)
+{
+ if (textureIndex>=0)
+ {
+
+
+ glActiveTexture(GL_TEXTURE0);
+ b3Assert(glGetError() ==GL_NO_ERROR);
+ InternalTextureHandle& h = m_data->m_textureHandles[textureIndex];
- m_data->m_textureHandles.push_back(textureHandle);
- return textureIndex;
+ //textures need to be flipped for OpenGL...
+ b3AlignedObjectArray<unsigned char> flippedTexels;
+ flippedTexels.resize(h.m_width* h.m_height * 3);
+ for (int i = 0; i < h.m_width; i++)
+ {
+ for (int j = 0; j < h.m_height; j++)
+ {
+ flippedTexels[(i + j*h.m_width) * 3] = texels[(i + (h.m_height - 1 -j )*h.m_width) * 3];
+ flippedTexels[(i + j*h.m_width) * 3+1] = texels[(i + (h.m_height - 1 - j)*h.m_width) * 3+1];
+ flippedTexels[(i + j*h.m_width) * 3+2] = texels[(i + (h.m_height - 1 - j)*h.m_width) * 3+2];
+ }
+ }
+
+
+ glBindTexture(GL_TEXTURE_2D,h.m_glTexture);
+ b3Assert(glGetError() ==GL_NO_ERROR);
+ const GLubyte* image= (const GLubyte*)texels;
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, h.m_width,h.m_height,0,GL_RGB,GL_UNSIGNED_BYTE,&flippedTexels[0]);
+ b3Assert(glGetError() ==GL_NO_ERROR);
+ glGenerateMipmap(GL_TEXTURE_2D);
+ b3Assert(glGetError() ==GL_NO_ERROR);
+ }
+}
+
+void GLInstancingRenderer::activateTexture(int textureIndex)
+{
+ glActiveTexture(GL_TEXTURE0);
+
+ if (textureIndex>=0)
+ {
+ glBindTexture(GL_TEXTURE_2D,m_data->m_textureHandles[textureIndex].m_glTexture);
+ } else
+ {
+ glBindTexture(GL_TEXTURE_2D,0);
+ }
}
void GLInstancingRenderer::updateShape(int shapeIndex, const float* vertices)
@@ -559,7 +701,7 @@ int GLInstancingRenderer::registerShape(const float* vertices, int numvertices,
if (textureId>=0)
{
- gfxObj->m_texturehandle = m_data->m_textureHandles[textureId];
+ gfxObj->m_texturehandle = m_data->m_textureHandles[textureId].m_glTexture;
}
gfxObj->m_primitiveType = primitiveType;
@@ -580,9 +722,13 @@ int GLInstancingRenderer::registerShape(const float* vertices, int numvertices,
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
- char* dest= (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_WRITE_ONLY);//GL_WRITE_ONLY
int vertexStrideInBytes = 9*sizeof(float);
int sz = numvertices*vertexStrideInBytes;
+#if 0
+
+ char* dest= (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_WRITE_ONLY);//GL_WRITE_ONLY
+
+
#ifdef B3_DEBUG
int totalUsed = vertexStrideInBytes*gfxObj->m_vertexArrayOffset+sz;
b3Assert(totalUsed<m_data->m_maxShapeCapacityInBytes);
@@ -590,6 +736,10 @@ int GLInstancingRenderer::registerShape(const float* vertices, int numvertices,
memcpy(dest+vertexStrideInBytes*gfxObj->m_vertexArrayOffset,vertices,sz);
glUnmapBuffer( GL_ARRAY_BUFFER);
+#else
+ glBufferSubData( GL_ARRAY_BUFFER,vertexStrideInBytes*gfxObj->m_vertexArrayOffset,sz,
+ vertices);
+#endif
glGenBuffers(1, &gfxObj->m_index_vbo);
@@ -828,18 +978,18 @@ void GLInstancingRenderer::resize(int width, int height)
const CommonCameraInterface* GLInstancingRenderer::getActiveCamera() const
{
- return &m_data->m_defaultCamera;
+ return m_data->m_activeCamera;
}
CommonCameraInterface* GLInstancingRenderer::getActiveCamera()
{
- return &m_data->m_defaultCamera;
+ return m_data->m_activeCamera;
}
void GLInstancingRenderer::setActiveCamera(CommonCameraInterface* cam)
{
- b3Assert(0);//not supported yet
+ m_data->m_activeCamera = cam;
}
@@ -860,14 +1010,12 @@ void GLInstancingRenderer::updateCamera(int upAxis)
b3Assert(0);
};
- m_data->m_defaultCamera.setCameraUpAxis(upAxis);
- m_data->m_defaultCamera.setAspectRatio((float)m_screenWidth/(float)m_screenHeight);
- m_data->m_defaultCamera.update();
+ m_data->m_activeCamera->setCameraUpAxis(upAxis);
+ m_data->m_activeCamera->setAspectRatio((float)m_screenWidth/(float)m_screenHeight);
+ m_data->m_defaultCamera1.update();
+ m_data->m_activeCamera->getCameraProjectionMatrix(m_data->m_projectionMatrix);
+ m_data->m_activeCamera->getCameraViewMatrix(m_data->m_viewMatrix);
- m_data->m_defaultCamera.getCameraProjectionMatrix(m_data->m_projectionMatrix);
- m_data->m_defaultCamera.getCameraViewMatrix(m_data->m_viewMatrix);
-
-
}
@@ -946,13 +1094,13 @@ void GLInstancingRenderer::renderScene()
//avoid some Intel driver on a Macbook Pro to lock-up
//todo: figure out what is going on on that machine
- glFlush();
+ //glFlush();
if (useShadowMap)
{
renderSceneInternal(B3_CREATE_SHADOWMAP_RENDERMODE);
- // glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
+ //glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE);
} else
@@ -1021,6 +1169,9 @@ void GLInstancingRenderer::drawPoints(const float* positions, const float color[
void GLInstancingRenderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float lineWidthIn)
{
+ glActiveTexture(GL_TEXTURE0);
+ glBindTexture(GL_TEXTURE_2D,0);
+
float lineWidth = lineWidthIn;
b3Clamp(lineWidth,(float)lineWidthRange[0],(float)lineWidthRange[1]);
glLineWidth(lineWidth);
@@ -1360,6 +1511,8 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+
+
float l_ClampColor[] = {1.0, 1.0, 1.0, 1.0};
glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, l_ClampColor);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
@@ -1381,7 +1534,7 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
// m_data->m_shadowMap->disable();
// return;
glEnable(GL_CULL_FACE);
- glCullFace(GL_BACK); // Cull back-facing triangles -> draw only front-facing triangles
+ glCullFace(GL_FRONT); // Cull back-facing triangles -> draw only front-facing triangles
b3Assert(glGetError() ==GL_NO_ERROR);
} else
@@ -1395,7 +1548,7 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
float depthViewMatrix[4][4];
b3Vector3 center = b3MakeVector3(0,0,0);
float upf[3];
- m_data->m_defaultCamera.getCameraUpVector(upf);
+ m_data->m_activeCamera->getCameraUpVector(upf);
b3Vector3 up = b3MakeVector3(upf[0],upf[1],upf[2]);
b3CreateLookAt(gLightPos,center,up,&depthViewMatrix[0][0]);
//b3CreateLookAt(lightPos,m_data->m_cameraTargetPosition,b3Vector3(0,1,0),(float*)depthModelViewMatrix2);
@@ -1432,6 +1585,9 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
{
B3_PROFILE("updateCamera");
// updateCamera();
+ m_data->m_activeCamera->getCameraProjectionMatrix(m_data->m_projectionMatrix);
+ m_data->m_activeCamera->getCameraViewMatrix(m_data->m_viewMatrix);
+
}
b3Assert(glGetError() ==GL_NO_ERROR);
@@ -1442,7 +1598,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
B3_PROFILE("glFlush2");
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
- glFlush();
+ //glFlush();
}
b3Assert(glGetError() ==GL_NO_ERROR);
@@ -1456,7 +1612,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
int curOffset = 0;
//GLuint lastBindTexture = 0;
-
+
for (int i=0;i<m_graphicsInstances.size();i++)
{
@@ -1580,6 +1736,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
printf("gfxObj->m_numGraphicsInstances=%d\n",gfxObj->m_numGraphicsInstances);
printf("indexCount=%d\n",indexCount);
*/
+
glUseProgram(createShadowMapInstancingShader);
glUniformMatrix4fv(createShadow_depthMVP, 1, false, &depthMVP[0][0]);
glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
@@ -1612,7 +1769,8 @@ b3Assert(glGetError() ==GL_NO_ERROR);
{
glDisable (GL_BLEND);
}
-
+ glActiveTexture(GL_TEXTURE0);
+ glBindTexture(GL_TEXTURE_2D,0);
break;
}
default:
@@ -1631,12 +1789,13 @@ b3Assert(glGetError() ==GL_NO_ERROR);
{
B3_PROFILE("glFlush");
- glFlush();
+ //glFlush();
}
if (renderMode==B3_CREATE_SHADOWMAP_RENDERMODE)
{
// writeTextureToPng(shadowMapWidth,shadowMapHeight,"shadowmap.png",4);
m_data->m_shadowMap->disable();
+ glBindFramebuffer( GL_FRAMEBUFFER, m_data->m_renderFrameBuffer);
glViewport(dims[0],dims[1],dims[2],dims[3]);
}
@@ -1682,4 +1841,16 @@ int GLInstancingRenderer::getInstanceCapacity() const
{
return m_data->m_maxNumObjectCapacity;
}
+
+void GLInstancingRenderer::setRenderFrameBuffer(unsigned int renderFrameBuffer)
+{
+ m_data->m_renderFrameBuffer = (GLuint) renderFrameBuffer;
+}
+
+int GLInstancingRenderer::getTotalNumInstances() const
+{
+ return m_data->m_totalNumInstances;
+}
+
+
#endif //NO_OPENGL3
diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h
index abdadb9..9fae49d 100644
--- a/examples/OpenGLWindow/GLInstancingRenderer.h
+++ b/examples/OpenGLWindow/GLInstancingRenderer.h
@@ -41,7 +41,7 @@ class GLInstancingRenderer : public CommonRenderInterface
int m_upAxis;
bool m_enableBlend;
- void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE);
+
@@ -52,17 +52,21 @@ public:
virtual void init();
virtual void renderScene();
+ virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE);
void InitShaders();
void CleanupShaders();
virtual void removeAllInstances();
-
+
virtual void updateShape(int shapeIndex, const float* vertices);
///vertices must be in the format x,y,z, nx,ny,nz, u,v
virtual int registerShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType=B3_GL_TRIANGLES, int textureIndex=-1);
virtual int registerTexture(const unsigned char* texels, int width, int height);
+ virtual void updateTexture(int textureIndex, const unsigned char* texels);
+ virtual void activateTexture(int textureIndex);
+
///position x,y,z, quaternion x,y,z,w, color r,g,b,a, scaling x,y,z
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
@@ -70,6 +74,7 @@ public:
void writeTransforms();
+
virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex);
virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)
{
@@ -87,11 +92,16 @@ public:
}
+ virtual void readSingleInstanceTransformFromCPU(int srcIndex, float* position, float* orientation);
+
virtual void writeSingleInstanceTransformToGPU(float* position, float* orientation, int srcIndex);
virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);
virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex);
+ virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex);
+ virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex);
+
virtual struct GLInstanceRendererInternalData* getInternalData();
@@ -122,6 +132,8 @@ public:
virtual int getInstanceCapacity() const;
+ virtual int getTotalNumInstances() const;
+
virtual void enableShadowMap();
virtual void enableBlend(bool blend)
{
@@ -129,6 +141,7 @@ public:
}
virtual void clearZBuffer();
+ virtual void setRenderFrameBuffer(unsigned int renderFrameBuffer);
};
#endif //GL_INSTANCING_RENDERER_H
diff --git a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp
index 6032851..3140668 100644
--- a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp
+++ b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp
@@ -1,7 +1,7 @@
#ifndef NO_OPENGL3
#include "GLPrimitiveRenderer.h"
#include "GLPrimInternalData.h"
-
+//#include "Bullet3Common/b3Logging.h"
#include "LoadShader.h"
#include <assert.h>
@@ -205,7 +205,8 @@ void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float
void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVertex& v1,const PrimVertex& v2,const PrimVertex& v3,float viewMat[16],float projMat[16], bool useRGBA)
{
-
+ //B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D");
+
assert(glGetError()==GL_NO_ERROR);
@@ -222,7 +223,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
bool useFiltering = false;
if (useFiltering)
{
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
} else
{
diff --git a/examples/OpenGLWindow/MacOpenGLWindow.h b/examples/OpenGLWindow/MacOpenGLWindow.h
index f17102a..d84b031 100644
--- a/examples/OpenGLWindow/MacOpenGLWindow.h
+++ b/examples/OpenGLWindow/MacOpenGLWindow.h
@@ -101,6 +101,9 @@ public:
virtual float getTimeInSeconds();
+ virtual int getWidth() const;
+ virtual int getHeight() const;
+
virtual void setRenderCallback( b3RenderCallback renderCallback);
diff --git a/examples/OpenGLWindow/MacOpenGLWindow.mm b/examples/OpenGLWindow/MacOpenGLWindow.mm
index 8187269..f4f8538 100644
--- a/examples/OpenGLWindow/MacOpenGLWindow.mm
+++ b/examples/OpenGLWindow/MacOpenGLWindow.mm
@@ -102,11 +102,11 @@ float loop;
}
-(void)drawRect:(NSRect)rect
{
+
if (([self frame].size.width != m_lastWidth) || ([self frame].size.height != m_lastHeight))
{
m_lastWidth = [self frame].size.width;
m_lastHeight = [self frame].size.height;
-
// Only needed on resize:
[m_context clearDrawable];
@@ -114,7 +114,6 @@ float loop;
float width = [self frame].size.width;
float height = [self frame].size.height;
-
// Get view dimensions in pixels
// glViewport(0,0,10,10);
@@ -209,16 +208,12 @@ struct MacOpenGLWindowInternalData
m_myview = 0;
m_pool = 0;
m_window = 0;
- m_width = -1;
- m_height = -1;
m_exitRequested = false;
}
NSApplication* m_myApp;
TestView* m_myview;
NSAutoreleasePool* m_pool;
NSWindow* m_window;
- int m_width;
- int m_height;
bool m_exitRequested;
};
@@ -294,8 +289,6 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
if (m_internalData)
closeWindow();
- int width = ci.m_width;
- int height = ci.m_height;
const char* windowTitle = ci.m_title;
@@ -303,9 +296,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
m_internalData = new MacOpenGLWindowInternalData;
- m_internalData->m_width = width;
- m_internalData->m_height = height;
-
+
m_internalData->m_pool = [NSAutoreleasePool new];
m_internalData->m_myApp = [NSApplication sharedApplication];
//myApp = [MyApp sharedApplication];
@@ -373,7 +364,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
[newItem release];
*/
- NSRect frame = NSMakeRect(0., 0., width, height);
+ NSRect frame = NSMakeRect(0., 0., ci.m_width, ci.m_height);
m_internalData->m_window = [NSWindow alloc];
[m_internalData->m_window initWithContentRect:frame
@@ -423,9 +414,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
[m_internalData->m_window makeKeyAndOrderFront: nil];
[m_internalData->m_myview MakeCurrent];
- //m_internalData->m_width = m_internalData->m_myview.GetWindowWidth;
- //m_internalData->m_height = m_internalData->m_myview.GetWindowHeight;
-
+
[NSApp activateIgnoringOtherApps:YES];
@@ -1035,13 +1024,13 @@ void MacOpenGLWindow::startRendering()
float aspect;
//b3Vector3 extents;
- if (m_internalData->m_width > m_internalData->m_height)
+ if (getWidth() > getHeight())
{
- aspect = (float)m_internalData->m_width / (float)m_internalData->m_height;
+ aspect = (float)getWidth() / (float)getHeight();
//extents.setValue(aspect * 1.0f, 1.0f,0);
} else
{
- aspect = (float)m_internalData->m_height / (float)m_internalData->m_width;
+ aspect = (float)getHeight() / (float)getWidth();
//extents.setValue(1.0f, aspect*1.f,0);
}
@@ -1082,7 +1071,7 @@ int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
NSOpenGLContext *foo = [NSOpenGLContext currentContext];
// get the url of a .txt file
NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel];
- NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",nil];
+ NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",@"stl",nil];
[zOpenPanel setAllowedFileTypes:zAryOfExtensions];
NSInteger zIntResult = [zOpenPanel runModal];
@@ -1132,12 +1121,28 @@ void MacOpenGLWindow::getMouseCoordinates(int& x, int& y)
}
+int MacOpenGLWindow::getWidth() const
+{
+ if (m_internalData && m_internalData->m_myview && m_internalData->m_myview.GetWindowWidth)
+ return m_internalData->m_myview.GetWindowWidth;
+
+ return 0;
+}
+
+int MacOpenGLWindow::getHeight() const
+{
+ if (m_internalData && m_internalData->m_myview && m_internalData->m_myview.GetWindowHeight)
+ return m_internalData->m_myview.GetWindowHeight;
+ return 0;
+}
+
+
void MacOpenGLWindow::setResizeCallback(b3ResizeCallback resizeCallback)
{
[m_internalData->m_myview setResizeCallback:resizeCallback];
if (resizeCallback)
{
- (resizeCallback)(m_internalData->m_width,m_internalData->m_height);
+ (resizeCallback)(getWidth(), getHeight());
}
}
diff --git a/examples/OpenGLWindow/Shaders/instancingPS.glsl b/examples/OpenGLWindow/Shaders/instancingPS.glsl
index fd992e5..310f5fe 100644
--- a/examples/OpenGLWindow/Shaders/instancingPS.glsl
+++ b/examples/OpenGLWindow/Shaders/instancingPS.glsl
@@ -25,7 +25,8 @@ void main(void)
vec4 texel = fragment.color*texture(Diffuse,vert.texcoord);//fragment.color;
vec3 ct,cf;
float intensity,at,af;
- intensity = max(dot(lightDir,normalize(normal)),0);
+
+ intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );
cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;
af = 1.0;
diff --git a/examples/OpenGLWindow/Shaders/instancingPS.h b/examples/OpenGLWindow/Shaders/instancingPS.h
index a32e7b2..8e2ed9f 100644
--- a/examples/OpenGLWindow/Shaders/instancingPS.h
+++ b/examples/OpenGLWindow/Shaders/instancingPS.h
@@ -22,7 +22,8 @@ static const char* instancingFragmentShader= \
" vec4 texel = fragment.color*texture(Diffuse,vert.texcoord);//fragment.color;\n"
" vec3 ct,cf;\n"
" float intensity,at,af;\n"
-" intensity = max(dot(lightDir,normalize(normal)),0);\n"
+" \n"
+" intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );\n"
" cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;\n"
" af = 1.0;\n"
" \n"
diff --git a/examples/OpenGLWindow/Shaders/instancingVS.glsl b/examples/OpenGLWindow/Shaders/instancingVS.glsl
index 1663f36..709c07a 100644
--- a/examples/OpenGLWindow/Shaders/instancingVS.glsl
+++ b/examples/OpenGLWindow/Shaders/instancingVS.glsl
@@ -61,7 +61,7 @@ out vec3 lightDir,normal,ambient;
void main(void)
{
vec4 q = instance_quaternion;
- ambient = vec3(0.6,.6,0.6);
+ ambient = vec3(0.5,.5,0.5);
vec4 worldNormal = (quatRotate3( vertexnormal,q));
normal = normalize(worldNormal).xyz;
diff --git a/examples/OpenGLWindow/Shaders/instancingVS.h b/examples/OpenGLWindow/Shaders/instancingVS.h
index 563d221..8be2f54 100644
--- a/examples/OpenGLWindow/Shaders/instancingVS.h
+++ b/examples/OpenGLWindow/Shaders/instancingVS.h
@@ -52,7 +52,7 @@ static const char* instancingVertexShader= \
"void main(void)\n"
"{\n"
" vec4 q = instance_quaternion;\n"
-" ambient = vec3(0.6,.6,0.6);\n"
+" ambient = vec3(0.5,.5,0.5);\n"
" \n"
" vec4 worldNormal = (quatRotate3( vertexnormal,q));\n"
" normal = normalize(worldNormal).xyz;\n"
diff --git a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl b/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl
index cd780fb..094ae76 100644
--- a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl
+++ b/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl
@@ -27,8 +27,7 @@ void main(void)
vec3 ct,cf;
float intensity,at,af;
- intensity = clamp( dot( normalize(normal),lightDir ), 0,1 );
-
+ intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );
af = 1.0;
@@ -37,11 +36,9 @@ void main(void)
//float bias = 0.005f;
- float bias = 0.0001*tan(acos(intensity));
- bias = clamp(bias, 0,0.01);
-
+
- float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z-bias)/ShadowCoord.w));
+ float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));
intensity = 0.7*intensity + 0.3*intensity*visibility;
diff --git a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h b/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h
index 0550ff5..f7f2a6a 100644
--- a/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h
+++ b/examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h
@@ -21,8 +21,7 @@ static const char* useShadowMapInstancingFragmentShader= \
" vec3 ct,cf;\n"
" float intensity,at,af;\n"
" \n"
-" intensity = clamp( dot( normalize(normal),lightDir ), 0,1 );\n"
-" \n"
+" intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );\n"
" \n"
" af = 1.0;\n"
" \n"
@@ -31,9 +30,8 @@ static const char* useShadowMapInstancingFragmentShader= \
" \n"
" //float bias = 0.005f;\n"
" \n"
-" float bias = 0.0001*tan(acos(intensity));\n"
-" bias = clamp(bias, 0,0.01);\n"
-" float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z-bias)/ShadowCoord.w));\n"
+" \n"
+" float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));\n"
" \n"
" intensity = 0.7*intensity + 0.3*intensity*visibility;\n"
" \n"
diff --git a/examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.glsl b/examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.glsl
index 9de5c0a..222e73c 100644
--- a/examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.glsl
+++ b/examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.glsl
@@ -65,7 +65,7 @@ out vec3 lightDir,normal,ambient;
void main(void)
{
vec4 q = instance_quaternion;
- ambient = vec3(0.6,.6,0.6);
+ ambient = vec3(0.5,.5,0.5);
vec4 worldNormal = (quatRotate3( vertexnormal,q));
diff --git a/examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.h b/examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.h
index e52ec62..a98e44f 100644
--- a/examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.h
+++ b/examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.h
@@ -55,7 +55,7 @@ static const char* useShadowMapInstancingVertexShader= \
"void main(void)\n"
"{\n"
" vec4 q = instance_quaternion;\n"
-" ambient = vec3(0.6,.6,0.6);\n"
+" ambient = vec3(0.5,.5,0.5);\n"
" \n"
" vec4 worldNormal = (quatRotate3( vertexnormal,q));\n"
" \n"
diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp
index 60554f5..f0ba375 100644
--- a/examples/OpenGLWindow/SimpleCamera.cpp
+++ b/examples/OpenGLWindow/SimpleCamera.cpp
@@ -3,8 +3,10 @@
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Matrix3x3.h"
+#include "Bullet3Common/b3Transform.h"
-struct SimpleCameraInternalData
+
+B3_ATTRIBUTE_ALIGNED16(struct) SimpleCameraInternalData
{
SimpleCameraInternalData()
:m_cameraTargetPosition(b3MakeVector3(0,0,0)),
@@ -16,10 +18,18 @@ struct SimpleCameraInternalData
m_pitch(0),
m_aspect(1),
m_frustumZNear(0.01),
- m_frustumZFar(1000)
+ m_frustumZFar(1000),
+ m_enableVR(false)
{
+ b3Transform tr;
+ tr.setIdentity();
+ tr.getOpenGLMatrix(m_offsetTransformVR);
}
- b3Vector3 m_cameraTargetPosition;
+
+ B3_DECLARE_ALIGNED_ALLOCATOR();
+
+ B3_ATTRIBUTE_ALIGNED16(float) m_offsetTransformVR[16];
+ b3Vector3 m_cameraTargetPosition;
float m_cameraDistance;
b3Vector3 m_cameraUp;
b3Vector3 m_cameraForward;
@@ -32,6 +42,11 @@ struct SimpleCameraInternalData
float m_aspect;
float m_frustumZNear;
float m_frustumZFar;
+
+ bool m_enableVR;
+ float m_viewMatrixVR[16];
+ float m_projectionMatrixVR[16];
+
};
@@ -46,7 +61,25 @@ SimpleCamera::~SimpleCamera()
delete m_data;
}
+void SimpleCamera::setVRCamera(const float viewMat[16], const float projectionMatrix[16])
+{
+ m_data->m_enableVR = true;
+ for (int i=0;i<16;i++)
+ {
+ m_data->m_viewMatrixVR[i] = viewMat[i];
+ m_data->m_projectionMatrixVR[i] = projectionMatrix[i];
+ }
+}
+void SimpleCamera::disableVRCamera()
+{
+ m_data->m_enableVR = false;
+}
+
+bool SimpleCamera::isVRCamera() const
+{
+ return m_data->m_enableVR ;
+}
static void b3CreateFrustum(
@@ -171,7 +204,7 @@ void SimpleCamera::update()
break;
default:
{
- b3Assert(0);
+ //b3Assert(0);
return;
}
};
@@ -209,11 +242,42 @@ void SimpleCamera::update()
void SimpleCamera::getCameraProjectionMatrix(float projectionMatrix[16]) const
{
- b3CreateFrustum(-m_data->m_aspect * m_data->m_frustumZNear, m_data->m_aspect * m_data->m_frustumZNear, -m_data->m_frustumZNear,m_data->m_frustumZNear, m_data->m_frustumZNear, m_data->m_frustumZFar,projectionMatrix);
+ if (m_data->m_enableVR)
+ {
+ for (int i=0;i<16;i++)
+ {
+ projectionMatrix[i] = m_data->m_projectionMatrixVR[i];
+ }
+ } else
+ {
+ b3CreateFrustum(-m_data->m_aspect * m_data->m_frustumZNear, m_data->m_aspect * m_data->m_frustumZNear, -m_data->m_frustumZNear,m_data->m_frustumZNear, m_data->m_frustumZNear, m_data->m_frustumZFar,projectionMatrix);
+ }
+}
+void SimpleCamera::setVRCameraOffsetTransform(const float offset[16])
+{
+ for (int i=0;i<16;i++)
+ {
+ m_data->m_offsetTransformVR[i] = offset[i];
+ }
}
void SimpleCamera::getCameraViewMatrix(float viewMatrix[16]) const
{
- b3CreateLookAt(m_data->m_cameraPosition,m_data->m_cameraTargetPosition,m_data->m_cameraUp,viewMatrix);
+ if (m_data->m_enableVR)
+ {
+ for (int i=0;i<16;i++)
+ {
+ b3Transform tr;
+ tr.setFromOpenGLMatrix(m_data->m_viewMatrixVR);
+ b3Transform shift=b3Transform::getIdentity();
+ shift.setFromOpenGLMatrix(m_data->m_offsetTransformVR);
+ tr = tr*shift;
+ tr.getOpenGLMatrix(viewMatrix);
+ //viewMatrix[i] = m_data->m_viewMatrixVR[i];
+ }
+ } else
+ {
+ b3CreateLookAt(m_data->m_cameraPosition,m_data->m_cameraTargetPosition,m_data->m_cameraUp,viewMatrix);
+ }
}
void SimpleCamera::getCameraTargetPosition(double pos[3]) const
diff --git a/examples/OpenGLWindow/SimpleCamera.h b/examples/OpenGLWindow/SimpleCamera.h
index 51a6c3c..7221f01 100644
--- a/examples/OpenGLWindow/SimpleCamera.h
+++ b/examples/OpenGLWindow/SimpleCamera.h
@@ -14,6 +14,12 @@ struct SimpleCamera : public CommonCameraInterface
virtual void getCameraProjectionMatrix(float m[16]) const;
virtual void getCameraViewMatrix(float m[16]) const;
+ virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16]);
+ virtual void setVRCameraOffsetTransform(const float offset[16]);
+ virtual void disableVRCamera();
+
+ virtual bool isVRCamera() const;
+
virtual void getCameraTargetPosition(float pos[3]) const;
virtual void getCameraPosition(float pos[3]) const;
diff --git a/examples/OpenGLWindow/SimpleOpenGL2App.h b/examples/OpenGLWindow/SimpleOpenGL2App.h
index 9cc3c9a..960dc1d 100644
--- a/examples/OpenGLWindow/SimpleOpenGL2App.h
+++ b/examples/OpenGLWindow/SimpleOpenGL2App.h
@@ -18,6 +18,7 @@ public:
virtual void swapBuffer();
virtual void drawText( const char* txt, int posX, int posY);
+ virtual void drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA){};
virtual void setBackgroundColor(float red, float green, float blue);
virtual int registerCubeShape(float halfExtentsX,float halfExtentsY, float halfExtentsZ, int textureIndex = -1, float textureScaling = 1)
{
diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
index 74c701a..050ec44 100644
--- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
+++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
@@ -64,6 +64,20 @@ void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(double* color, int src
{
}
+
+void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(float* scale, int srcIndex)
+{
+}
+void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(double* scale, int srcIndex)
+{
+}
+
+
+int SimpleOpenGL2Renderer::getTotalNumInstances() const
+{
+ return 0;
+}
+
void SimpleOpenGL2Renderer::getCameraViewMatrix(float viewMat[16]) const
{
b3Assert(0);
diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h
index b1e6448..393cc73 100644
--- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.h
+++ b/examples/OpenGLWindow/SimpleOpenGL2Renderer.h
@@ -32,6 +32,8 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);
virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex);
+ virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex);
+ virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex);
virtual void getCameraViewMatrix(float viewMat[16]) const;
virtual void getCameraProjectionMatrix(float projMat[16]) const;
@@ -50,6 +52,10 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
{
return -1;
}
+ virtual void updateTexture(int textureIndex, const unsigned char* texels) {}
+ virtual void activateTexture(int textureIndex) {}
+
+
virtual int registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling);
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
@@ -64,6 +70,8 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex);
+ virtual int getTotalNumInstances() const;
+
virtual void writeTransforms();
virtual void drawLine(const double from[4], const double to[4], const double color[4], double lineWidth);
@@ -78,6 +86,7 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
virtual void clearZBuffer();
+
virtual struct GLInstanceRendererInternalData* getInternalData()
{
return 0;
diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp
index 241ff7b..6abcd21 100644
--- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp
+++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp
@@ -56,8 +56,11 @@ static void SimpleResizeCallback( float widthf, float heightf)
{
int width = (int)widthf;
int height = (int)heightf;
- gApp->m_instancingRenderer->resize(width,height);
- gApp->m_primRenderer->setScreenSize(width,height);
+ if (gApp && gApp->m_instancingRenderer)
+ gApp->m_instancingRenderer->resize(width,height);
+
+ if (gApp && gApp->m_primRenderer)
+ gApp->m_primRenderer->setScreenSize(width,height);
}
@@ -104,7 +107,7 @@ static GLuint BindFont(const CTexFont *_Font)
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER,GL_NEAREST);
- glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,GL_NEAREST);
+ glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glBindTexture(GL_TEXTURE_2D, 0);
return TexID;
@@ -115,6 +118,7 @@ extern unsigned char OpenSansData[];
SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height, bool allowRetina)
{
gApp = this;
+
m_data = new SimpleInternalData;
m_data->m_frameDumpPngFileName = 0;
m_data->m_renderTexture = 0;
@@ -123,6 +127,7 @@ SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height, boo
m_data->m_upAxis = 1;
m_window = new b3gDefaultOpenGLWindow();
+
m_window->setAllowRetina(allowRetina);
b3gWindowConstructionInfo ci;
@@ -141,6 +146,9 @@ SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height, boo
1.f);
m_window->startRendering();
+ width = m_window->getWidth();
+ height = m_window->getHeight();
+
b3Assert(glGetError() ==GL_NO_ERROR);
#ifndef __APPLE__
@@ -160,17 +168,22 @@ SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height, boo
b3Assert(glGetError() ==GL_NO_ERROR);
- m_primRenderer = new GLPrimitiveRenderer(width,height);
m_parameterInterface = 0;
-
+
b3Assert(glGetError() ==GL_NO_ERROR);
- m_instancingRenderer = new GLInstancingRenderer(128*1024,64*1024*1024);
- m_renderer = m_instancingRenderer ;
- m_instancingRenderer->init();
- m_instancingRenderer->resize(width,height);
+ m_instancingRenderer = new GLInstancingRenderer(128*1024,128*1024*1024);
- b3Assert(glGetError() ==GL_NO_ERROR);
+ m_primRenderer = new GLPrimitiveRenderer(width,height);
+
+ m_renderer = m_instancingRenderer ;
+ m_window->setResizeCallback(SimpleResizeCallback);
+
+
+ m_instancingRenderer->init();
+ m_instancingRenderer->resize(width,height);
+ m_primRenderer->setScreenSize(width,height);
+ b3Assert(glGetError() ==GL_NO_ERROR);
m_instancingRenderer->InitShaders();
@@ -178,8 +191,7 @@ SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height, boo
m_window->setMouseButtonCallback(SimpleMouseButtonCallback);
m_window->setKeyboardCallback(SimpleKeyboardCallback);
m_window->setWheelCallback(SimpleWheelCallback);
- m_window->setResizeCallback(SimpleResizeCallback);
-
+
TwGenerateDefaultFonts();
m_data->m_fontTextureId = BindFont(g_DefaultNormalFont);
m_data->m_largeFontTextureId = BindFont(g_DefaultLargeFont);
@@ -219,7 +231,7 @@ struct sth_stash* SimpleOpenGL3App::getFontStash()
void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float worldPosY, float worldPosZ, float size1)
{
-
+ B3_PROFILE("SimpleOpenGL3App::drawText3D");
float viewMat[16];
float projMat[16];
CommonCameraInterface* cam = m_instancingRenderer->getActiveCamera();
@@ -262,6 +274,7 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world
bool measureOnly = false;
float fontSize= 32;//64;//512;//128;
+
sth_draw_text(m_data->m_fontStash,
m_data->m_droidRegular,fontSize,posX,posY,
txt,&dx, this->m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),measureOnly,m_window->getRetinaScale());
@@ -414,6 +427,18 @@ void SimpleOpenGL3App::drawText( const char* txt, int posXi, int posYi)
glDisable(GL_BLEND);
}
+
+void SimpleOpenGL3App::drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)
+{
+ glEnable(GL_BLEND);
+ glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+ m_primRenderer->drawTexturedRect(x0,y0,x1,y1,color,u0,v0,u1,v1,useRGBA);
+ glDisable(GL_BLEND);
+}
+
+
+
struct GfxVertex
{
float x,y,z,w;
@@ -627,13 +652,38 @@ void SimpleOpenGL3App::setBackgroundColor(float red, float green, float blue)
SimpleOpenGL3App::~SimpleOpenGL3App()
{
+
+ delete m_instancingRenderer;
delete m_primRenderer ;
-
+ sth_delete(m_data->m_fontStash);
+ delete m_data->m_renderCallbacks;
+ TwDeleteDefaultFonts();
m_window->closeWindow();
+
delete m_window;
delete m_data ;
}
+void SimpleOpenGL3App::getScreenPixels(unsigned char* rgbaBuffer, int bufferSizeInBytes, float* depthBuffer, int depthBufferSizeInBytes)
+{
+
+ int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
+ int height = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight();
+ if ((width*height*4) == bufferSizeInBytes)
+ {
+ glReadPixels(0,0,width, height, GL_RGBA, GL_UNSIGNED_BYTE, rgbaBuffer);
+ int glstat = glGetError();
+ b3Assert(glstat==GL_NO_ERROR);
+ }
+ if ((width*height*sizeof(float)) == depthBufferSizeInBytes)
+ {
+ glReadPixels(0,0,width, height, GL_DEPTH_COMPONENT, GL_FLOAT, depthBuffer);
+ int glstat = glGetError();
+ b3Assert(glstat==GL_NO_ERROR);
+ }
+
+}
+
//#define STB_IMAGE_WRITE_IMPLEMENTATION
#include "stb_image_write.h"
static void writeTextureToFile(int textureWidth, int textureHeight, const char* fileName, FILE* ffmpegVideo)
@@ -702,11 +752,13 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char*
void SimpleOpenGL3App::swapBuffer()
{
- m_window->endRendering();
+
if (m_data->m_frameDumpPngFileName)
{
- writeTextureToFile((int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(),
- (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(),m_data->m_frameDumpPngFileName,
+ int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
+ int height = (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight();
+ writeTextureToFile(width,
+ height,m_data->m_frameDumpPngFileName,
m_data->m_ffmpegFile);
m_data->m_renderTexture->disable();
if (m_data->m_ffmpegFile==0)
@@ -714,7 +766,8 @@ void SimpleOpenGL3App::swapBuffer()
m_data->m_frameDumpPngFileName = 0;
}
}
- m_window->startRendering();
+ m_window->endRendering();
+ m_window->startRendering();
}
// see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/
@@ -725,12 +778,15 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
char cmd[8192];
#ifdef _WIN32
- sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
- "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName);
+ sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
+ "-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
+
+ //sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
+ // "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName);
#else
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
- "-threads 0 -y -crf 0 -b 50000k -vf vflip %s", width, height, mp4FileName);
+ "-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
#endif
//sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
@@ -775,7 +831,7 @@ void SimpleOpenGL3App::dumpNextFrameToPng(const char* filename)
, 0,GL_RGBA, GL_FLOAT, 0);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.h b/examples/OpenGLWindow/SimpleOpenGL3App.h
index 57d5225..8a3f477 100644
--- a/examples/OpenGLWindow/SimpleOpenGL3App.h
+++ b/examples/OpenGLWindow/SimpleOpenGL3App.h
@@ -24,6 +24,7 @@ struct SimpleOpenGL3App : public CommonGraphicsApp
virtual void registerGrid(int xres, int yres, float color0[4], float color1[4]);
void dumpNextFrameToPng(const char* pngFilename);
void dumpFramesToVideo(const char* mp4Filename);
+ void getScreenPixels(unsigned char* rgbaBuffer, int bufferSizeInBytes, float* depthBuffer, int depthBufferSizeInBytes);
void drawGrid(DrawGridData data=DrawGridData());
virtual void setUpAxis(int axis);
@@ -32,6 +33,7 @@ struct SimpleOpenGL3App : public CommonGraphicsApp
virtual void swapBuffer();
virtual void drawText( const char* txt, int posX, int posY);
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size);
+ virtual void drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA);
struct sth_stash* getFontStash();
diff --git a/examples/OpenGLWindow/TwFonts.cpp b/examples/OpenGLWindow/TwFonts.cpp
index e5cd375..eaf351e 100644
--- a/examples/OpenGLWindow/TwFonts.cpp
+++ b/examples/OpenGLWindow/TwFonts.cpp
@@ -4883,18 +4883,33 @@ static const unsigned char s_FontFixed1[] = {
void TwGenerateDefaultFonts()
{
- g_DefaultSmallFont = TwGenerateFont(s_Font0, FONT0_BM_W, FONT0_BM_H);
- assert(g_DefaultSmallFont && g_DefaultSmallFont->m_NbCharRead==224);
+ if (g_DefaultSmallFont == 0)
+ {
+ g_DefaultSmallFont = TwGenerateFont(s_Font0, FONT0_BM_W, FONT0_BM_H);
+ assert(g_DefaultSmallFont && g_DefaultSmallFont->m_NbCharRead == 224);
+ }
- g_DefaultNormalFont = TwGenerateFont(s_Font1, FONT1_BM_W, FONT1_BM_H);
- assert(g_DefaultNormalFont && g_DefaultNormalFont->m_NbCharRead==224);
+ if (g_DefaultNormalFont == 0)
+ {
+ g_DefaultNormalFont = TwGenerateFont(s_Font1, FONT1_BM_W, FONT1_BM_H);
+ assert(g_DefaultNormalFont && g_DefaultNormalFont->m_NbCharRead == 224);
+ }
- g_DefaultNormalFontAA = TwGenerateFont(s_Font1AA, FONT1AA_BM_W, FONT1AA_BM_H);
- assert(g_DefaultNormalFontAA && g_DefaultNormalFontAA->m_NbCharRead==224);
- g_DefaultLargeFont = TwGenerateFont(s_Font2AA, FONT2AA_BM_W, FONT2AA_BM_H);
- assert(g_DefaultLargeFont && g_DefaultLargeFont->m_NbCharRead==224);
- g_DefaultFixed1Font = TwGenerateFont(s_FontFixed1, FONTFIXED1_BM_W, FONTFIXED1_BM_H);
- assert(g_DefaultFixed1Font && g_DefaultFixed1Font->m_NbCharRead==224);
+ if (g_DefaultNormalFontAA == 0)
+ {
+ g_DefaultNormalFontAA = TwGenerateFont(s_Font1AA, FONT1AA_BM_W, FONT1AA_BM_H);
+ assert(g_DefaultNormalFontAA && g_DefaultNormalFontAA->m_NbCharRead == 224);
+ }
+ if (g_DefaultLargeFont == 0)
+ {
+ g_DefaultLargeFont = TwGenerateFont(s_Font2AA, FONT2AA_BM_W, FONT2AA_BM_H);
+ assert(g_DefaultLargeFont && g_DefaultLargeFont->m_NbCharRead == 224);
+ }
+ if (g_DefaultFixed1Font == 0)
+ {
+ g_DefaultFixed1Font = TwGenerateFont(s_FontFixed1, FONTFIXED1_BM_W, FONTFIXED1_BM_H);
+ assert(g_DefaultFixed1Font && g_DefaultFixed1Font->m_NbCharRead == 224);
+ }
}
// ---------------------------------------------------------------------------
diff --git a/examples/OpenGLWindow/Win32OpenGLWindow.cpp b/examples/OpenGLWindow/Win32OpenGLWindow.cpp
index 6efb95a..0e076b9 100644
--- a/examples/OpenGLWindow/Win32OpenGLWindow.cpp
+++ b/examples/OpenGLWindow/Win32OpenGLWindow.cpp
@@ -1,3 +1,4 @@
+#ifdef _WIN32
/*
Copyright (c) 2012 Advanced Micro Devices, Inc.
@@ -179,4 +180,21 @@ int Win32OpenGLWindow::fileOpenDialog(char* fileName, int maxFileNameLength)
//return 0;
}
+int Win32OpenGLWindow::getWidth() const
+{
+ if (m_data)
+ return m_data->m_openglViewportWidth;
+ return 0;
+}
+
+int Win32OpenGLWindow::getHeight() const
+{
+ if (m_data)
+ return m_data->m_openglViewportHeight;
+ return 0;
+}
+
+
+#endif
+
diff --git a/examples/OpenGLWindow/Win32OpenGLWindow.h b/examples/OpenGLWindow/Win32OpenGLWindow.h
index bf8a0f5..901ce65 100644
--- a/examples/OpenGLWindow/Win32OpenGLWindow.h
+++ b/examples/OpenGLWindow/Win32OpenGLWindow.h
@@ -53,6 +53,9 @@ public:
virtual float getRetinaScale() const {return 1.f;}
virtual void setAllowRetina(bool /*allowRetina*/) {};
+ virtual int getWidth() const;
+ virtual int getHeight() const;
+
virtual int fileOpenDialog(char* fileName, int maxFileNameLength);
};
diff --git a/examples/OpenGLWindow/Win32Window.cpp b/examples/OpenGLWindow/Win32Window.cpp
index 245ccb7..4c8b600 100644
--- a/examples/OpenGLWindow/Win32Window.cpp
+++ b/examples/OpenGLWindow/Win32Window.cpp
@@ -1,3 +1,4 @@
+#ifdef _WIN32
/*
Copyright (c) 2012 Advanced Micro Devices, Inc.
@@ -434,11 +435,10 @@ void Win32Window::setWindowTitle(const char* titleChar)
wchar_t windowTitle[1024];
swprintf(windowTitle, 1024, L"%hs", titleChar);
- DWORD dwResult;
-
#ifdef _WIN64
SetWindowTextW(m_data->m_hWnd, windowTitle);
#else
+ DWORD dwResult;
SendMessageTimeoutW(m_data->m_hWnd, WM_SETTEXT, 0,
reinterpret_cast<LPARAM>(windowTitle),
SMTO_ABORTIFHUNG, 2000, &dwResult);
@@ -717,15 +717,6 @@ void Win32Window::runMainLoop()
void Win32Window::startRendering()
{
pumpMessage();
-
-// glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT); //clear buffers
-
- //glCullFace(GL_BACK);
- //glFrontFace(GL_CCW);
- // glEnable(GL_DEPTH_TEST);
-
-
-
}
@@ -805,5 +796,5 @@ b3WheelCallback Win32Window::getWheelCallback()
{
return m_data->m_wheelCallback;
}
-
+#endif
diff --git a/examples/OpenGLWindow/X11OpenGLWindow.cpp b/examples/OpenGLWindow/X11OpenGLWindow.cpp
index 01a07d1..789be78 100644
--- a/examples/OpenGLWindow/X11OpenGLWindow.cpp
+++ b/examples/OpenGLWindow/X11OpenGLWindow.cpp
@@ -172,6 +172,8 @@ struct InternalData2
XEvent m_xev;
GLXFBConfig m_bestFbc;
int m_modifierFlags;
+ int m_glWidth;
+ int m_glHeight;
#ifdef DYNAMIC_LOAD_X11_FUNCTIONS
//dynamically load stuff
@@ -207,6 +209,9 @@ struct InternalData2
InternalData2()
:m_dpy(0),
m_vi(0),
+ m_modifierFlags(0),
+ m_glWidth(-1),
+ m_glHeight(-1),
m_wheelCallback(0),
m_mouseMoveCallback(0),
m_mouseButtonCallback(0),
@@ -456,6 +461,7 @@ void X11OpenGLWindow::enableOpenGL()
printf( "Making context current\n" );
glXMakeCurrent( m_data->m_dpy, m_data->m_win, ctx );
+ m_data->m_glc = ctx;
} else
{
@@ -510,6 +516,9 @@ void X11OpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
m_data->m_dpy = MyXOpenDisplay(NULL);
+ m_data->m_glWidth = ci.m_width;
+ m_data->m_glHeight = ci.m_height;
+
if(m_data->m_dpy == NULL) {
printf("\n\tcannot connect to X server\n\n");
exit(0);
@@ -929,6 +938,9 @@ void X11OpenGLWindow::pumpMessage()
{
// printf("@");
// fflush(0);
+ m_data->m_glWidth = m_data->m_xev.xconfigure.width;
+ m_data->m_glHeight = m_data->m_xev.xconfigure.height;
+
if (m_data->m_resizeCallback)
{
(*m_data->m_resizeCallback)(m_data->m_xev.xconfigure.width,m_data->m_xev.xconfigure.height);
@@ -1031,6 +1043,10 @@ void X11OpenGLWindow::setMouseButtonCallback(b3MouseButtonCallback mouseCallback
void X11OpenGLWindow::setResizeCallback(b3ResizeCallback resizeCallback)
{
+ if (resizeCallback && m_data->m_glWidth>0 && m_data->m_glHeight > 0)
+ {
+ resizeCallback(m_data->m_glWidth, m_data->m_glHeight);
+ }
m_data->m_resizeCallback = resizeCallback;
}
@@ -1063,12 +1079,26 @@ b3KeyboardCallback X11OpenGLWindow::getKeyboardCallback()
return m_data->m_keyboardCallback;
}
+int X11OpenGLWindow::getWidth() const
+{
+ if (m_data)
+ return m_data->m_glWidth;
+ return 0;
+}
+int X11OpenGLWindow::getHeight() const
+{
+ if (m_data)
+ return m_data->m_glHeight;
+ return 0;
+}
+
+
#include <stdio.h>
int X11OpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
{
int len = 0;
- FILE * output = popen("zenity --file-selection --file-filter=\"*.urdf\" --file-filter=\"*.*\"","r");
+ FILE * output = popen("zenity --file-selection --file-filter=\"*.urdf\" --file-filter=\"*.sdf\" --file-filter=\"*.obj\" --file-filter=\"*.*\"","r");
if (output)
{
while( fgets(filename, maxNameLength-1, output) != NULL )
@@ -1078,7 +1108,7 @@ int X11OpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
{
filename[len-1]=0;
printf("file open (length=%d) = %s\n", len,filename);
- }
+ }
}
pclose(output);
} else
@@ -1087,5 +1117,5 @@ int X11OpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
}
MyXRaiseWindow(m_data->m_dpy, m_data->m_win);
return len;
-
+
}
diff --git a/examples/OpenGLWindow/X11OpenGLWindow.h b/examples/OpenGLWindow/X11OpenGLWindow.h
index cc28aae..fd88689 100644
--- a/examples/OpenGLWindow/X11OpenGLWindow.h
+++ b/examples/OpenGLWindow/X11OpenGLWindow.h
@@ -54,7 +54,7 @@ public:
virtual void setResizeCallback(b3ResizeCallback resizeCallback);
virtual void setWheelCallback(b3WheelCallback wheelCallback);
virtual void setKeyboardCallback( b3KeyboardCallback keyboardCallback);
-
+
virtual b3MouseMoveCallback getMouseMoveCallback();
virtual b3MouseButtonCallback getMouseButtonCallback();
virtual b3ResizeCallback getResizeCallback();
@@ -65,9 +65,14 @@ public:
virtual void setWindowTitle(const char* title);
+ virtual int getWidth() const;
+
+ virtual int getHeight() const;
+
int fileOpenDialog(char* filename, int maxNameLength);
};
+
#endif
diff --git a/examples/OpenGLWindow/opengl_fontstashcallbacks.cpp b/examples/OpenGLWindow/opengl_fontstashcallbacks.cpp
index 18db746..3b9f9fe 100644
--- a/examples/OpenGLWindow/opengl_fontstashcallbacks.cpp
+++ b/examples/OpenGLWindow/opengl_fontstashcallbacks.cpp
@@ -37,7 +37,6 @@ InternalOpenGL2RenderCallbacks::~InternalOpenGL2RenderCallbacks()
void InternalOpenGL2RenderCallbacks::display2()
{
-
assert(glGetError()==GL_NO_ERROR);
// glViewport(0,0,10,10);
@@ -127,7 +126,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly
texture->m_texels = (unsigned char*)malloc(textureWidth*textureHeight);
memset(texture->m_texels,0,textureWidth*textureHeight);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, textureWidth, textureHeight, 0, GL_RED, GL_UNSIGNED_BYTE, texture->m_texels);
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
assert(glGetError()==GL_NO_ERROR);
@@ -188,7 +187,7 @@ void InternalOpenGL2RenderCallbacks::render(sth_texture* texture)
bool useFiltering = false;
if (useFiltering)
{
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
} else
{
diff --git a/examples/RenderingExamples/CoordinateSystemDemo.cpp b/examples/RenderingExamples/CoordinateSystemDemo.cpp
index 37ecc7f..7f35af8 100644
--- a/examples/RenderingExamples/CoordinateSystemDemo.cpp
+++ b/examples/RenderingExamples/CoordinateSystemDemo.cpp
@@ -156,7 +156,7 @@ public:
}
};
-struct CommonExampleInterface* CoordinateSystemCreateFunc(struct CommonExampleOptions& options)
+CommonExampleInterface* CoordinateSystemCreateFunc(struct CommonExampleOptions& options)
{
return new CoordinateSystemDemo(options.m_guiHelper->getAppInterface());
}
diff --git a/examples/RenderingExamples/DynamicTexturedCubeDemo.cpp b/examples/RenderingExamples/DynamicTexturedCubeDemo.cpp
new file mode 100644
index 0000000..1316663
--- /dev/null
+++ b/examples/RenderingExamples/DynamicTexturedCubeDemo.cpp
@@ -0,0 +1,152 @@
+
+#include "DynamicTexturedCubeDemo.h"
+#include "Bullet3Common/b3Logging.h"
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "GwenGUISupport/GraphingTexture.h"
+
+#include "../CommonInterfaces/Common2dCanvasInterface.h"
+#include "../RenderingExamples/TimeSeriesCanvas.h"
+#include "../RenderingExamples/TimeSeriesFontData.h"
+#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "TinyVRGui.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+
+class DynamicTexturedCubeDemo : public CommonExampleInterface
+{
+ CommonGraphicsApp* m_app;
+ float m_x;
+ float m_y;
+ float m_z;
+ b3AlignedObjectArray<int> m_movingInstances;
+
+
+ TinyVRGui* m_tinyVrGUI;
+
+ enum
+ {
+ numCubesX = 1,
+ numCubesY = 1
+ };
+public:
+
+ DynamicTexturedCubeDemo(CommonGraphicsApp* app)
+ :m_app(app),
+ m_x(0),
+ m_y(0),
+ m_z(0),
+ m_tinyVrGUI(0)
+ {
+ m_app->setUpAxis(2);
+
+ {
+ b3Vector3 extents=b3MakeVector3(100,100,100);
+ extents[m_app->getUpAxis()]=1;
+
+ int xres = 20;
+ int yres = 20;
+
+ b3Vector4 color0=b3MakeVector4(0.1, 0.1, 0.5,1);
+ b3Vector4 color1=b3MakeVector4(0.6, 0.6, 0.6,1);
+ m_app->registerGrid(xres, yres, color0, color1);
+ }
+
+ ComboBoxParams comboParams;
+ comboParams.m_comboboxId = 0;
+ comboParams.m_numItems = 0;
+ comboParams.m_startItem = 0;
+ comboParams.m_callback = 0;//MyComboBoxCallback;
+ comboParams.m_userPointer = 0;//this;
+
+
+ m_tinyVrGUI = new TinyVRGui(comboParams,m_app->m_renderer);
+ m_tinyVrGUI->init();
+
+ m_app->m_renderer->writeTransforms();
+ }
+ virtual ~DynamicTexturedCubeDemo()
+ {
+ delete m_tinyVrGUI;
+ m_app->m_renderer->enableBlend(false);
+ }
+
+
+ virtual void physicsDebugDraw(int debugDrawMode)
+ {
+
+ }
+ virtual void initPhysics()
+ {
+ }
+ virtual void exitPhysics()
+ {
+
+ }
+
+ virtual void stepSimulation(float deltaTime)
+ {
+ static b3Transform tr = b3Transform::getIdentity();
+ static b3Scalar t = 0.f;
+ t+=deltaTime;
+ tr.setOrigin(b3MakeVector3(0.,0.,2.)+b3MakeVector3(0.,0.,0.02*b3Sin(t)));
+
+ m_tinyVrGUI->tick(deltaTime,tr);
+
+ m_app->m_renderer->writeTransforms();
+
+ }
+ virtual void renderScene()
+ {
+
+ m_app->m_renderer->renderScene();
+ }
+
+
+ virtual void physicsDebugDraw()
+ {
+
+ }
+ virtual bool mouseMoveCallback(float x,float y)
+ {
+ return false;
+ }
+ virtual bool mouseButtonCallback(int button, int state, float x, float y)
+ {
+ return false;
+ }
+ virtual bool keyboardCallback(int key, int state)
+ {
+ return false;
+ }
+
+ virtual void resetCamera()
+ {
+ float dist = 1.15;
+ float pitch = 396;
+ float yaw = 33.7;
+ float targetPos[3]={-0.5,0.7,1.45};
+ if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
+ {
+ m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
+ m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
+ m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
+ m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
+ }
+ }
+
+};
+
+
+class CommonExampleInterface* DynamicTexturedCubeDemoCreateFunc(struct CommonExampleOptions& options)
+{
+ return new DynamicTexturedCubeDemo(options.m_guiHelper->getAppInterface());
+}
+
+
+
+
diff --git a/examples/RenderingExamples/DynamicTexturedCubeDemo.h b/examples/RenderingExamples/DynamicTexturedCubeDemo.h
new file mode 100644
index 0000000..a00185b
--- /dev/null
+++ b/examples/RenderingExamples/DynamicTexturedCubeDemo.h
@@ -0,0 +1,6 @@
+#ifndef DYNAMIC_TEXTURED_CUBE_DEMO_H
+#define DYNAMIC_TEXTURED_CUBE_DEMO_H
+
+class CommonExampleInterface* DynamicTexturedCubeDemoCreateFunc(struct CommonExampleOptions& options);
+
+#endif //DYNAMIC_TEXTURED_CUBE_DEMO_H
diff --git a/examples/RenderingExamples/RaytracerSetup.cpp b/examples/RenderingExamples/RaytracerSetup.cpp
index 2aba3f3..2996e8b 100644
--- a/examples/RenderingExamples/RaytracerSetup.cpp
+++ b/examples/RenderingExamples/RaytracerSetup.cpp
@@ -260,12 +260,22 @@ void RaytracerPhysicsSetup::stepSimulation(float deltaTime)
float fov = 2.0 * atanf (tanFov);
+
btVector3 cameraPosition(5,0,0);
btVector3 cameraTargetPosition(0,0,0);
- btVector3 rayFrom = cameraPosition;
- btVector3 rayForward = cameraTargetPosition-cameraPosition;
- rayForward.normalize();
+
+ if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
+ {
+ m_app->m_renderer->getActiveCamera()->getCameraPosition(cameraPosition);
+ m_app->m_renderer->getActiveCamera()->getCameraTargetPosition(cameraTargetPosition);
+ }
+
+ btVector3 rayFrom = cameraPosition;
+ btVector3 rayForward = cameraTargetPosition-cameraPosition;
+
+
+ rayForward.normalize();
float farPlane = 600.f;
rayForward*= farPlane;
@@ -295,7 +305,7 @@ void RaytracerPhysicsSetup::stepSimulation(float deltaTime)
for (x=0;x<m_internalData->m_width;x++)
{
- for (int y=0;y<m_internalData->m_height;y++)
+ for (y=0;y<m_internalData->m_height;y++)
{
btVector4 rgba(0,0,0,0);
btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
diff --git a/examples/RenderingExamples/TimeSeriesCanvas.cpp b/examples/RenderingExamples/TimeSeriesCanvas.cpp
index 5bed56a..bfd2e9d 100644
--- a/examples/RenderingExamples/TimeSeriesCanvas.cpp
+++ b/examples/RenderingExamples/TimeSeriesCanvas.cpp
@@ -72,13 +72,14 @@ struct TimeSeriesInternalData
TimeSeriesCanvas::TimeSeriesCanvas(struct Common2dCanvasInterface* canvasInterface, int width, int height, const char* windowTitle)
{
- btAssert(canvasInterface);
-
m_internalData = new TimeSeriesInternalData(width,height);
m_internalData->m_canvasInterface = canvasInterface;
-
- m_internalData->m_canvasIndex = m_internalData->m_canvasInterface->createCanvas(windowTitle,m_internalData->m_width,m_internalData->m_height);
+
+ if (canvasInterface)
+ {
+ m_internalData->m_canvasIndex = m_internalData->m_canvasInterface->createCanvas(windowTitle,m_internalData->m_width,m_internalData->m_height);
+ }
}
void TimeSeriesCanvas::addDataSource(const char* dataSourceLabel, unsigned char red,unsigned char green,unsigned char blue)
@@ -103,26 +104,32 @@ void TimeSeriesCanvas::addDataSource(const char* dataSourceLabel, unsigned char
m_internalData->m_dataSources.push_back(dataSource);
}
-void TimeSeriesCanvas::setupTimeSeries(float yScale, int ticksPerSecond, int startTime)
+void TimeSeriesCanvas::setupTimeSeries(float yScale, int ticksPerSecond, int startTime, bool clearCanvas)
{
+ if (0==m_internalData->m_canvasInterface)
+ return;
+
m_internalData->m_pixelsPerUnit = -(m_internalData->m_height/3.f)/yScale;
m_internalData->m_ticksPerSecond = ticksPerSecond;
m_internalData->m_yScale = yScale;
m_internalData->m_dataSources.clear();
- for (int i=0;i<m_internalData->m_width;i++)
+
+ if (clearCanvas)
{
- for (int j=0;j<m_internalData->m_height;j++)
+ for (int i=0;i<m_internalData->m_width;i++)
{
+ for (int j=0;j<m_internalData->m_height;j++)
+ {
- m_internalData->m_canvasInterface->setPixel(m_internalData->m_canvasIndex,i,j,
- m_internalData->m_backgroundRed,
- m_internalData->m_backgroundGreen,
- m_internalData->m_backgroundBlue,
- m_internalData->m_backgroundAlpha);
+ m_internalData->m_canvasInterface->setPixel(m_internalData->m_canvasIndex,i,j,
+ m_internalData->m_backgroundRed,
+ m_internalData->m_backgroundGreen,
+ m_internalData->m_backgroundBlue,
+ m_internalData->m_backgroundAlpha);
+ }
}
}
-
float zeroPixelCoord = m_internalData->m_zero;
float pixelsPerUnit = m_internalData->m_pixelsPerUnit;
@@ -203,7 +210,7 @@ void TimeSeriesCanvas::shift1PixelToLeft()
int countdown = resetVal;
//shift pixture one pixel to the left
- for (int j=0;j<m_internalData->m_height-48;j++)
+ for (int j=50;j<m_internalData->m_height-48;j++)
{
for (int i=40;i<this->m_internalData->m_width;i++)
{
@@ -282,6 +289,9 @@ void TimeSeriesCanvas::shift1PixelToLeft()
void TimeSeriesCanvas::insertDataAtCurrentTime(float orgV, int dataSourceIndex, bool connectToPrevious)
{
+ if (0==m_internalData->m_canvasInterface)
+ return;
+
btAssert(dataSourceIndex < m_internalData->m_dataSources.size());
float zero = m_internalData->m_zero;
diff --git a/examples/RenderingExamples/TimeSeriesCanvas.h b/examples/RenderingExamples/TimeSeriesCanvas.h
index 3a42692..41f4662 100644
--- a/examples/RenderingExamples/TimeSeriesCanvas.h
+++ b/examples/RenderingExamples/TimeSeriesCanvas.h
@@ -6,17 +6,17 @@ class TimeSeriesCanvas
protected:
struct TimeSeriesInternalData* m_internalData;
void shift1PixelToLeft();
- void grapicalPrintf(const char* str, void* fontData, int rasterposx,int rasterposy,unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha);
-
+
public:
TimeSeriesCanvas(struct Common2dCanvasInterface* canvasInterface, int width, int height, const char* windowTitle);
virtual ~TimeSeriesCanvas();
- void setupTimeSeries(float yScale, int ticksPerSecond, int startTime);
+ void setupTimeSeries(float yScale, int ticksPerSecond, int startTime, bool clearCanvas=true);
void addDataSource(const char* dataSourceLabel, unsigned char red,unsigned char green,unsigned char blue);
void insertDataAtCurrentTime(float value, int dataSourceIndex, bool connectToPrevious);
float getCurrentTime() const;
+ void grapicalPrintf(const char* str, void* fontData, int rasterposx,int rasterposy,unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha);
virtual void nextTick();
diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp
new file mode 100644
index 0000000..a38b13a
--- /dev/null
+++ b/examples/RenderingExamples/TinyRendererSetup.cpp
@@ -0,0 +1,413 @@
+
+#include "RaytracerSetup.h"
+
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../TinyRenderer/TinyRenderer.h"
+#include "../CommonInterfaces/Common2dCanvasInterface.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btBulletCollisionCommon.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../ExampleBrowser/CollisionShape2TriangleMesh.h"
+#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+
+
+struct TinyRendererSetupInternalData
+{
+
+ TGAImage m_rgbColorBuffer;
+ b3AlignedObjectArray<float> m_depthBuffer;
+ b3AlignedObjectArray<int> m_segmentationMaskBuffer;
+
+
+ int m_width;
+ int m_height;
+
+ btAlignedObjectArray<btConvexShape*> m_shapePtr;
+ btAlignedObjectArray<btTransform> m_transforms;
+ btAlignedObjectArray<TinyRenderObjectData*> m_renderObjects;
+
+ btVoronoiSimplexSolver m_simplexSolver;
+ btScalar m_pitch;
+ btScalar m_roll;
+ btScalar m_yaw;
+
+ int m_textureHandle;
+ int m_animateRenderer;
+
+ TinyRendererSetupInternalData(int width, int height)
+ :m_roll(0),
+ m_pitch(0),
+ m_yaw(0),
+
+ m_width(width),
+ m_height(height),
+ m_rgbColorBuffer(width,height,TGAImage::RGB),
+ m_textureHandle(0),
+ m_animateRenderer(0)
+ {
+ m_depthBuffer.resize(m_width*m_height);
+// m_segmentationMaskBuffer.resize(m_width*m_height);
+
+ }
+ void updateTransforms()
+ {
+ int numObjects = m_shapePtr.size();
+ m_transforms.resize(numObjects);
+ for (int i=0;i<numObjects;i++)
+ {
+ m_transforms[i].setIdentity();
+ //btVector3 pos(0.f,-(2.5* numObjects * 0.5)+i*2.5f, 0.f);
+ btVector3 pos(0.f,+i*2.5f, 0.f);
+ m_transforms[i].setIdentity();
+ m_transforms[i].setOrigin( pos );
+ btQuaternion orn;
+ if (i < 2)
+ {
+ orn.setEuler(m_yaw,m_pitch,m_roll);
+ m_transforms[i].setRotation(orn);
+ }
+ }
+ if (m_animateRenderer)
+ {
+ m_pitch += 0.005f;
+ m_yaw += 0.01f;
+ }
+ }
+
+};
+
+struct TinyRendererSetup : public CommonExampleInterface
+{
+
+ struct GUIHelperInterface* m_guiHelper;
+ struct CommonGraphicsApp* m_app;
+ struct TinyRendererSetupInternalData* m_internalData;
+ bool m_useSoftware;
+
+
+ TinyRendererSetup(struct GUIHelperInterface* guiHelper);
+
+ virtual ~TinyRendererSetup();
+
+ virtual void initPhysics();
+
+ virtual void exitPhysics();
+
+ virtual void stepSimulation(float deltaTime);
+
+
+ virtual void physicsDebugDraw(int debugFlags);
+
+ virtual void syncPhysicsToGraphics(struct GraphicsPhysicsBridge& gfxBridge);
+
+ virtual bool mouseMoveCallback(float x,float y);
+
+ virtual bool mouseButtonCallback(int button, int state, float x, float y);
+
+ virtual bool keyboardCallback(int key, int state);
+
+ virtual void renderScene()
+ {
+ }
+
+ void animateRenderer(int animateRendererIndex)
+ {
+ m_internalData->m_animateRenderer = animateRendererIndex;
+ }
+
+
+ void selectRenderer(int rendererIndex)
+ {
+ m_useSoftware = (rendererIndex==0);
+ }
+
+ void resetCamera()
+ {
+ float dist = 11;
+ float pitch = 52;
+ float yaw = 35;
+ float targetPos[3]={0,0.46,0};
+ m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+ }
+
+};
+
+
+TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
+{
+ m_useSoftware = false;
+
+ m_guiHelper = gui;
+ m_app = gui->getAppInterface();
+ m_internalData = new TinyRendererSetupInternalData(gui->getAppInterface()->m_window->getWidth(),gui->getAppInterface()->m_window->getHeight());
+
+ m_app->m_renderer->enableBlend(true);
+
+ const char* fileName = "textured_sphere_smooth.obj";
+ fileName = "cube.obj";
+
+
+ {
+
+ {
+ int shapeId = -1;
+
+ b3ImportMeshData meshData;
+ if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
+ {
+ int textureIndex = -1;
+
+ if (meshData.m_textureImage)
+ {
+ textureIndex = m_guiHelper->getRenderInterface()->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight);
+ }
+
+ shapeId = m_guiHelper->getRenderInterface()->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
+ meshData.m_gfxShape->m_numvertices,
+ &meshData.m_gfxShape->m_indices->at(0),
+ meshData.m_gfxShape->m_numIndices,
+ B3_GL_TRIANGLES,
+ textureIndex);
+
+ float position[4]={0,0,0,1};
+ float orn[4]={0,0,0,1};
+ float color[4]={1,1,1,1};
+ float scaling[4]={1,1,1,1};
+
+ m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+ m_guiHelper->getRenderInterface()->writeTransforms();
+
+ m_internalData->m_shapePtr.push_back(0);
+ TinyRenderObjectData* ob = new TinyRenderObjectData(
+ m_internalData->m_rgbColorBuffer,
+ m_internalData->m_depthBuffer,
+ &m_internalData->m_segmentationMaskBuffer,
+ m_internalData->m_renderObjects.size());
+
+ meshData.m_gfxShape->m_scaling[0] = scaling[0];
+ meshData.m_gfxShape->m_scaling[1] = scaling[1];
+ meshData.m_gfxShape->m_scaling[2] = scaling[2];
+
+ const int* indices = &meshData.m_gfxShape->m_indices->at(0);
+ ob->registerMeshShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
+ meshData.m_gfxShape->m_numvertices,
+ indices,
+ meshData.m_gfxShape->m_numIndices,color, meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight);
+
+ ob->m_localScaling.setValue(scaling[0],scaling[1],scaling[2]);
+
+ m_internalData->m_renderObjects.push_back(ob);
+
+
+
+ delete meshData.m_gfxShape;
+ delete meshData.m_textureImage;
+ }
+ }
+ }
+
+
+}
+
+TinyRendererSetup::~TinyRendererSetup()
+{
+ m_app->m_renderer->enableBlend(false);
+ delete m_internalData;
+}
+
+
+const char* itemsanimate[] = {"Fixed", "Rotate"};
+void TinyRendererComboCallbackAnimate(int combobox, const char* item, void* userPointer)
+{
+ TinyRendererSetup* cl = (TinyRendererSetup*) userPointer;
+ b3Assert(cl);
+ int index=-1;
+ int numItems = sizeof(itemsanimate)/sizeof(char*);
+ for (int i=0;i<numItems;i++)
+ {
+ if (!strcmp(item,itemsanimate[i]))
+ {
+ index = i;
+ }
+ }
+ cl->animateRenderer(index);
+}
+
+
+const char* items[] = {"Software", "OpenGL"};
+
+
+void TinyRendererComboCallback(int combobox, const char* item, void* userPointer)
+{
+ TinyRendererSetup* cl = (TinyRendererSetup*) userPointer;
+ b3Assert(cl);
+ int index=-1;
+ int numItems = sizeof(items)/sizeof(char*);
+ for (int i=0;i<numItems;i++)
+ {
+ if (!strcmp(item,items[i]))
+ {
+ index = i;
+ }
+ }
+ cl->selectRenderer(index);
+}
+
+
+void TinyRendererSetup::initPhysics()
+{
+ //request a visual bitma/texture we can render to
+
+ m_app->setUpAxis(2);
+
+ CommonRenderInterface* render = m_app->m_renderer;
+
+ m_internalData->m_textureHandle = render->registerTexture(m_internalData->m_rgbColorBuffer.buffer(),m_internalData->m_width,m_internalData->m_height);
+
+ {
+ ComboBoxParams comboParams;
+ comboParams.m_userPointer = this;
+ comboParams.m_numItems=sizeof(items)/sizeof(char*);
+ comboParams.m_startItem = 1;
+ comboParams.m_items=items;
+ comboParams.m_callback =TinyRendererComboCallback;
+ m_guiHelper->getParameterInterface()->registerComboBox( comboParams);
+ }
+
+ {
+
+ ComboBoxParams comboParams;
+ comboParams.m_userPointer = this;
+ comboParams.m_numItems=sizeof(itemsanimate)/sizeof(char*);
+ comboParams.m_startItem = 0;
+ comboParams.m_items=itemsanimate;
+ comboParams.m_callback =TinyRendererComboCallbackAnimate;
+ m_guiHelper->getParameterInterface()->registerComboBox( comboParams);
+ }
+
+}
+
+
+void TinyRendererSetup::exitPhysics()
+{
+
+}
+
+
+void TinyRendererSetup::stepSimulation(float deltaTime)
+{
+ m_internalData->updateTransforms();
+
+ if (!m_useSoftware)
+ {
+
+ for (int i=0;i<m_internalData->m_transforms.size();i++)
+ {
+ m_guiHelper->getRenderInterface()->writeSingleInstanceTransformToCPU(m_internalData->m_transforms[i].getOrigin(),m_internalData->m_transforms[i].getRotation(),i);
+ }
+ m_guiHelper->getRenderInterface()->writeTransforms();
+ m_guiHelper->getRenderInterface()->renderScene();
+ } else
+ {
+
+ TGAColor clearColor;
+ clearColor.bgra[0] = 200;
+ clearColor.bgra[1] = 200;
+ clearColor.bgra[2] = 200;
+ clearColor.bgra[3] = 255;
+ for(int y=0;y<m_internalData->m_height;++y)
+ {
+ for(int x=0;x<m_internalData->m_width;++x)
+ {
+ m_internalData->m_rgbColorBuffer.set(x,y,clearColor);
+ m_internalData->m_depthBuffer[x+y*m_internalData->m_width] = -1e30f;
+ }
+ }
+
+
+ ATTRIBUTE_ALIGNED16(btScalar modelMat2[16]);
+ ATTRIBUTE_ALIGNED16(float viewMat[16]);
+ ATTRIBUTE_ALIGNED16(float projMat[16]);
+ CommonRenderInterface* render = this->m_app->m_renderer;
+ render->getActiveCamera()->getCameraViewMatrix(viewMat);
+ render->getActiveCamera()->getCameraProjectionMatrix(projMat);
+
+
+
+ for (int o=0;o<this->m_internalData->m_renderObjects.size();o++)
+ {
+
+ const btTransform& tr = m_internalData->m_transforms[o];
+ tr.getOpenGLMatrix(modelMat2);
+
+
+ for (int i=0;i<4;i++)
+ {
+ for (int j=0;j<4;j++)
+ {
+ m_internalData->m_renderObjects[o]->m_modelMatrix[i][j] = float(modelMat2[i+4*j]);
+ m_internalData->m_renderObjects[o]->m_viewMatrix[i][j] = viewMat[i+4*j];
+ m_internalData->m_renderObjects[o]->m_projectionMatrix[i][j] = projMat[i+4*j];
+
+ btVector3 lightDirWorld;
+ switch (m_app->getUpAxis())
+ {
+ case 1:
+ lightDirWorld = btVector3(-50.f,100,30);
+ break;
+ case 2:
+ lightDirWorld = btVector3(-50.f,30,100);
+ break;
+ default:{}
+ };
+
+ m_internalData->m_renderObjects[o]->m_lightDirWorld = lightDirWorld.normalized();
+
+ }
+ }
+ TinyRenderer::renderObject(*m_internalData->m_renderObjects[o]);
+ }
+ //m_app->drawText("hello",500,500);
+ render->activateTexture(m_internalData->m_textureHandle);
+ render->updateTexture(m_internalData->m_textureHandle,m_internalData->m_rgbColorBuffer.buffer());
+ float color[4] = {1,1,1,1};
+ m_app->drawTexturedRect(0,0,m_app->m_window->getWidth(), m_app->m_window->getHeight(),color,0,0,1,1,true);
+ }
+}
+
+
+void TinyRendererSetup::physicsDebugDraw(int debugDrawFlags)
+{
+}
+
+bool TinyRendererSetup::mouseMoveCallback(float x,float y)
+{
+ return false;
+}
+
+bool TinyRendererSetup::mouseButtonCallback(int button, int state, float x, float y)
+{
+ return false;
+}
+
+bool TinyRendererSetup::keyboardCallback(int key, int state)
+{
+ return false;
+}
+
+
+void TinyRendererSetup::syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge)
+{
+}
+
+ CommonExampleInterface* TinyRendererCreateFunc(struct CommonExampleOptions& options)
+ {
+ return new TinyRendererSetup(options.m_guiHelper);
+ }
diff --git a/examples/RenderingExamples/TinyRendererSetup.h b/examples/RenderingExamples/TinyRendererSetup.h
new file mode 100644
index 0000000..76d07ba
--- /dev/null
+++ b/examples/RenderingExamples/TinyRendererSetup.h
@@ -0,0 +1,6 @@
+#ifndef TINYRENDERER_SETUP_H
+#define TINYRENDERER_SETUP_H
+
+class CommonExampleInterface* TinyRendererCreateFunc(struct CommonExampleOptions& options);
+
+#endif //TINYRENDERER_SETUP_H
diff --git a/examples/RenderingExamples/TinyVRGui.cpp b/examples/RenderingExamples/TinyVRGui.cpp
new file mode 100644
index 0000000..3dfcc0e
--- /dev/null
+++ b/examples/RenderingExamples/TinyVRGui.cpp
@@ -0,0 +1,218 @@
+#include "TinyVRGui.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../ExampleBrowser/GwenGUISupport/GraphingTexture.h"
+
+#include "../CommonInterfaces/Common2dCanvasInterface.h"
+#include "../RenderingExamples/TimeSeriesCanvas.h"
+#include "../RenderingExamples/TimeSeriesFontData.h"
+#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+
+
+struct TestCanvasInterface2 : public Common2dCanvasInterface
+{
+ b3AlignedObjectArray<unsigned char>& m_texelsRGB;
+ int m_width;
+ int m_height;
+
+ TestCanvasInterface2(b3AlignedObjectArray<unsigned char>& texelsRGB, int width, int height)
+ :m_width(width),
+ m_height(height),
+ m_texelsRGB(texelsRGB)
+ {
+ }
+
+ virtual ~TestCanvasInterface2()
+ {}
+ virtual int createCanvas(const char* canvasName, int width, int height)
+ {
+ return 0;
+ }
+ virtual void destroyCanvas(int canvasId)
+ {
+ }
+ virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha)
+ {
+ if (x>=0 && x<m_width && y>=0 && y<m_height)
+ {
+ m_texelsRGB[(x+y*m_width)*3+0] = red;
+ m_texelsRGB[(x+y*m_width)*3+1] = green;
+ m_texelsRGB[(x+y*m_width)*3+2] = blue;
+ }
+ }
+ virtual void getPixel(int canvasId, int x, int y, unsigned char& red, unsigned char& green,unsigned char& blue, unsigned char& alpha)
+ {
+ if (x>=0 && x<m_width && y>=0 && y<m_height)
+ {
+ red = m_texelsRGB[(x+y*m_width)*3+0];
+ green = m_texelsRGB[(x+y*m_width)*3+1];
+ blue = m_texelsRGB[(x+y*m_width)*3+2];
+ }
+ }
+
+ virtual void refreshImageData(int canvasId)
+ {
+
+ }
+
+};
+
+
+
+struct TinyVRGuiInternalData
+{
+ CommonRenderInterface* m_renderer;
+
+ b3AlignedObjectArray<unsigned char> m_texelsRGB;
+ TestCanvasInterface2* m_testCanvas;
+ TimeSeriesCanvas* m_timeSeries;
+ int m_src;
+ int m_textureId;
+ int m_gfxObjectId;
+
+ TinyVRGuiInternalData()
+ :m_renderer(0),
+ m_testCanvas(0),
+ m_timeSeries(0),
+ m_src(-1),
+ m_textureId(-1),
+ m_gfxObjectId(-1)
+ {
+ }
+};
+
+
+TinyVRGui::TinyVRGui(struct ComboBoxParams& params, struct CommonRenderInterface* renderer)
+{
+ m_data = new TinyVRGuiInternalData;
+ m_data->m_renderer = renderer;
+
+
+}
+
+TinyVRGui::~TinyVRGui()
+{
+ delete m_data->m_timeSeries;
+ delete m_data->m_testCanvas;
+ delete m_data;
+}
+
+bool TinyVRGui::init()
+{
+ {
+
+
+ int width = 256;
+ int height = 256;
+ m_data->m_texelsRGB.resize(width*height*3);
+ for (int i=0;i<width;i++)
+ for (int j=0;j<height;j++)
+ {
+ m_data->m_texelsRGB[(i+j*width)*3+0] = 155;
+ m_data->m_texelsRGB[(i+j*width)*3+1] = 155;
+ m_data->m_texelsRGB[(i+j*width)*3+2] = 255;
+ }
+
+ m_data->m_testCanvas = new TestCanvasInterface2(m_data->m_texelsRGB,width,height);
+ m_data->m_timeSeries = new TimeSeriesCanvas(m_data->m_testCanvas,width,height,"time series");
+
+
+ bool clearCanvas = false;
+
+ m_data->m_timeSeries->setupTimeSeries(3,100, 0,clearCanvas);
+ m_data->m_timeSeries->addDataSource("Some sine wave", 255,0,0);
+ m_data->m_timeSeries->addDataSource("Some cosine wave", 0,255,0);
+ m_data->m_timeSeries->addDataSource("Delta Time (*10)", 0,0,255);
+ m_data->m_timeSeries->addDataSource("Tan", 255,0,255);
+ m_data->m_timeSeries->addDataSource("Some cosine wave2", 255,255,0);
+ m_data->m_timeSeries->addDataSource("Empty source2", 255,0,255);
+
+ m_data->m_textureId = m_data->m_renderer->registerTexture(&m_data->m_texelsRGB[0],width,height);
+
+ {
+
+ const char* fileName = "cube.obj";//"textured_sphere_smooth.obj";
+ //fileName = "cube.obj";
+
+ int shapeId = -1;
+
+ b3ImportMeshData meshData;
+ if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
+ {
+
+
+ shapeId = m_data->m_renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
+ meshData.m_gfxShape->m_numvertices,
+ &meshData.m_gfxShape->m_indices->at(0),
+ meshData.m_gfxShape->m_numIndices,
+ B3_GL_TRIANGLES,
+ m_data->m_textureId);
+
+ float position[4]={0,0,2,1};
+ float orn[4]={0,0,0,1};
+ float color[4]={1,1,1,1};
+ float scaling[4]={.1,.1,.1,1};
+
+ m_data->m_gfxObjectId = m_data->m_renderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+ m_data->m_renderer->writeTransforms();
+
+
+
+ meshData.m_gfxShape->m_scaling[0] = scaling[0];
+ meshData.m_gfxShape->m_scaling[1] = scaling[1];
+ meshData.m_gfxShape->m_scaling[2] = scaling[2];
+
+
+ delete meshData.m_gfxShape;
+ delete meshData.m_textureImage;
+ }
+
+
+
+ }
+ }
+
+ m_data->m_renderer->writeTransforms();
+ return true;
+}
+
+
+void TinyVRGui::tick(b3Scalar deltaTime, const b3Transform& guiWorldTransform)
+{
+ float time = m_data->m_timeSeries->getCurrentTime();
+ float v = sinf(time);
+ m_data->m_timeSeries->insertDataAtCurrentTime(v,0,true);
+ v = cosf(time);
+ m_data->m_timeSeries->insertDataAtCurrentTime(v,1,true);
+ v = tanf(time);
+ m_data->m_timeSeries->insertDataAtCurrentTime(v,3,true);
+ m_data->m_timeSeries->insertDataAtCurrentTime(deltaTime*10,2,true);
+
+ m_data->m_timeSeries->nextTick();
+
+ m_data->m_renderer->updateTexture(m_data->m_textureId,&m_data->m_texelsRGB[0]);
+ m_data->m_renderer->writeSingleInstanceTransformToCPU(guiWorldTransform.getOrigin(),guiWorldTransform.getRotation(),m_data->m_gfxObjectId);
+ m_data->m_renderer->writeTransforms();
+}
+
+void TinyVRGui::clearTextArea()
+{
+ int width = 256;
+ int height = 50;
+ for (int i=0;i<width;i++)
+ for (int j=0;j<height;j++)
+ {
+ m_data->m_texelsRGB[(i+j*width)*3+0] = 155;
+ m_data->m_texelsRGB[(i+j*width)*3+1] = 155;
+ m_data->m_texelsRGB[(i+j*width)*3+2] = 255;
+ }
+}
+
+void TinyVRGui::grapicalPrintf(const char* str,int rasterposx,int rasterposy,unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha)
+{
+ m_data->m_timeSeries->grapicalPrintf(str,sTimeSeriesFontData,rasterposx,rasterposy,red,green,blue,alpha);
+}
+
diff --git a/examples/RenderingExamples/TinyVRGui.h b/examples/RenderingExamples/TinyVRGui.h
new file mode 100644
index 0000000..23432e4
--- /dev/null
+++ b/examples/RenderingExamples/TinyVRGui.h
@@ -0,0 +1,25 @@
+
+#ifndef TINY_VR_GUI_H
+#define TINY_VR_GUI_H
+
+#include "Bullet3Common/b3Transform.h"
+
+class TinyVRGui
+{
+ struct TinyVRGuiInternalData* m_data;
+
+public:
+
+ TinyVRGui(struct ComboBoxParams& params, struct CommonRenderInterface* renderer);
+ virtual ~TinyVRGui();
+
+ bool init();
+ void tick(b3Scalar deltaTime, const b3Transform& guiWorldTransform);
+
+ void clearTextArea();
+ void grapicalPrintf(const char* str,int rasterposx,int rasterposy,unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha);
+
+};
+
+
+#endif //TINY_VR_GUI_H
diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/RigidBody/RigidBodySoftContact.cpp
similarity index 53%
copy from examples/BasicDemo/BasicExample.cpp
copy to examples/RigidBody/RigidBodySoftContact.cpp
index d501b51..1e7cd2c 100644
--- a/examples/BasicDemo/BasicExample.cpp
+++ b/examples/RigidBody/RigidBodySoftContact.cpp
@@ -15,31 +15,38 @@ subject to the following restrictions:
-#include "BasicExample.h"
+#include "RigidBodySoftContact.h"
#include "btBulletDynamicsCommon.h"
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Z 5
+#define ARRAY_SIZE_Y 1
+#define ARRAY_SIZE_X 1
+#define ARRAY_SIZE_Z 1
#include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
-struct BasicExample : public CommonRigidBodyBase
+
+
+struct RigidBodySoftContact : public CommonRigidBodyBase
{
- BasicExample(struct GUIHelperInterface* helper)
+ RigidBodySoftContact(struct GUIHelperInterface* helper)
:CommonRigidBodyBase(helper)
{
}
- virtual ~BasicExample(){}
+ virtual ~RigidBodySoftContact()
+ {
+
+ }
virtual void initPhysics();
virtual void renderScene();
void resetCamera()
{
- float dist = 41;
+ float dist = 3;
float pitch = 52;
float yaw = 35;
float targetPos[3]={0,0.46,0};
@@ -47,17 +54,47 @@ struct BasicExample : public CommonRigidBodyBase
}
};
-void BasicExample::initPhysics()
+
+
+
+
+void RigidBodySoftContact::initPhysics()
{
+
+
m_guiHelper->setUpAxis(1);
- createEmptyDynamicsWorld();
+ //createEmptyDynamicsWorld();
+ {
+ ///collision configuration contains default setup for memory, collision setup
+ m_collisionConfiguration = new btDefaultCollisionConfiguration();
+ //m_collisionConfiguration->setConvexConvexMultipointIterations();
+
+ ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+ m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+ m_broadphase = new btDbvtBroadphase();
+
+ ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
+ btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
+ //btMLCPSolver* sol = new btMLCPSolver(new btSolveProjectedGaussSeidel());
+ m_solver = sol;
+
+ m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
+
+ m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
+ }
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
if (m_dynamicsWorld->getDebugDrawer())
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
-
+ m_dynamicsWorld->getSolverInfo().m_erp2 = 0.f;
+ m_dynamicsWorld->getSolverInfo().m_globalCfm = 0.f;
+ m_dynamicsWorld->getSolverInfo().m_numIterations = 3;
+ m_dynamicsWorld->getSolverInfo().m_solverMode = SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
+ m_dynamicsWorld->getSolverInfo().m_splitImpulse = false;
+
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
@@ -73,7 +110,10 @@ void BasicExample::initPhysics()
{
btScalar mass(0.);
- createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
+ btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
+
+ body->setContactStiffnessAndDamping(300,10);
+
}
@@ -81,16 +121,20 @@ void BasicExample::initPhysics()
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
- btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
+ //btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
- //btCollisionShape* colShape = new btSphereShape(btScalar(1.));
+ btCollisionShape* childShape = new btSphereShape(btScalar(0.5));
+ btCompoundShape* colShape = new btCompoundShape();
+ colShape->addChildShape(btTransform::getIdentity(),childShape);
+
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
-
+
+ startTransform.setRotation(btQuaternion(btVector3(1,1,1),SIMD_PI/10.));
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
@@ -108,12 +152,13 @@ void BasicExample::initPhysics()
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(btVector3(
- btScalar(2.0*i),
- btScalar(20+2.0*k),
+ btScalar(2.0*i+0.1),
+ btScalar(3+2.0*k),
btScalar(2.0*j)));
- createRigidBody(mass,startTransform,colShape);
+ btRigidBody* body = createRigidBody(mass,startTransform,colShape);
+ //body->setAngularVelocity(btVector3(1,1,1));
}
@@ -125,7 +170,7 @@ void BasicExample::initPhysics()
}
-void BasicExample::renderScene()
+void RigidBodySoftContact::renderScene()
{
CommonRigidBodyBase::renderScene();
@@ -137,9 +182,9 @@ void BasicExample::renderScene()
-CommonExampleInterface* BasicExampleCreateFunc(CommonExampleOptions& options)
+CommonExampleInterface* RigidBodySoftContactCreateFunc(CommonExampleOptions& options)
{
- return new BasicExample(options.m_guiHelper);
+ return new RigidBodySoftContact(options.m_guiHelper);
}
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/RigidBody/RigidBodySoftContact.h
similarity index 75%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/RigidBody/RigidBodySoftContact.h
index 13c37a4..aa19530 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/RigidBody/RigidBodySoftContact.h
@@ -13,16 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
+#ifndef RIGID_BODY_SOFT_CONTACT_H
+#define RIGID_BODY_SOFT_CONTACT_H
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
+class CommonExampleInterface* RigidBodySoftContactCreateFunc(struct CommonExampleOptions& options);
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+#endif //RIGID_BODY_SOFT_CONTACT_H
diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp
new file mode 100644
index 0000000..b46706e
--- /dev/null
+++ b/examples/RoboticsLearning/GripperGraspExample.cpp
@@ -0,0 +1,470 @@
+
+#include "GripperGraspExample.h"
+
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../SharedMemory/SharedMemoryPublic.h"
+#include <string>
+
+#include "b3RobotSimAPI.h"
+#include "../Utils/b3Clock.h"
+
+static btScalar sGripperVerticalVelocity = -0.2f;
+static btScalar sGripperClosingTargetVelocity = 0.5f;
+
+class GripperGraspExample : public CommonExampleInterface
+{
+ CommonGraphicsApp* m_app;
+ GUIHelperInterface* m_guiHelper;
+ b3RobotSimAPI m_robotSim;
+ int m_options;
+ int m_r2d2Index;
+ int m_gripperIndex;
+
+ float m_x;
+ float m_y;
+ float m_z;
+ b3AlignedObjectArray<int> m_movingInstances;
+ enum
+ {
+ numCubesX = 20,
+ numCubesY = 20
+ };
+public:
+
+ GripperGraspExample(GUIHelperInterface* helper, int options)
+ :m_app(helper->getAppInterface()),
+ m_guiHelper(helper),
+ m_options(options),
+ m_r2d2Index(-1),
+ m_gripperIndex(-1),
+ m_x(0),
+ m_y(0),
+ m_z(0)
+ {
+ m_app->setUpAxis(2);
+ }
+ virtual ~GripperGraspExample()
+ {
+ m_app->m_renderer->enableBlend(false);
+ }
+
+
+ virtual void physicsDebugDraw(int debugDrawMode)
+ {
+ m_robotSim.debugDraw(debugDrawMode);
+ }
+ virtual void initPhysics()
+ {
+ bool connected = m_robotSim.connect(m_guiHelper);
+ b3Printf("robotSim connected = %d",connected);
+
+ if ((m_options & eGRIPPER_GRASP)!=0)
+ {
+
+ {
+ SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
+ slider.m_minVal=-2;
+ slider.m_maxVal=2;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+
+ {
+ SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity
+ );
+ slider.m_minVal=-1;
+ slider.m_maxVal=1;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+ {
+ b3RobotSimLoadFileArgs args("");
+ b3RobotSimLoadFileResults results;
+ args.m_fileName = "cube_small.urdf";
+ args.m_startPosition.setValue(0, 0, .107);
+ args.m_startOrientation.setEulerZYX(0, 0, 0);
+ args.m_useMultiBody = true;
+ m_robotSim.loadFile(args, results);
+ }
+
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf";
+ args.m_fileType = B3_SDF_FILE;
+ args.m_useMultiBody = true;
+ b3RobotSimLoadFileResults results;
+
+ if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
+ {
+
+ m_gripperIndex = results.m_uniqueObjectIds[0];
+ int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
+ b3Printf("numJoints = %d",numJoints);
+
+ for (int i=0;i<numJoints;i++)
+ {
+ b3JointInfo jointInfo;
+ m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo);
+ b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
+ }
+
+ /*
+ int fingerJointIndices[2]={1,3};
+ double fingerTargetPositions[2]={-0.04,0.04};
+ for (int i=0;i<2;i++)
+ {
+ b3JointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
+ controlArgs.m_targetPosition = fingerTargetPositions[i];
+ controlArgs.m_kp = 5.0;
+ controlArgs.m_kd = 3.0;
+ controlArgs.m_maxTorqueValue = 1.0;
+ m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
+ }
+ */
+ int fingerJointIndices[3]={0,1,3};
+ double fingerTargetVelocities[3]={-0.2,.5,-.5};
+ double maxTorqueValues[3]={40.0,50.0,50.0};
+ for (int i=0;i<3;i++)
+ {
+ b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+ controlArgs.m_targetVelocity = fingerTargetVelocities[i];
+ controlArgs.m_maxTorqueValue = maxTorqueValues[i];
+ controlArgs.m_kd = 1.;
+ m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
+ }
+ }
+ }
+
+ if (1)
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "plane.urdf";
+ args.m_startPosition.setValue(0,0,0);
+ args.m_startOrientation.setEulerZYX(0,0,0);
+ args.m_forceOverrideFixedBase = true;
+ args.m_useMultiBody = true;
+ b3RobotSimLoadFileResults results;
+ m_robotSim.loadFile(args,results);
+
+ }
+ m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+ m_robotSim.setNumSimulationSubSteps(4);
+ }
+
+ if ((m_options & eTWO_POINT_GRASP)!=0)
+ {
+ {
+ b3RobotSimLoadFileArgs args("");
+ b3RobotSimLoadFileResults results;
+ args.m_fileName = "cube_small.urdf";
+ args.m_startPosition.setValue(0, 0, .107);
+ args.m_startOrientation.setEulerZYX(0, 0, 0);
+ args.m_useMultiBody = true;
+ m_robotSim.loadFile(args, results);
+ }
+
+ {
+ b3RobotSimLoadFileArgs args("");
+ b3RobotSimLoadFileResults results;
+ args.m_fileName = "cube_gripper_left.urdf";
+ args.m_startPosition.setValue(0.068, 0.02, 0.11);
+ args.m_useMultiBody = true;
+ m_robotSim.loadFile(args, results);
+
+ b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+ controlArgs.m_targetVelocity = -0.1;
+ controlArgs.m_maxTorqueValue = 10.0;
+ controlArgs.m_kd = 1.;
+ m_robotSim.setJointMotorControl(1,0,controlArgs);
+ }
+
+ {
+ b3RobotSimLoadFileArgs args("");
+ b3RobotSimLoadFileResults results;
+ args.m_fileName = "cube_gripper_right.urdf";
+ args.m_startPosition.setValue(-0.068, 0.02, 0.11);
+ args.m_useMultiBody = true;
+ m_robotSim.loadFile(args, results);
+
+ b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+ controlArgs.m_targetVelocity = 0.1;
+ controlArgs.m_maxTorqueValue = 10.0;
+ controlArgs.m_kd = 1.;
+ m_robotSim.setJointMotorControl(2,0,controlArgs);
+ }
+
+ if (1)
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "plane.urdf";
+ args.m_startPosition.setValue(0,0,0);
+ args.m_startOrientation.setEulerZYX(0,0,0);
+ args.m_forceOverrideFixedBase = true;
+ args.m_useMultiBody = true;
+ b3RobotSimLoadFileResults results;
+ m_robotSim.loadFile(args,results);
+
+ }
+ m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+ m_robotSim.setNumSimulationSubSteps(4);
+ }
+
+ if ((m_options & eONE_MOTOR_GRASP)!=0)
+ {
+
+ {
+ SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
+ slider.m_minVal=-2;
+ slider.m_maxVal=2;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+
+ {
+ SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity
+ );
+ slider.m_minVal=-1;
+ slider.m_maxVal=1;
+ m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+ }
+ {
+ b3RobotSimLoadFileArgs args("");
+ b3RobotSimLoadFileResults results;
+ args.m_fileName = "sphere_small.urdf";
+ args.m_startPosition.setValue(0, 0, .107);
+ args.m_startOrientation.setEulerZYX(0, 0, 0);
+ args.m_useMultiBody = true;
+ m_robotSim.loadFile(args, results);
+ }
+
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf";
+ args.m_fileType = B3_SDF_FILE;
+ args.m_useMultiBody = true;
+ b3RobotSimLoadFileResults results;
+
+ if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
+ {
+
+ m_gripperIndex = results.m_uniqueObjectIds[0];
+ int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
+ b3Printf("numJoints = %d",numJoints);
+
+ for (int i=0;i<numJoints;i++)
+ {
+ b3JointInfo jointInfo;
+ m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo);
+ b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
+ }
+
+ for (int i=0;i<8;i++)
+ {
+ b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+ controlArgs.m_maxTorqueValue = 0.0;
+ m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs);
+ }
+
+ }
+ }
+
+ if (1)
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "plane.urdf";
+ args.m_startPosition.setValue(0,0,0);
+ args.m_startOrientation.setEulerZYX(0,0,0);
+ args.m_forceOverrideFixedBase = true;
+ args.m_useMultiBody = true;
+ b3RobotSimLoadFileResults results;
+ m_robotSim.loadFile(args,results);
+
+ }
+ m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+
+ /*
+ b3JointInfo sliderJoint1;
+ sliderJoint1.m_parentFrame[0] = 0;
+ sliderJoint1.m_parentFrame[1] = 0;
+ sliderJoint1.m_parentFrame[2] = 0.06;
+ sliderJoint1.m_parentFrame[3] = 0;
+ sliderJoint1.m_parentFrame[4] = 0;
+ sliderJoint1.m_parentFrame[5] = 0;
+ sliderJoint1.m_parentFrame[6] = 1.0;
+ sliderJoint1.m_childFrame[0] = 0;
+ sliderJoint1.m_childFrame[1] = 0;
+ sliderJoint1.m_childFrame[2] = 0;
+ sliderJoint1.m_childFrame[3] = 0;
+ sliderJoint1.m_childFrame[4] = 0;
+ sliderJoint1.m_childFrame[5] = 0;
+ sliderJoint1.m_childFrame[6] = 1.0;
+ sliderJoint1.m_jointAxis[0] = 1.0;
+ sliderJoint1.m_jointAxis[1] = 0.0;
+ sliderJoint1.m_jointAxis[2] = 0.0;
+ sliderJoint1.m_jointType = ePrismaticType;
+ b3JointInfo sliderJoint2;
+ sliderJoint2.m_parentFrame[0] = 0;
+ sliderJoint2.m_parentFrame[1] = 0;
+ sliderJoint2.m_parentFrame[2] = 0.06;
+ sliderJoint2.m_parentFrame[3] = 0;
+ sliderJoint2.m_parentFrame[4] = 0;
+ sliderJoint2.m_parentFrame[5] = 0;
+ sliderJoint2.m_parentFrame[6] = 1.0;
+ sliderJoint2.m_childFrame[0] = 0;
+ sliderJoint2.m_childFrame[1] = 0;
+ sliderJoint2.m_childFrame[2] = 0;
+ sliderJoint2.m_childFrame[3] = 0;
+ sliderJoint2.m_childFrame[4] = 0;
+ sliderJoint2.m_childFrame[5] = 1.0;
+ sliderJoint2.m_childFrame[6] = 0;
+ sliderJoint2.m_jointAxis[0] = 1.0;
+ sliderJoint2.m_jointAxis[1] = 0.0;
+ sliderJoint2.m_jointAxis[2] = 0.0;
+ sliderJoint2.m_jointType = ePrismaticType;
+ m_robotSim.createJoint(1, 0, 1, 3, &sliderJoint1);
+ m_robotSim.createJoint(1, 0, 1, 6, &sliderJoint2);
+ */
+ b3JointInfo revoluteJoint1;
+ revoluteJoint1.m_parentFrame[0] = -0.055;
+ revoluteJoint1.m_parentFrame[1] = 0;
+ revoluteJoint1.m_parentFrame[2] = 0.02;
+ revoluteJoint1.m_parentFrame[3] = 0;
+ revoluteJoint1.m_parentFrame[4] = 0;
+ revoluteJoint1.m_parentFrame[5] = 0;
+ revoluteJoint1.m_parentFrame[6] = 1.0;
+ revoluteJoint1.m_childFrame[0] = 0;
+ revoluteJoint1.m_childFrame[1] = 0;
+ revoluteJoint1.m_childFrame[2] = 0;
+ revoluteJoint1.m_childFrame[3] = 0;
+ revoluteJoint1.m_childFrame[4] = 0;
+ revoluteJoint1.m_childFrame[5] = 0;
+ revoluteJoint1.m_childFrame[6] = 1.0;
+ revoluteJoint1.m_jointAxis[0] = 1.0;
+ revoluteJoint1.m_jointAxis[1] = 0.0;
+ revoluteJoint1.m_jointAxis[2] = 0.0;
+ revoluteJoint1.m_jointType = ePoint2PointType;
+ b3JointInfo revoluteJoint2;
+ revoluteJoint2.m_parentFrame[0] = 0.055;
+ revoluteJoint2.m_parentFrame[1] = 0;
+ revoluteJoint2.m_parentFrame[2] = 0.02;
+ revoluteJoint2.m_parentFrame[3] = 0;
+ revoluteJoint2.m_parentFrame[4] = 0;
+ revoluteJoint2.m_parentFrame[5] = 0;
+ revoluteJoint2.m_parentFrame[6] = 1.0;
+ revoluteJoint2.m_childFrame[0] = 0;
+ revoluteJoint2.m_childFrame[1] = 0;
+ revoluteJoint2.m_childFrame[2] = 0;
+ revoluteJoint2.m_childFrame[3] = 0;
+ revoluteJoint2.m_childFrame[4] = 0;
+ revoluteJoint2.m_childFrame[5] = 0;
+ revoluteJoint2.m_childFrame[6] = 1.0;
+ revoluteJoint2.m_jointAxis[0] = 1.0;
+ revoluteJoint2.m_jointAxis[1] = 0.0;
+ revoluteJoint2.m_jointAxis[2] = 0.0;
+ revoluteJoint2.m_jointType = ePoint2PointType;
+ m_robotSim.createJoint(1, 2, 1, 4, &revoluteJoint1);
+ m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2);
+ }
+
+ }
+ virtual void exitPhysics()
+ {
+ m_robotSim.disconnect();
+ }
+ virtual void stepSimulation(float deltaTime)
+ {
+ if ((m_options & eGRIPPER_GRASP)!=0)
+ {
+ if ((m_gripperIndex>=0))
+ {
+ int fingerJointIndices[3]={0,1,3};
+ double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
+ ,-sGripperClosingTargetVelocity
+ };
+ double maxTorqueValues[3]={40.0,50.0,50.0};
+ for (int i=0;i<3;i++)
+ {
+ b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+ controlArgs.m_targetVelocity = fingerTargetVelocities[i];
+ controlArgs.m_maxTorqueValue = maxTorqueValues[i];
+ controlArgs.m_kd = 1.;
+ m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
+ }
+ }
+ }
+
+ if ((m_options & eONE_MOTOR_GRASP)!=0)
+ {
+ int fingerJointIndices[2]={0,1};
+ double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
+ };
+ double maxTorqueValues[2]={50.0,50.0};
+ for (int i=0;i<2;i++)
+ {
+ b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+ controlArgs.m_targetVelocity = fingerTargetVelocities[i];
+ controlArgs.m_maxTorqueValue = maxTorqueValues[i];
+ controlArgs.m_kd = 1.;
+ m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
+ }
+ }
+
+
+ m_robotSim.stepSimulation();
+ }
+ virtual void renderScene()
+ {
+ m_robotSim.renderScene();
+
+ //m_app->m_renderer->renderScene();
+
+ }
+
+
+ virtual void physicsDebugDraw()
+ {
+
+ }
+ virtual bool mouseMoveCallback(float x,float y)
+ {
+ return false;
+ }
+ virtual bool mouseButtonCallback(int button, int state, float x, float y)
+ {
+ return false;
+ }
+ virtual bool keyboardCallback(int key, int state)
+ {
+ return false;
+ }
+
+ virtual void resetCamera()
+ {
+ float dist = 1.5;
+ float pitch = 12;
+ float yaw = -10;
+ float targetPos[3]={-0.2,0.8,0.3};
+ if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
+ {
+ m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
+ m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
+ m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
+ m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
+ }
+ }
+
+};
+
+
+class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options)
+{
+ return new GripperGraspExample(options.m_guiHelper, options.m_option);
+}
+
+
+
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/RoboticsLearning/GripperGraspExample.h
similarity index 71%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/RoboticsLearning/GripperGraspExample.h
index 13c37a4..3717782 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/RoboticsLearning/GripperGraspExample.h
@@ -1,6 +1,6 @@
/*
Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2015 Google Inc. http://bulletphysics.org
+Copyright (c) 2016 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
@@ -13,16 +13,17 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
+#ifndef GRIPPER_GRASP_EXAMPLE_H
+#define GRIPPER_GRASP_EXAMPLE_H
-enum btInverseDynamicsExampleOptions
+enum GripperGraspExampleOptions
{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
+ eGRIPPER_GRASP=1,
+ eTWO_POINT_GRASP=2,
+ eONE_MOTOR_GRASP=4,
};
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
+class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+#endif //GRIPPER_GRASP_EXAMPLE_H
diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp
new file mode 100644
index 0000000..a07d6f6
--- /dev/null
+++ b/examples/RoboticsLearning/KukaGraspExample.cpp
@@ -0,0 +1,319 @@
+
+#include "KukaGraspExample.h"
+#include "../SharedMemory/IKTrajectoryHelper.h"
+
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include <string>
+
+#include "b3RobotSimAPI.h"
+#include "../Utils/b3Clock.h"
+
+///quick demo showing the right-handed coordinate system and positive rotations around each axis
+class KukaGraspExample : public CommonExampleInterface
+{
+ CommonGraphicsApp* m_app;
+ GUIHelperInterface* m_guiHelper;
+ b3RobotSimAPI m_robotSim;
+ int m_kukaIndex;
+
+ IKTrajectoryHelper m_ikHelper;
+ int m_targetSphereInstance;
+ b3Vector3 m_targetPos;
+ b3Vector3 m_worldPos;
+ b3Vector4 m_targetOri;
+ b3Vector4 m_worldOri;
+ double m_time;
+ int m_options;
+
+ b3AlignedObjectArray<int> m_movingInstances;
+ enum
+ {
+ numCubesX = 20,
+ numCubesY = 20
+ };
+public:
+
+ KukaGraspExample(GUIHelperInterface* helper, int options)
+ :m_app(helper->getAppInterface()),
+ m_guiHelper(helper),
+ m_options(options),
+ m_kukaIndex(-1),
+ m_time(0)
+ {
+ m_targetPos.setValue(0.5,0,1);
+ m_worldPos.setValue(0, 0, 0);
+ m_app->setUpAxis(2);
+ }
+ virtual ~KukaGraspExample()
+ {
+ m_app->m_renderer->enableBlend(false);
+ }
+
+
+ virtual void physicsDebugDraw(int debugDrawMode)
+ {
+
+ }
+ virtual void initPhysics()
+ {
+
+ ///create some graphics proxy for the tracking target
+ ///the endeffector tries to track it using Inverse Kinematics
+ {
+ int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM);
+ b3Quaternion orn(0, 0, 0, 1);
+ b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
+ b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
+ m_targetSphereInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, m_targetPos, orn, color, scaling);
+ }
+ m_app->m_renderer->writeTransforms();
+
+
+
+
+
+ bool connected = m_robotSim.connect(m_guiHelper);
+ b3Printf("robotSim connected = %d",connected);
+
+
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "kuka_iiwa/model.urdf";
+ args.m_startPosition.setValue(0,0,0);
+ b3RobotSimLoadFileResults results;
+ if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
+ {
+ m_kukaIndex = results.m_uniqueObjectIds[0];
+ int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
+ b3Printf("numJoints = %d",numJoints);
+
+ for (int i=0;i<numJoints;i++)
+ {
+ b3JointInfo jointInfo;
+ m_robotSim.getJointInfo(m_kukaIndex,i,&jointInfo);
+ b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
+ }
+ /*
+ int wheelJointIndices[4]={2,3,6,7};
+ int wheelTargetVelocities[4]={-10,-10,-10,-10};
+ for (int i=0;i<4;i++)
+ {
+ b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+ controlArgs.m_targetVelocity = wheelTargetVelocities[i];
+ controlArgs.m_maxTorqueValue = 1e30;
+ m_robotSim.setJointMotorControl(m_kukaIndex,wheelJointIndices[i],controlArgs);
+ }
+ */
+ }
+
+ if (0)
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "kiva_shelf/model.sdf";
+ args.m_forceOverrideFixedBase = true;
+ args.m_fileType = B3_SDF_FILE;
+ args.m_startOrientation = b3Quaternion(0,0,0,1);
+ b3RobotSimLoadFileResults results;
+ m_robotSim.loadFile(args,results);
+ }
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "plane.urdf";
+ args.m_startPosition.setValue(0,0,0);
+ args.m_forceOverrideFixedBase = true;
+ b3RobotSimLoadFileResults results;
+ m_robotSim.loadFile(args,results);
+ m_robotSim.setGravity(b3MakeVector3(0,0,0));
+ }
+
+ }
+
+ }
+ virtual void exitPhysics()
+ {
+ m_robotSim.disconnect();
+ }
+ virtual void stepSimulation(float deltaTime)
+ {
+ float dt = deltaTime;
+ btClamp(dt,0.0001f,0.01f);
+
+ m_time+=dt;
+ m_targetPos.setValue(0.4-0.4*b3Cos( m_time), 0, 0.8+0.4*b3Cos( m_time));
+ m_targetOri.setValue(0, 1.0, 0, 0);
+
+
+ int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
+
+ if (numJoints==7)
+ {
+ double q_current[7]={0,0,0,0,0,0,0};
+
+ double world_position[3]={0,0,0};
+ double world_orientation[4]={0,0,0,0};
+ b3JointStates jointStates;
+
+ if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
+ {
+ //skip the base positions (7 values)
+ b3Assert(7+numJoints == jointStates.m_numDegreeOfFreedomQ);
+ for (int i=0;i<numJoints;i++)
+ {
+ q_current[i] = jointStates.m_actualStateQ[i+7];
+ }
+ }
+ // compute body position and orientation
+ m_robotSim.getLinkState(0, 6, world_position, world_orientation);
+ m_worldPos.setValue(world_position[0], world_position[1], world_position[2]);
+ m_worldOri.setValue(world_orientation[0], world_orientation[1], world_orientation[2], world_orientation[3]);
+
+ b3Vector3DoubleData targetPosDataOut;
+ m_targetPos.serializeDouble(targetPosDataOut);
+ b3Vector3DoubleData worldPosDataOut;
+ m_worldPos.serializeDouble(worldPosDataOut);
+ b3Vector3DoubleData targetOriDataOut;
+ m_targetOri.serializeDouble(targetOriDataOut);
+ b3Vector3DoubleData worldOriDataOut;
+ m_worldOri.serializeDouble(worldOriDataOut);
+
+
+ b3RobotSimInverseKinematicArgs ikargs;
+ b3RobotSimInverseKinematicsResults ikresults;
+
+
+ ikargs.m_bodyUniqueId = m_kukaIndex;
+// ikargs.m_currentJointPositions = q_current;
+// ikargs.m_numPositions = 7;
+ ikargs.m_endEffectorTargetPosition[0] = targetPosDataOut.m_floats[0];
+ ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1];
+ ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2];
+
+ ikargs.m_flags |= /*B3_HAS_IK_TARGET_ORIENTATION +*/ B3_HAS_NULL_SPACE_VELOCITY;
+
+ ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
+ ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
+ ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2];
+ ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3];
+ ikargs.m_endEffectorLinkIndex = 6;
+
+ // Settings based on default KUKA arm setting
+ ikargs.m_lowerLimits.resize(numJoints);
+ ikargs.m_upperLimits.resize(numJoints);
+ ikargs.m_jointRanges.resize(numJoints);
+ ikargs.m_restPoses.resize(numJoints);
+ ikargs.m_lowerLimits[0] = -2.32;
+ ikargs.m_lowerLimits[1] = -1.6;
+ ikargs.m_lowerLimits[2] = -2.32;
+ ikargs.m_lowerLimits[3] = -1.6;
+ ikargs.m_lowerLimits[4] = -2.32;
+ ikargs.m_lowerLimits[5] = -1.6;
+ ikargs.m_lowerLimits[6] = -2.4;
+ ikargs.m_upperLimits[0] = 2.32;
+ ikargs.m_upperLimits[1] = 1.6;
+ ikargs.m_upperLimits[2] = 2.32;
+ ikargs.m_upperLimits[3] = 1.6;
+ ikargs.m_upperLimits[4] = 2.32;
+ ikargs.m_upperLimits[5] = 1.6;
+ ikargs.m_upperLimits[6] = 2.4;
+ ikargs.m_jointRanges[0] = 5.8;
+ ikargs.m_jointRanges[1] = 4;
+ ikargs.m_jointRanges[2] = 5.8;
+ ikargs.m_jointRanges[3] = 4;
+ ikargs.m_jointRanges[4] = 5.8;
+ ikargs.m_jointRanges[5] = 4;
+ ikargs.m_jointRanges[6] = 6;
+ ikargs.m_restPoses[0] = 0;
+ ikargs.m_restPoses[1] = 0;
+ ikargs.m_restPoses[2] = 0;
+ ikargs.m_restPoses[3] = SIMD_HALF_PI;
+ ikargs.m_restPoses[4] = 0;
+ ikargs.m_restPoses[5] = -SIMD_HALF_PI*0.66;
+ ikargs.m_restPoses[6] = 0;
+ ikargs.m_numDegreeOfFreedom = numJoints;
+
+ if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
+ {
+ //copy the IK result to the desired state of the motor/actuator
+ for (int i=0;i<numJoints;i++)
+ {
+ b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
+ t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
+ t.m_maxTorqueValue = 100.0;
+ t.m_kp= 1.0;
+ t.m_targetVelocity = 0;
+ t.m_kd = 1.0;
+ m_robotSim.setJointMotorControl(m_kukaIndex,i,t);
+
+ }
+ }
+ }
+
+
+ m_robotSim.stepSimulation();
+ }
+ virtual void renderScene()
+ {
+ m_robotSim.renderScene();
+
+
+ b3Quaternion orn(0, 0, 0, 1);
+
+
+ m_app->m_renderer->writeSingleInstanceTransformToCPU(m_targetPos, orn, m_targetSphereInstance);
+ m_app->m_renderer->writeTransforms();
+
+ //draw the end-effector target sphere
+
+ //m_app->m_renderer->renderScene();
+ }
+
+
+ virtual void physicsDebugDraw()
+ {
+
+ }
+ virtual bool mouseMoveCallback(float x,float y)
+ {
+ return false;
+ }
+ virtual bool mouseButtonCallback(int button, int state, float x, float y)
+ {
+ return false;
+ }
+ virtual bool keyboardCallback(int key, int state)
+ {
+ return false;
+ }
+
+ virtual void resetCamera()
+ {
+ float dist = 3;
+ float pitch = 0;
+ float yaw = 30;
+ float targetPos[3]={-0.2,0.8,0.3};
+ if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
+ {
+ m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
+ m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
+ m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
+ m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
+ }
+ }
+
+};
+
+
+class CommonExampleInterface* KukaGraspExampleCreateFunc(struct CommonExampleOptions& options)
+{
+ return new KukaGraspExample(options.m_guiHelper, options.m_option);
+}
+
+
+
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/RoboticsLearning/KukaGraspExample.h
similarity index 71%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/RoboticsLearning/KukaGraspExample.h
index 13c37a4..8275adb 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/RoboticsLearning/KukaGraspExample.h
@@ -1,6 +1,6 @@
/*
Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2015 Google Inc. http://bulletphysics.org
+Copyright (c) 2016 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
@@ -13,16 +13,15 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
+#ifndef KUKA_GRASP_EXAMPLE_H
+#define KUKA_GRASP_EXAMPLE_H
-enum btInverseDynamicsExampleOptions
+enum KukaGraspExampleOptions
{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
+ eKUKA_GRASP_DLS_IK=1,
};
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
+class CommonExampleInterface* KukaGraspExampleCreateFunc(struct CommonExampleOptions& options);
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+#endif //KUKA_GRASP_EXAMPLE_H
diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp
new file mode 100644
index 0000000..e0c9cad
--- /dev/null
+++ b/examples/RoboticsLearning/R2D2GraspExample.cpp
@@ -0,0 +1,233 @@
+
+#include "R2D2GraspExample.h"
+
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include <string>
+
+#include "b3RobotSimAPI.h"
+#include "../Utils/b3Clock.h"
+
+///quick demo showing the right-handed coordinate system and positive rotations around each axis
+class R2D2GraspExample : public CommonExampleInterface
+{
+ CommonGraphicsApp* m_app;
+ GUIHelperInterface* m_guiHelper;
+ b3RobotSimAPI m_robotSim;
+ int m_options;
+ int m_r2d2Index;
+
+ float m_x;
+ float m_y;
+ float m_z;
+ b3AlignedObjectArray<int> m_movingInstances;
+ enum
+ {
+ numCubesX = 20,
+ numCubesY = 20
+ };
+public:
+
+ R2D2GraspExample(GUIHelperInterface* helper, int options)
+ :m_app(helper->getAppInterface()),
+ m_guiHelper(helper),
+ m_options(options),
+ m_r2d2Index(-1),
+ m_x(0),
+ m_y(0),
+ m_z(0)
+ {
+ m_app->setUpAxis(2);
+ }
+ virtual ~R2D2GraspExample()
+ {
+ m_app->m_renderer->enableBlend(false);
+ }
+
+
+ virtual void physicsDebugDraw(int debugDrawMode)
+ {
+
+ }
+ virtual void initPhysics()
+ {
+ bool connected = m_robotSim.connect(m_guiHelper);
+ b3Printf("robotSim connected = %d",connected);
+
+
+ if ((m_options & eROBOTIC_LEARN_GRASP)!=0)
+ {
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "r2d2.urdf";
+ args.m_startPosition.setValue(0,0,.5);
+ b3RobotSimLoadFileResults results;
+ if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
+ {
+ int m_r2d2Index = results.m_uniqueObjectIds[0];
+ int numJoints = m_robotSim.getNumJoints(m_r2d2Index);
+ b3Printf("numJoints = %d",numJoints);
+
+ for (int i=0;i<numJoints;i++)
+ {
+ b3JointInfo jointInfo;
+ m_robotSim.getJointInfo(m_r2d2Index,i,&jointInfo);
+ b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
+ }
+ int wheelJointIndices[4]={2,3,6,7};
+ int wheelTargetVelocities[4]={-10,-10,-10,-10};
+ for (int i=0;i<4;i++)
+ {
+ b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+ controlArgs.m_targetVelocity = wheelTargetVelocities[i];
+ controlArgs.m_maxTorqueValue = 1e30;
+ m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs);
+ }
+ }
+ }
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "kiva_shelf/model.sdf";
+ args.m_forceOverrideFixedBase = true;
+ args.m_fileType = B3_SDF_FILE;
+ args.m_startOrientation = b3Quaternion(0,0,0,1);
+ b3RobotSimLoadFileResults results;
+ m_robotSim.loadFile(args,results);
+ }
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "plane.urdf";
+ args.m_startPosition.setValue(0,0,0);
+ args.m_forceOverrideFixedBase = true;
+ b3RobotSimLoadFileResults results;
+ m_robotSim.loadFile(args,results);
+ m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+ }
+
+ }
+
+ if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0)
+ {
+ b3RobotSimLoadFileArgs args("");
+ b3RobotSimLoadFileResults results;
+ {
+ args.m_fileName = "cube_soft.urdf";
+ args.m_startPosition.setValue(0,0,2.5);
+ args.m_startOrientation.setEulerZYX(0,0.2,0);
+ m_robotSim.loadFile(args,results);
+ }
+ {
+ args.m_fileName = "cube_no_friction.urdf";
+ args.m_startPosition.setValue(0,2,2.5);
+ args.m_startOrientation.setEulerZYX(0,0.2,0);
+ m_robotSim.loadFile(args,results);
+ }
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "plane.urdf";
+ args.m_startPosition.setValue(0,0,0);
+ args.m_startOrientation.setEulerZYX(0,0.2,0);
+ args.m_forceOverrideFixedBase = true;
+ b3RobotSimLoadFileResults results;
+ m_robotSim.loadFile(args,results);
+ m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+ }
+ }
+
+ if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0)
+ {
+ b3RobotSimLoadFileArgs args("");
+ b3RobotSimLoadFileResults results;
+ {
+ args.m_fileName = "sphere2_rolling_friction.urdf";
+ args.m_startPosition.setValue(0,0,2.5);
+ args.m_startOrientation.setEulerZYX(0,0,0);
+ args.m_useMultiBody = true;
+ m_robotSim.loadFile(args,results);
+ }
+ {
+ args.m_fileName = "sphere2.urdf";
+ args.m_startPosition.setValue(0,2,2.5);
+ args.m_startOrientation.setEulerZYX(0,0,0);
+ args.m_useMultiBody = true;
+ m_robotSim.loadFile(args,results);
+ }
+ {
+ b3RobotSimLoadFileArgs args("");
+ args.m_fileName = "plane.urdf";
+ args.m_startPosition.setValue(0,0,0);
+ args.m_startOrientation.setEulerZYX(0,0.2,0);
+ args.m_useMultiBody = true;
+ args.m_forceOverrideFixedBase = true;
+ b3RobotSimLoadFileResults results;
+ m_robotSim.loadFile(args,results);
+ m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+ }
+ }
+
+
+ }
+ virtual void exitPhysics()
+ {
+ m_robotSim.disconnect();
+ }
+ virtual void stepSimulation(float deltaTime)
+ {
+ m_robotSim.stepSimulation();
+ }
+ virtual void renderScene()
+ {
+ m_robotSim.renderScene();
+
+ //m_app->m_renderer->renderScene();
+ }
+
+
+ virtual void physicsDebugDraw()
+ {
+
+ }
+ virtual bool mouseMoveCallback(float x,float y)
+ {
+ return false;
+ }
+ virtual bool mouseButtonCallback(int button, int state, float x, float y)
+ {
+ return false;
+ }
+ virtual bool keyboardCallback(int key, int state)
+ {
+ return false;
+ }
+
+ virtual void resetCamera()
+ {
+ float dist = 3;
+ float pitch = -75;
+ float yaw = 30;
+ float targetPos[3]={-0.2,0.8,0.3};
+ if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
+ {
+ m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
+ m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
+ m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
+ m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
+ }
+ }
+
+};
+
+
+class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options)
+{
+ return new R2D2GraspExample(options.m_guiHelper, options.m_option);
+}
+
+
+
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/RoboticsLearning/R2D2GraspExample.h
similarity index 70%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/RoboticsLearning/R2D2GraspExample.h
index 13c37a4..1861b46 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/RoboticsLearning/R2D2GraspExample.h
@@ -1,6 +1,6 @@
/*
Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2015 Google Inc. http://bulletphysics.org
+Copyright (c) 2016 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
@@ -13,16 +13,17 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
+#ifndef R2D2_GRASP_EXAMPLE_H
+#define R2D2_GRASP_EXAMPLE_H
-enum btInverseDynamicsExampleOptions
+enum RobotLearningExampleOptions
{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
+ eROBOTIC_LEARN_GRASP=1,
+ eROBOTIC_LEARN_COMPLIANT_CONTACT=2,
+ eROBOTIC_LEARN_ROLLING_FRICTION=4,
};
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
+class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options);
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
+#endif //R2D2_GRASP_EXAMPLE_H
diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp
new file mode 100644
index 0000000..068bf16
--- /dev/null
+++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp
@@ -0,0 +1,999 @@
+#include "b3RobotSimAPI.h"
+
+//#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+//#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include "../SharedMemory/PhysicsDirectC_API.h"
+#include "../SharedMemory/PhysicsDirect.h"
+
+#include <string>
+#include "../Utils/b3Clock.h"
+
+
+#include "../MultiThreading/b3ThreadSupportInterface.h"
+
+
+void RobotThreadFunc(void* userPtr,void* lsMemory);
+void* RobotlsMemoryFunc();
+#define MAX_ROBOT_NUM_THREADS 1
+enum
+ {
+ numCubesX = 20,
+ numCubesY = 20
+ };
+
+
+enum TestRobotSimCommunicationEnums
+{
+ eRequestTerminateRobotSim= 13,
+ eRobotSimIsUnInitialized,
+ eRobotSimIsInitialized,
+ eRobotSimInitializationFailed,
+ eRobotSimHasTerminated
+};
+
+enum MultiThreadedGUIHelperCommunicationEnums
+{
+ eRobotSimGUIHelperIdle= 13,
+ eRobotSimGUIHelperRegisterTexture,
+ eRobotSimGUIHelperRegisterGraphicsShape,
+ eRobotSimGUIHelperRegisterGraphicsInstance,
+ eRobotSimGUIHelperCreateCollisionShapeGraphicsObject,
+ eRobotSimGUIHelperCreateCollisionObjectGraphicsObject,
+ eRobotSimGUIHelperRemoveAllGraphicsInstances,
+ eRobotSimGUIHelperCopyCameraImageData,
+};
+
+#include <stdio.h>
+//#include "BulletMultiThreaded/PlatformDefinitions.h"
+
+#ifndef _WIN32
+#include "../MultiThreading/b3PosixThreadSupport.h"
+
+b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads)
+{
+ b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("RobotSimThreads",
+ RobotThreadFunc,
+ RobotlsMemoryFunc,
+ numThreads);
+ b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
+
+ return threadSupport;
+
+}
+
+
+#elif defined( _WIN32)
+#include "../MultiThreading/b3Win32ThreadSupport.h"
+
+b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads)
+{
+ b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("RobotSimThreads",RobotThreadFunc,RobotlsMemoryFunc,numThreads);
+ b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
+ return threadSupport;
+
+}
+#endif
+
+
+
+struct RobotSimArgs
+{
+ RobotSimArgs()
+ :m_physicsServerPtr(0)
+ {
+ }
+ b3CriticalSection* m_cs;
+
+ PhysicsServerSharedMemory* m_physicsServerPtr;
+ b3AlignedObjectArray<b3Vector3> m_positions;
+};
+
+struct RobotSimThreadLocalStorage
+{
+ int threadId;
+};
+
+
+void RobotThreadFunc(void* userPtr,void* lsMemory)
+{
+ printf("RobotThreadFunc thread started\n");
+ RobotSimThreadLocalStorage* localStorage = (RobotSimThreadLocalStorage*) lsMemory;
+
+ RobotSimArgs* args = (RobotSimArgs*) userPtr;
+ int workLeft = true;
+ b3Clock clock;
+ clock.reset();
+ bool init = true;
+ if (init)
+ {
+
+ args->m_cs->lock();
+ args->m_cs->setSharedParam(0,eRobotSimIsInitialized);
+ args->m_cs->unlock();
+
+ do
+ {
+//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread?
+#if 0
+ double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.;
+ if (deltaTimeInSeconds<(1./260.))
+ {
+ if (deltaTimeInSeconds<.001)
+ continue;
+ }
+
+ clock.reset();
+#endif //
+ args->m_physicsServerPtr->processClientCommands();
+
+ } while (args->m_cs->getSharedParam(0)!=eRequestTerminateRobotSim);
+ } else
+ {
+ args->m_cs->lock();
+ args->m_cs->setSharedParam(0,eRobotSimInitializationFailed);
+ args->m_cs->unlock();
+ }
+ //do nothing
+}
+
+
+
+void* RobotlsMemoryFunc()
+{
+ //don't create local store memory, just return 0
+ return new RobotSimThreadLocalStorage;
+}
+
+
+
+ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper2 : public GUIHelperInterface
+{
+ CommonGraphicsApp* m_app;
+
+ b3CriticalSection* m_cs;
+
+
+public:
+
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
+ GUIHelperInterface* m_childGuiHelper;
+
+ const unsigned char* m_texels;
+ int m_textureWidth;
+ int m_textureHeight;
+
+
+ int m_shapeIndex;
+ const float* m_position;
+ const float* m_quaternion;
+ const float* m_color;
+ const float* m_scaling;
+
+ const float* m_vertices;
+ int m_numvertices;
+ const int* m_indices;
+ int m_numIndices;
+ int m_primitiveType;
+ int m_textureId;
+ int m_instanceId;
+
+
+ MultiThreadedOpenGLGuiHelper2(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
+ :m_app(app)
+ ,m_cs(0),
+ m_texels(0),
+ m_textureId(-1)
+ {
+ m_childGuiHelper = guiHelper;;
+
+ }
+
+ virtual ~MultiThreadedOpenGLGuiHelper2()
+ {
+ delete m_childGuiHelper;
+ }
+
+ void setCriticalSection(b3CriticalSection* cs)
+ {
+ m_cs = cs;
+ }
+
+ b3CriticalSection* getCriticalSection()
+ {
+ return m_cs;
+ }
+
+ virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color)
+ {
+ createCollisionObjectGraphicsObject((btCollisionObject*)body, color);
+ }
+
+ btCollisionObject* m_obj;
+ btVector3 m_color2;
+
+ virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
+ {
+ m_obj = obj;
+ m_color2 = color;
+ m_cs->lock();
+ m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionObjectGraphicsObject);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+ {
+ }
+
+ }
+
+ btCollisionShape* m_colShape;
+ virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
+ {
+ m_colShape = collisionShape;
+ m_cs->lock();
+ m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionShapeGraphicsObject);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+ {
+ }
+
+ }
+
+ virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
+ {
+ //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects.
+ //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms
+ if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0)
+ {
+ m_childGuiHelper->syncPhysicsToGraphics(rbWorld);
+ }
+ }
+
+ virtual void render(const btDiscreteDynamicsWorld* rbWorld)
+ {
+ m_childGuiHelper->render(0);
+ }
+
+ virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
+
+ virtual int registerTexture(const unsigned char* texels, int width, int height)
+ {
+ m_texels = texels;
+ m_textureWidth = width;
+ m_textureHeight = height;
+
+ m_cs->lock();
+ m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterTexture);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+ {
+ }
+ return m_textureId;
+ }
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
+ {
+ m_vertices = vertices;
+ m_numvertices = numvertices;
+ m_indices = indices;
+ m_numIndices = numIndices;
+ m_primitiveType = primitiveType;
+ m_textureId = textureId;
+
+ m_cs->lock();
+ m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsShape);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+ {
+ }
+ return m_shapeIndex;
+ }
+ virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
+ {
+ m_shapeIndex = shapeIndex;
+ m_position = position;
+ m_quaternion = quaternion;
+ m_color = color;
+ m_scaling = scaling;
+
+ m_cs->lock();
+ m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsInstance);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+ {
+ }
+ return m_instanceId;
+ }
+
+ virtual void removeAllGraphicsInstances()
+ {
+ m_cs->lock();
+ m_cs->setSharedParam(1,eRobotSimGUIHelperRemoveAllGraphicsInstances);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+ {
+ }
+ }
+
+ virtual Common2dCanvasInterface* get2dCanvasInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonParameterInterface* getParameterInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonRenderInterface* getRenderInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonGraphicsApp* getAppInterface()
+ {
+ return m_childGuiHelper->getAppInterface();
+ }
+
+
+ virtual void setUpAxis(int axis)
+ {
+ m_childGuiHelper->setUpAxis(axis);
+ }
+ virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
+ {
+ }
+
+ float m_viewMatrix[16];
+ float m_projectionMatrix[16];
+ unsigned char* m_pixelsRGBA;
+ int m_rgbaBufferSizeInPixels;
+ float* m_depthBuffer;
+ int m_depthBufferSizeInPixels;
+ int* m_segmentationMaskBuffer;
+ int m_segmentationMaskBufferSizeInPixels;
+ int m_startPixelIndex;
+ int m_destinationWidth;
+ int m_destinationHeight;
+ int* m_numPixelsCopied;
+
+ virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
+ unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
+ float* depthBuffer, int depthBufferSizeInPixels,
+ int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
+ int startPixelIndex, int destinationWidth,
+ int destinationHeight, int* numPixelsCopied)
+ {
+ m_cs->lock();
+ for (int i=0;i<16;i++)
+ {
+ m_viewMatrix[i] = viewMatrix[i];
+ m_projectionMatrix[i] = projectionMatrix[i];
+ }
+ m_pixelsRGBA = pixelsRGBA;
+ m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels;
+ m_depthBuffer = depthBuffer;
+ m_depthBufferSizeInPixels = depthBufferSizeInPixels;
+ m_segmentationMaskBuffer = segmentationMaskBuffer;
+ m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels;
+ m_startPixelIndex = startPixelIndex;
+ m_destinationWidth = destinationWidth;
+ m_destinationHeight = destinationHeight;
+ m_numPixelsCopied = numPixelsCopied;
+
+ m_cs->setSharedParam(1,eRobotSimGUIHelperCopyCameraImageData);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+ {
+ }
+ }
+
+ virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
+ {
+ }
+
+ virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
+ {
+ }
+};
+
+
+
+
+
+struct b3RobotSimAPI_InternalData
+{
+ //GUIHelperInterface* m_guiHelper;
+ PhysicsServerSharedMemory m_physicsServer;
+ b3PhysicsClientHandle m_physicsClient;
+
+
+ b3ThreadSupportInterface* m_threadSupport;
+ RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS];
+ MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper;
+ PhysicsDirect* m_clientServerDirect;
+
+ bool m_useMultiThreading;
+ bool m_connected;
+
+ b3RobotSimAPI_InternalData()
+ :m_threadSupport(0),
+ m_multiThreadedHelper(0),
+ m_clientServerDirect(0),
+ m_physicsClient(0),
+ m_useMultiThreading(false),
+ m_connected(false)
+ {
+ }
+};
+
+b3RobotSimAPI::b3RobotSimAPI()
+{
+ m_data = new b3RobotSimAPI_InternalData;
+}
+
+void b3RobotSimAPI::stepSimulation()
+{
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ b3Assert(b3CanSubmitCommand(m_data->m_physicsClient));
+ if (b3CanSubmitCommand(m_data->m_physicsClient))
+ {
+ statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3InitStepSimulationCommand(m_data->m_physicsClient));
+ statusType = b3GetStatusType(statusHandle);
+ b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED);
+ }
+}
+
+void b3RobotSimAPI::setGravity(const b3Vector3& gravityAcceleration)
+{
+ b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient);
+ b3SharedMemoryStatusHandle statusHandle;
+ b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+ b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
+
+}
+
+void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps)
+{
+ b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient);
+ b3SharedMemoryStatusHandle statusHandle;
+ b3PhysicsParamSetNumSubSteps(command, numSubSteps);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+ b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
+}
+
+/*
+b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
+ const double* jointPositionsQ, double targetPosition[3]);
+
+int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
+ int* bodyUniqueId,
+ int* dofCount,
+ double* jointPositions);
+*/
+bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results)
+{
+ btAssert(args.m_endEffectorLinkIndex>=0);
+ btAssert(args.m_bodyUniqueId>=0);
+
+
+ b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId);
+ if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY))
+ {
+ b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
+ } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
+ {
+ b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation);
+ } else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
+ {
+ b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
+ } else
+ {
+ b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition);
+ }
+
+
+ b3SharedMemoryStatusHandle statusHandle;
+ statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+
+
+
+ int numPos=0;
+
+ bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
+ &results.m_bodyUniqueId,
+ &numPos,
+ 0);
+ if (result && numPos)
+ {
+ results.m_calculatedJointPositions.resize(numPos);
+ result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
+ &results.m_bodyUniqueId,
+ &numPos,
+ &results.m_calculatedJointPositions[0]);
+
+ }
+ return result;
+}
+
+
+b3RobotSimAPI::~b3RobotSimAPI()
+{
+ delete m_data;
+}
+
+void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
+{
+ if (0==m_data->m_multiThreadedHelper)
+ return;
+
+ switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
+ {
+ case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject:
+ {
+ m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_data->m_multiThreadedHelper->m_colShape);
+ m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+ m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+ m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+
+ break;
+ }
+ case eRobotSimGUIHelperCreateCollisionObjectGraphicsObject:
+ {
+ m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_data->m_multiThreadedHelper->m_obj,
+ m_data->m_multiThreadedHelper->m_color2);
+ m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+ m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+ m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eRobotSimGUIHelperRegisterTexture:
+ {
+
+ m_data->m_multiThreadedHelper->m_textureId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_data->m_multiThreadedHelper->m_texels,
+ m_data->m_multiThreadedHelper->m_textureWidth,m_data->m_multiThreadedHelper->m_textureHeight);
+
+ m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+ m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+ m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+
+ break;
+ }
+ case eRobotSimGUIHelperRegisterGraphicsShape:
+ {
+ m_data->m_multiThreadedHelper->m_shapeIndex = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape(
+ m_data->m_multiThreadedHelper->m_vertices,
+ m_data->m_multiThreadedHelper->m_numvertices,
+ m_data->m_multiThreadedHelper->m_indices,
+ m_data->m_multiThreadedHelper->m_numIndices,
+ m_data->m_multiThreadedHelper->m_primitiveType,
+ m_data->m_multiThreadedHelper->m_textureId);
+
+ m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+ m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+ m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eRobotSimGUIHelperRegisterGraphicsInstance:
+ {
+ m_data->m_multiThreadedHelper->m_instanceId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance(
+ m_data->m_multiThreadedHelper->m_shapeIndex,
+ m_data->m_multiThreadedHelper->m_position,
+ m_data->m_multiThreadedHelper->m_quaternion,
+ m_data->m_multiThreadedHelper->m_color,
+ m_data->m_multiThreadedHelper->m_scaling);
+
+ m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+ m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+ m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eRobotSimGUIHelperRemoveAllGraphicsInstances:
+ {
+ m_data->m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
+ int numRenderInstances = m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
+ b3Assert(numRenderInstances==0);
+
+ m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+ m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+ m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eRobotSimGUIHelperCopyCameraImageData:
+ {
+ m_data->m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_data->m_multiThreadedHelper->m_viewMatrix,
+ m_data->m_multiThreadedHelper->m_projectionMatrix,
+ m_data->m_multiThreadedHelper->m_pixelsRGBA,
+ m_data->m_multiThreadedHelper->m_rgbaBufferSizeInPixels,
+ m_data->m_multiThreadedHelper->m_depthBuffer,
+ m_data->m_multiThreadedHelper->m_depthBufferSizeInPixels,
+ m_data->m_multiThreadedHelper->m_segmentationMaskBuffer,
+ m_data->m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels,
+ m_data->m_multiThreadedHelper->m_startPixelIndex,
+ m_data->m_multiThreadedHelper->m_destinationWidth,
+ m_data->m_multiThreadedHelper->m_destinationHeight,
+ m_data->m_multiThreadedHelper->m_numPixelsCopied);
+ m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+ m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+ m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+
+ break;
+ }
+ case eRobotSimGUIHelperIdle:
+ default:
+ {
+
+ }
+ }
+
+}
+
+b3SharedMemoryStatusHandle b3RobotSimAPI::submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle)
+{
+ int timeout = 1024*1024*1024;
+ b3SharedMemoryStatusHandle statusHandle=0;
+
+ b3SubmitClientCommand(physClient,commandHandle);
+
+ while ((statusHandle==0) && (timeout-- > 0))
+ {
+ statusHandle =b3ProcessServerStatus(physClient);
+ processMultiThreadedGraphicsRequests();
+ }
+ return (b3SharedMemoryStatusHandle) statusHandle;
+}
+
+int b3RobotSimAPI::getNumJoints(int bodyUniqueId) const
+{
+ return b3GetNumJoints(m_data->m_physicsClient,bodyUniqueId);
+}
+
+bool b3RobotSimAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
+{
+ return (b3GetJointInfo(m_data->m_physicsClient,bodyUniqueId, jointIndex,jointInfo)!=0);
+}
+
+void b3RobotSimAPI::createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
+{
+ b3SharedMemoryStatusHandle statusHandle;
+ b3Assert(b3CanSubmitCommand(m_data->m_physicsClient));
+ if (b3CanSubmitCommand(m_data->m_physicsClient))
+ {
+ statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3CreateJoint(m_data->m_physicsClient, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
+ }
+}
+
+
+bool b3RobotSimAPI::getJointStates(int bodyUniqueId, b3JointStates& state)
+{
+ b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId);
+ b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+
+ if (statusHandle)
+ {
+ double rootInertialFrame[7];
+ const double* rootLocalInertialFrame;
+ const double* actualStateQ;
+ const double* actualStateQdot;
+ const double* jointReactionForces;
+
+ int stat = b3GetStatusActualState(statusHandle,
+ &state.m_bodyUniqueId,
+ &state.m_numDegreeOfFreedomQ,
+ &state.m_numDegreeOfFreedomU,
+ &rootLocalInertialFrame,
+ &actualStateQ,
+ &actualStateQdot,
+ &jointReactionForces);
+ if (stat)
+ {
+ state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ);
+ state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU);
+
+ for (int i=0;i<state.m_numDegreeOfFreedomQ;i++)
+ {
+ state.m_actualStateQ[i] = actualStateQ[i];
+ }
+ for (int i=0;i<state.m_numDegreeOfFreedomU;i++)
+ {
+ state.m_actualStateQdot[i] = actualStateQdot[i];
+ }
+ int numJoints = getNumJoints(bodyUniqueId);
+ state.m_jointReactionForces.resize(6*numJoints);
+ for (int i=0;i<numJoints*6;i++)
+ {
+ state.m_jointReactionForces[i] = jointReactionForces[i];
+ }
+
+ return true;
+ }
+ //rootInertialFrame,
+ // &state.m_actualStateQ[0],
+ // &state.m_actualStateQdot[0],
+ // &state.m_jointReactionForces[0]);
+
+
+ }
+ return false;
+}
+
+void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointMotorArgs& args)
+{
+ b3SharedMemoryStatusHandle statusHandle;
+ switch (args.m_controlMode)
+ {
+ case CONTROL_MODE_VELOCITY:
+ {
+ b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClient, bodyUniqueId, CONTROL_MODE_VELOCITY);
+ b3JointInfo jointInfo;
+ b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
+ int uIndex = jointInfo.m_uIndex;
+ b3JointControlSetKd(command,uIndex,args.m_kd);
+ b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
+ b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+ break;
+ }
+ case CONTROL_MODE_POSITION_VELOCITY_PD:
+ {
+ b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClient, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
+ b3JointInfo jointInfo;
+ b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
+ int uIndex = jointInfo.m_uIndex;
+ int qIndex = jointInfo.m_qIndex;
+
+ b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition);
+ b3JointControlSetKp(command,uIndex,args.m_kp);
+ b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
+ b3JointControlSetKd(command,uIndex,args.m_kd);
+ b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+ break;
+ }
+ case CONTROL_MODE_TORQUE:
+ {
+ b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClient, bodyUniqueId, CONTROL_MODE_TORQUE);
+ b3JointInfo jointInfo;
+ b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
+ int uIndex = jointInfo.m_uIndex;
+ b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+ break;
+ }
+ default:
+ {
+ b3Error("Unknown control command in b3RobotSimAPI::setJointMotorControl");
+ }
+ }
+}
+
+bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotSimLoadFileResults& results)
+{
+ bool statusOk = false;
+
+ int robotUniqueId = -1;
+ b3Assert(m_data->m_connected);
+ switch (args.m_fileType)
+ {
+ case B3_URDF_FILE:
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str());
+
+ //setting the initial position, orientation and other arguments are optional
+
+ b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
+ args.m_startPosition[1],
+ args.m_startPosition[2]);
+ b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0]
+ ,args.m_startOrientation[1]
+ ,args.m_startOrientation[2]
+ ,args.m_startOrientation[3]);
+ if (args.m_forceOverrideFixedBase)
+ {
+ b3LoadUrdfCommandSetUseFixedBase(command,true);
+ }
+ b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
+ statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
+ statusType = b3GetStatusType(statusHandle);
+
+ b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
+ robotUniqueId = b3GetStatusBodyIndex(statusHandle);
+ results.m_uniqueObjectIds.push_back(robotUniqueId);
+ statusOk = true;
+ break;
+ }
+ case B3_SDF_FILE:
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str());
+ b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
+ statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
+ statusType = b3GetStatusType(statusHandle);
+ b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
+ if (statusType == CMD_SDF_LOADING_COMPLETED)
+ {
+ int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
+ if (numBodies)
+ {
+ results.m_uniqueObjectIds.resize(numBodies);
+ int numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
+
+ }
+ statusOk = true;
+ }
+
+ break;
+ }
+ default:
+ {
+ b3Warning("Unknown file type in b3RobotSimAPI::loadFile");
+ }
+
+ }
+
+ return statusOk;
+}
+
+bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper)
+{
+ if (m_data->m_useMultiThreading)
+ {
+ m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper);
+
+ MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper);
+
+
+
+
+ m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS);
+
+ for (int i = 0; i < m_data->m_threadSupport->getNumTasks(); i++)
+ {
+ RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*)m_data->m_threadSupport->getThreadLocalMemory(i);
+ b3Assert(storage);
+ storage->threadId = i;
+ //storage->m_sharedMem = data->m_sharedMem;
+ }
+
+
+
+
+ for (int w = 0; w < MAX_ROBOT_NUM_THREADS; w++)
+ {
+ m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection();
+ m_data->m_args[w].m_cs->setSharedParam(0, eRobotSimIsUnInitialized);
+ int numMoving = 0;
+ m_data->m_args[w].m_positions.resize(numMoving);
+ m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer;
+ int index = 0;
+
+ m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*)&m_data->m_args[w], w);
+ while (m_data->m_args[w].m_cs->getSharedParam(0) == eRobotSimIsUnInitialized)
+ {
+ }
+ }
+
+ m_data->m_args[0].m_cs->setSharedParam(1, eRobotSimGUIHelperIdle);
+ m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs);
+
+ bool serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper);
+ b3Assert(serverConnected);
+
+ m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
+ }
+ else
+ {
+ m_data->m_clientServerDirect = new PhysicsDirect();
+ bool connected = m_data->m_clientServerDirect->connect(guiHelper);
+ m_data->m_physicsClient = (b3PhysicsClientHandle)m_data->m_clientServerDirect;
+
+ }
+
+ //client connected
+ m_data->m_connected = b3CanSubmitCommand(m_data->m_physicsClient);
+
+ b3Assert(m_data->m_connected);
+ return m_data->m_connected && m_data->m_connected;
+}
+
+void b3RobotSimAPI::disconnect()
+{
+ if (m_data->m_useMultiThreading)
+ {
+ for (int i = 0; i < MAX_ROBOT_NUM_THREADS; i++)
+ {
+ m_data->m_args[i].m_cs->lock();
+ m_data->m_args[i].m_cs->setSharedParam(0, eRequestTerminateRobotSim);
+ m_data->m_args[i].m_cs->unlock();
+ }
+ int numActiveThreads = MAX_ROBOT_NUM_THREADS;
+
+ while (numActiveThreads)
+ {
+ int arg0, arg1;
+ if (m_data->m_threadSupport->isTaskCompleted(&arg0, &arg1, 0))
+ {
+ numActiveThreads--;
+ printf("numActiveThreads = %d\n", numActiveThreads);
+
+ }
+ else
+ {
+ }
+ };
+
+ printf("stopping threads\n");
+
+ delete m_data->m_threadSupport;
+ m_data->m_threadSupport = 0;
+ }
+ b3DisconnectSharedMemory(m_data->m_physicsClient);
+ m_data->m_physicsServer.disconnectSharedMemory(true);
+
+ m_data->m_connected = false;
+}
+
+
+void b3RobotSimAPI::debugDraw(int debugDrawMode)
+{
+ if (m_data->m_clientServerDirect)
+ {
+ m_data->m_clientServerDirect->debugDraw(debugDrawMode);
+ }
+}
+void b3RobotSimAPI::renderScene()
+{
+ if (m_data->m_useMultiThreading)
+ {
+ if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
+ {
+ m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
+ }
+ }
+
+ if (m_data->m_clientServerDirect)
+ {
+ m_data->m_clientServerDirect->renderScene();
+ }
+ m_data->m_physicsServer.renderScene();
+}
+
+void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
+{
+ b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClient, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
+ b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+
+ if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
+ {
+ b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
+ }
+}
+
+void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation)
+{
+ b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId);
+ b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+
+ if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+ {
+ b3LinkState linkState;
+ b3GetLinkState(m_data->m_physicsClient, statusHandle, linkIndex, &linkState);
+ worldPosition[0] = linkState.m_worldPosition[0];
+ worldPosition[1] = linkState.m_worldPosition[1];
+ worldPosition[2] = linkState.m_worldPosition[2];
+ worldOrientation[0] = linkState.m_worldOrientation[0];
+ worldOrientation[1] = linkState.m_worldOrientation[1];
+ worldOrientation[2] = linkState.m_worldOrientation[2];
+ worldOrientation[3] = linkState.m_worldOrientation[3];
+ }
+}
\ No newline at end of file
diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h
new file mode 100644
index 0000000..798b892
--- /dev/null
+++ b/examples/RoboticsLearning/b3RobotSimAPI.h
@@ -0,0 +1,169 @@
+#ifndef B3_ROBOT_SIM_API_H
+#define B3_ROBOT_SIM_API_H
+
+///todo: remove those includes from this header
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include "../SharedMemory/SharedMemoryPublic.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3Transform.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+#include <string>
+
+enum b3RigidSimFileType
+{
+ B3_URDF_FILE=1,
+ B3_SDF_FILE,
+ B3_AUTO_DETECT_FILE//todo based on extension
+};
+
+struct b3JointStates
+{
+ int m_bodyUniqueId;
+ int m_numDegreeOfFreedomQ;
+ int m_numDegreeOfFreedomU;
+ b3Transform m_rootLocalInertialFrame;
+ b3AlignedObjectArray<double> m_actualStateQ;
+ b3AlignedObjectArray<double> m_actualStateQdot;
+ b3AlignedObjectArray<double> m_jointReactionForces;
+};
+
+struct b3RobotSimLoadFileArgs
+{
+ std::string m_fileName;
+ b3Vector3 m_startPosition;
+ b3Quaternion m_startOrientation;
+ bool m_forceOverrideFixedBase;
+ bool m_useMultiBody;
+ int m_fileType;
+
+
+ b3RobotSimLoadFileArgs(const std::string& fileName)
+ :m_fileName(fileName),
+ m_startPosition(b3MakeVector3(0,0,0)),
+ m_startOrientation(b3Quaternion(0,0,0,1)),
+ m_forceOverrideFixedBase(false),
+ m_useMultiBody(true),
+ m_fileType(B3_URDF_FILE)
+ {
+ }
+};
+
+
+struct b3RobotSimLoadFileResults
+{
+ b3AlignedObjectArray<int> m_uniqueObjectIds;
+ b3RobotSimLoadFileResults()
+ {
+ }
+};
+
+struct b3JointMotorArgs
+{
+ int m_controlMode;
+
+ double m_targetPosition;
+ double m_kp;
+
+ double m_targetVelocity;
+ double m_kd;
+
+ double m_maxTorqueValue;
+
+ b3JointMotorArgs(int controlMode)
+ :m_controlMode(controlMode),
+ m_targetPosition(0),
+ m_kp(0.1),
+ m_targetVelocity(0),
+ m_kd(0.9),
+ m_maxTorqueValue(1000)
+ {
+ }
+};
+
+enum b3InverseKinematicsFlags
+{
+ B3_HAS_IK_TARGET_ORIENTATION=1,
+ B3_HAS_NULL_SPACE_VELOCITY=2,
+};
+
+struct b3RobotSimInverseKinematicArgs
+{
+ int m_bodyUniqueId;
+// double* m_currentJointPositions;
+// int m_numPositions;
+ double m_endEffectorTargetPosition[3];
+ double m_endEffectorTargetOrientation[4];
+ int m_endEffectorLinkIndex;
+ int m_flags;
+ int m_numDegreeOfFreedom;
+ b3AlignedObjectArray<double> m_lowerLimits;
+ b3AlignedObjectArray<double> m_upperLimits;
+ b3AlignedObjectArray<double> m_jointRanges;
+ b3AlignedObjectArray<double> m_restPoses;
+
+ b3RobotSimInverseKinematicArgs()
+ :m_bodyUniqueId(-1),
+ m_endEffectorLinkIndex(-1),
+ m_flags(0)
+ {
+ m_endEffectorTargetPosition[0]=0;
+ m_endEffectorTargetPosition[1]=0;
+ m_endEffectorTargetPosition[2]=0;
+
+ m_endEffectorTargetOrientation[0]=0;
+ m_endEffectorTargetOrientation[1]=0;
+ m_endEffectorTargetOrientation[2]=0;
+ m_endEffectorTargetOrientation[3]=1;
+ }
+};
+
+struct b3RobotSimInverseKinematicsResults
+{
+ int m_bodyUniqueId;
+ b3AlignedObjectArray<double> m_calculatedJointPositions;
+};
+
+class b3RobotSimAPI
+{
+ struct b3RobotSimAPI_InternalData* m_data;
+ void processMultiThreadedGraphicsRequests();
+ b3SharedMemoryStatusHandle submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
+
+public:
+ b3RobotSimAPI();
+ virtual ~b3RobotSimAPI();
+
+ bool connect(struct GUIHelperInterface* guiHelper);
+ void disconnect();
+
+ bool loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotSimLoadFileResults& results);
+
+ int getNumJoints(int bodyUniqueId) const;
+
+ bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
+
+ void createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
+
+ bool getJointStates(int bodyUniqueId, b3JointStates& state);
+
+ void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args);
+
+ void stepSimulation();
+
+ void setGravity(const b3Vector3& gravityAcceleration);
+
+ void setNumSimulationSubSteps(int numSubSteps);
+
+ bool calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results);
+
+ void renderScene();
+ void debugDraw(int debugDrawMode);
+
+ void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
+
+ void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation);
+};
+
+#endif //B3_ROBOT_SIM_API_H
diff --git a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp
index 1d32131..7e53be9 100644
--- a/examples/RollingFrictionDemo/RollingFrictionDemo.cpp
+++ b/examples/RollingFrictionDemo/RollingFrictionDemo.cpp
@@ -234,9 +234,8 @@ void RollingFrictionDemo::initPhysics()
{
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
- b3ResourcePath p;
char resourcePath[1024];
- if (p.findResourcePath("slope.bullet",resourcePath,1024))
+ if (b3ResourcePath::findResourcePath("slope.bullet",resourcePath,1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
diff --git a/examples/SharedMemory/BodyJointInfoUtility.h b/examples/SharedMemory/BodyJointInfoUtility.h
index ca0c1bf..4488d68 100644
--- a/examples/SharedMemory/BodyJointInfoUtility.h
+++ b/examples/SharedMemory/BodyJointInfoUtility.h
@@ -57,6 +57,8 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
}
info.m_jointType = mb->m_links[link].m_jointType;
+ info.m_jointDamping = mb->m_links[link].m_jointDamping;
+ info.m_jointFriction = mb->m_links[link].m_jointFriction;
if ((mb->m_links[link].m_jointType == eRevoluteType) ||
(mb->m_links[link].m_jointType == ePrismaticType)) {
diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp
new file mode 100644
index 0000000..13bd8a2
--- /dev/null
+++ b/examples/SharedMemory/IKTrajectoryHelper.cpp
@@ -0,0 +1,206 @@
+#include "IKTrajectoryHelper.h"
+#include "BussIK/Node.h"
+#include "BussIK/Tree.h"
+#include "BussIK/Jacobian.h"
+#include "BussIK/VectorRn.h"
+#include "BussIK/MatrixRmn.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "BulletDynamics/Featherstone/btMultiBody.h"
+
+
+#define RADIAN(X) ((X)*RadiansToDegrees)
+
+//use BussIK and Reflexxes to convert from Cartesian endeffector future target to
+//joint space positions at each real-time (simulation) step
+struct IKTrajectoryHelperInternalData
+{
+ VectorR3 m_endEffectorTargetPosition;
+ VectorRn m_nullSpaceVelocity;
+
+ b3AlignedObjectArray<Node*> m_ikNodes;
+ Jacobian* m_ikJacobian;
+
+ IKTrajectoryHelperInternalData()
+ {
+ m_endEffectorTargetPosition.SetZero();
+ m_nullSpaceVelocity.SetZero();
+ }
+};
+
+
+IKTrajectoryHelper::IKTrajectoryHelper()
+{
+ m_data = new IKTrajectoryHelperInternalData;
+}
+
+IKTrajectoryHelper::~IKTrajectoryHelper()
+{
+ delete m_data;
+}
+
+
+bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
+ const double endEffectorTargetOrientation[4],
+ const double endEffectorWorldPosition[3],
+ const double endEffectorWorldOrientation[4],
+ const double* q_current, int numQ,int endEffectorIndex,
+ double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
+{
+ bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;
+
+ m_data->m_ikJacobian = new Jacobian(useAngularPart,numQ);
+
+// Reset(m_ikTree,m_ikJacobian);
+
+ m_data->m_ikJacobian->Reset();
+
+ bool UseJacobianTargets1 = false;
+
+ if ( UseJacobianTargets1 ) {
+ m_data->m_ikJacobian->SetJtargetActive();
+ }
+ else {
+ m_data->m_ikJacobian->SetJendActive();
+ }
+ VectorR3 targets;
+ targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
+ m_data->m_ikJacobian->ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
+
+ // Set one end effector world position from Bullet
+ VectorRn deltaS(3);
+ for (int i = 0; i < 3; ++i)
+ {
+ deltaS.Set(i,dampIk[i]*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
+ }
+
+ // Set one end effector world orientation from Bullet
+ VectorRn deltaR(3);
+ if (useAngularPart)
+ {
+ btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]);
+ btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]);
+ btQuaternion deltaQ = endQ*startQ.inverse();
+ float angle = deltaQ.getAngle();
+ btVector3 axis = deltaQ.getAxis();
+ if (angle > PI) {
+ angle -= 2.0*PI;
+ }
+ else if (angle < -PI) {
+ angle += 2.0*PI;
+ }
+ float angleDot = angle;
+ btVector3 angularVel = angleDot*axis.normalize();
+ for (int i = 0; i < 3; ++i)
+ {
+ deltaR.Set(i,dampIk[i+3]*angularVel[i]);
+ }
+ }
+
+ {
+
+ if (useAngularPart)
+ {
+ VectorRn deltaC(6);
+ MatrixRmn completeJacobian(6,numQ);
+ for (int i = 0; i < 3; ++i)
+ {
+ deltaC.Set(i,deltaS[i]);
+ deltaC.Set(i+3,deltaR[i]);
+ for (int j = 0; j < numQ; ++j)
+ {
+ completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
+ completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]);
+ }
+ }
+ m_data->m_ikJacobian->SetDeltaS(deltaC);
+ m_data->m_ikJacobian->SetJendTrans(completeJacobian);
+ } else
+ {
+ VectorRn deltaC(3);
+ MatrixRmn completeJacobian(3,numQ);
+ for (int i = 0; i < 3; ++i)
+ {
+ deltaC.Set(i,deltaS[i]);
+ for (int j = 0; j < numQ; ++j)
+ {
+ completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
+ }
+ }
+ m_data->m_ikJacobian->SetDeltaS(deltaC);
+ m_data->m_ikJacobian->SetJendTrans(completeJacobian);
+ }
+ }
+
+ // Calculate the change in theta values
+ switch (ikMethod) {
+ case IK2_JACOB_TRANS:
+ m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method
+ break;
+ case IK2_DLS:
+ case IK2_VEL_DLS:
+ case IK2_VEL_DLS_WITH_ORIENTATION:
+ m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
+ break;
+ case IK2_VEL_DLS_WITH_NULLSPACE:
+ case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
+ assert(m_data->m_nullSpaceVelocity.GetLength()==numQ);
+ m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
+ break;
+ case IK2_DLS_SVD:
+ m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD();
+ break;
+ case IK2_PURE_PSEUDO:
+ m_data->m_ikJacobian->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
+ break;
+ case IK2_SDLS:
+ m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method
+ break;
+ default:
+ m_data->m_ikJacobian->ZeroDeltaThetas();
+ break;
+ }
+
+ // Use for velocity IK, update theta dot
+ //m_data->m_ikJacobian->UpdateThetaDot();
+
+ // Use for position IK, incrementally update theta
+ //m_data->m_ikJacobian->UpdateThetas();
+
+ // Apply the change in the theta values
+ //m_data->m_ikJacobian->UpdatedSClampValue(&targets);
+
+ for (int i=0;i<numQ;i++)
+ {
+ // Use for velocity IK
+ q_new[i] = m_data->m_ikJacobian->dTheta[i] + q_current[i];
+
+ // Use for position IK
+ //q_new[i] = m_data->m_ikNodes[i]->GetTheta();
+ }
+ return true;
+}
+
+bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose)
+{
+ m_data->m_nullSpaceVelocity.SetLength(numQ);
+ m_data->m_nullSpaceVelocity.SetZero();
+ double stayCloseToZeroGain = 0.1;
+ double stayAwayFromLimitsGain = 10.0;
+
+ // Stay close to zero
+ for (int i = 0; i < numQ; ++i)
+ {
+ m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (rest_pose[i]-q_current[i]);
+ }
+
+ // Stay away from joint limits
+ for (int i = 0; i < numQ; ++i) {
+ if (q_current[i] > upper_limit[i]) {
+ m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (upper_limit[i] - q_current[i]) / joint_range[i];
+ }
+ if (q_current[i] < lower_limit[i]) {
+ m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (lower_limit[i] - q_current[i]) / joint_range[i];
+ }
+ }
+ return true;
+}
\ No newline at end of file
diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h
new file mode 100644
index 0000000..71f8eea
--- /dev/null
+++ b/examples/SharedMemory/IKTrajectoryHelper.h
@@ -0,0 +1,36 @@
+#ifndef IK_TRAJECTORY_HELPER_H
+#define IK_TRAJECTORY_HELPER_H
+
+enum IK2_Method
+{
+ IK2_JACOB_TRANS=0,
+ IK2_PURE_PSEUDO,
+ IK2_DLS,
+ IK2_SDLS ,
+ IK2_DLS_SVD,
+ IK2_VEL_DLS,
+ IK2_VEL_DLS_WITH_ORIENTATION,
+ IK2_VEL_DLS_WITH_NULLSPACE,
+ IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE,
+};
+
+
+class IKTrajectoryHelper
+{
+ struct IKTrajectoryHelperInternalData* m_data;
+
+public:
+ IKTrajectoryHelper();
+ virtual ~IKTrajectoryHelper();
+
+ bool computeIK(const double endEffectorTargetPosition[3],
+ const double endEffectorTargetOrientation[4],
+ const double endEffectorWorldPosition[3],
+ const double endEffectorWorldOrientation[4],
+ const double* q_old, int numQ, int endEffectorIndex,
+ double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
+
+ bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose);
+
+};
+#endif //IK_TRAJECTORY_HELPER_H
diff --git a/examples/SharedMemory/InProcessMemory.cpp b/examples/SharedMemory/InProcessMemory.cpp
new file mode 100644
index 0000000..6a8c7ec
--- /dev/null
+++ b/examples/SharedMemory/InProcessMemory.cpp
@@ -0,0 +1,49 @@
+#include "InProcessMemory.h"
+
+
+#include "LinearMath/btHashMap.h"
+
+struct InProcessMemoryInternalData
+{
+ btHashMap<btHashInt, void*> m_memoryPointers;
+
+};
+
+
+InProcessMemory::InProcessMemory()
+{
+ m_data = new InProcessMemoryInternalData;
+}
+
+InProcessMemory::~InProcessMemory()
+{
+ for (int i=0;i<m_data->m_memoryPointers.size();i++)
+ {
+ void** ptrptr = m_data->m_memoryPointers.getAtIndex(i);
+ if (ptrptr)
+ {
+ void* ptr = *ptrptr;
+ free(ptr);
+ }
+ }
+ delete m_data;
+}
+
+void* InProcessMemory::allocateSharedMemory(int key, int size, bool allowCreation)
+{
+ void** ptrptr = m_data->m_memoryPointers[key];
+ if (ptrptr)
+ {
+ return *ptrptr;
+ }
+
+ void* ptr = malloc(size);
+ m_data->m_memoryPointers.insert(key,ptr);
+ return ptr;
+}
+
+void InProcessMemory::releaseSharedMemory(int /*key*/, int /*size*/)
+{
+ //we don't release the memory here, but in the destructor instead,
+ //so multiple users could 'share' the memory given some key
+}
\ No newline at end of file
diff --git a/examples/SharedMemory/InProcessMemory.h b/examples/SharedMemory/InProcessMemory.h
new file mode 100644
index 0000000..40bed81
--- /dev/null
+++ b/examples/SharedMemory/InProcessMemory.h
@@ -0,0 +1,19 @@
+#ifndef IN_PROCESS_MEMORY_H
+#define IN_PROCESS_MEMORY_H
+
+#include "SharedMemoryInterface.h"
+
+class InProcessMemory : public SharedMemoryInterface
+{
+ struct InProcessMemoryInternalData* m_data;
+
+public:
+
+ InProcessMemory();
+ virtual ~InProcessMemory();
+
+ virtual void* allocateSharedMemory(int key, int size, bool allowCreation);
+ virtual void releaseSharedMemory(int key, int size);
+};
+
+#endif
diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h
index ac9e040..3fc0fa1 100644
--- a/examples/SharedMemory/PhysicsClient.h
+++ b/examples/SharedMemory/PhysicsClient.h
@@ -24,9 +24,15 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0;
- virtual int getNumJoints(int bodyIndex) const = 0;
+ virtual int getNumBodies() const = 0;
- virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
+ virtual int getBodyUniqueId(int serialIndex) const = 0;
+
+ virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const = 0;
+
+ virtual int getNumJoints(int bodyUniqueId) const = 0;
+
+ virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0;
@@ -37,6 +43,11 @@ public:
virtual const float* getDebugLinesFrom() const = 0;
virtual const float* getDebugLinesTo() const = 0;
virtual const float* getDebugLinesColor() const = 0;
+
+ virtual void getCachedCameraImage(struct b3CameraImageData* cameraData)=0;
+
+ virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0;
+
};
#endif // BT_PHYSICS_CLIENT_API_H
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 4784c87..7350f76 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -1,10 +1,56 @@
#include "PhysicsClientC_API.h"
#include "PhysicsClientSharedMemory.h"
#include "Bullet3Common/b3Scalar.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+
#include <string.h>
#include "SharedMemoryCommands.h"
+b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+ command->m_type = CMD_LOAD_SDF;
+ int len = strlen(sdfFileName);
+ if (len<MAX_SDF_FILENAME_LENGTH)
+ {
+ strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
+ } else
+ {
+ command->m_sdfArguments.m_sdfFileName[0] = 0;
+ }
+ command->m_updateFlags = SDF_ARGS_FILE_NAME;
+
+ return (b3SharedMemoryCommandHandle) command;
+}
+
+b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+ command->m_type = CMD_SAVE_WORLD;
+ int len = strlen(sdfFileName);
+ if (len<MAX_SDF_FILENAME_LENGTH)
+ {
+ strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
+ } else
+ {
+ command->m_sdfArguments.m_sdfFileName[0] = 0;
+ }
+ command->m_updateFlags = SDF_ARGS_FILE_NAME;
+
+ return (b3SharedMemoryCommandHandle) command;
+}
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
@@ -29,6 +75,27 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie
return (b3SharedMemoryCommandHandle) command;
}
+int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_LOAD_URDF);
+ command->m_updateFlags |=URDF_ARGS_USE_MULTIBODY;
+ command->m_urdfArguments.m_useMultiBody = useMultiBody;
+
+ return 0;
+}
+
+int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_LOAD_SDF);
+ command->m_updateFlags |=URDF_ARGS_USE_MULTIBODY;
+ command->m_sdfArguments.m_useMultiBody = useMultiBody;
+
+ return 0;
+}
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase)
{
@@ -41,8 +108,6 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle,
return 0;
}
-
-
int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -72,7 +137,7 @@ b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
- b3Assert(cl->canSubmitCommand());
+ b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS;
@@ -91,6 +156,24 @@ int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, doub
return 0;
}
+int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+ command->m_physSimParamArgs.m_allowRealTimeSimulation = enableRealTimeSimulation;
+ command->m_updateFlags |= SIM_PARAM_UPDATE_REAL_TIME_SIMULATION;
+ return 0;
+}
+
+int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+ command->m_physSimParamArgs.m_numSolverIterations = numSolverIterations;
+ command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS;
+ return 0;
+}
+
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -100,6 +183,28 @@ int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double
return 0;
}
+int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+ command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS;
+ command->m_physSimParamArgs.m_numSimulationSubSteps = numSubSteps;
+ return 0;
+}
+
+
+int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+ command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP;
+ command->m_physSimParamArgs.m_defaultContactERP = defaultContactERP;
+ return 0;
+}
+
+
+int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
+
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -127,7 +232,12 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand
}
-b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode)
+b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode)
+{
+ return b3JointControlCommandInit2(physClient,0,controlMode);
+}
+
+b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -136,8 +246,12 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy
b3Assert(command);
command->m_type = CMD_SEND_DESIRED_STATE;
command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
- command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = 0;
+ command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId;
command->m_updateFlags = 0;
+ for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+ {
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0;
+ }
return (b3SharedMemoryCommandHandle) command;
}
@@ -146,6 +260,9 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
+
return 0;
}
@@ -154,6 +271,9 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
+
return 0;
}
@@ -162,6 +282,9 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
+
return 0;
}
@@ -170,6 +293,9 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
+
return 0;
}
@@ -179,6 +305,9 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+
return 0;
}
@@ -187,6 +316,9 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+ command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+
return 0;
}
@@ -218,6 +350,28 @@ void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandl
for (int ii(0); ii < 6; ++ii) {
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
}
+ state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
+ }
+}
+
+void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state)
+{
+ const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
+ b3Assert(status);
+ int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
+ b3Assert(bodyIndex>=0);
+ if (bodyIndex>=0)
+ {
+ for (int i = 0; i < 3; ++i)
+ {
+ state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i];
+ state->m_localInertialPosition[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + i];
+ }
+ for (int i = 0; i < 4; ++i)
+ {
+ state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i];
+ state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + 3 + i];
+ }
}
}
@@ -313,6 +467,7 @@ int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
{
+
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
@@ -321,6 +476,11 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl
command->m_type = CMD_INIT_POSE;
command->m_updateFlags =0;
command->m_initPoseArgs.m_bodyUniqueId = bodyIndex;
+ //a bit slow, initialing the full range to zero...
+ for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+ {
+ command->m_initPoseArgs.m_hasInitialStateQ[i] = 0;
+ }
return (b3SharedMemoryCommandHandle) command;
}
@@ -333,6 +493,11 @@ int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle
command->m_initPoseArgs.m_initialStateQ[0] = startPosX;
command->m_initPoseArgs.m_initialStateQ[1] = startPosY;
command->m_initPoseArgs.m_initialStateQ[2] = startPosZ;
+
+ command->m_initPoseArgs.m_hasInitialStateQ[0] = 1;
+ command->m_initPoseArgs.m_hasInitialStateQ[1] = 1;
+ command->m_initPoseArgs.m_hasInitialStateQ[2] = 1;
+
return 0;
}
@@ -346,6 +511,12 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan
command->m_initPoseArgs.m_initialStateQ[4] = startOrnY;
command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ;
command->m_initPoseArgs.m_initialStateQ[6] = startOrnW;
+
+ command->m_initPoseArgs.m_hasInitialStateQ[3] = 1;
+ command->m_initPoseArgs.m_hasInitialStateQ[4] = 1;
+ command->m_initPoseArgs.m_hasInitialStateQ[5] = 1;
+ command->m_initPoseArgs.m_hasInitialStateQ[6] = 1;
+
return 0;
}
@@ -358,6 +529,7 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
for (int i=0;i<numJointPositions;i++)
{
command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i];
+ command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1;
}
return 0;
}
@@ -374,6 +546,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0)
{
command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition;
+ command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1;
}
return 0;
}
@@ -381,7 +554,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
-b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient)
+b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -392,7 +565,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys
command->m_type = CMD_CREATE_SENSOR;
command->m_updateFlags = 0;
command->m_createSensorArguments.m_numJointSensorChanges = 0;
- command->m_createSensorArguments.m_bodyUniqueId = 0;
+ command->m_createSensorArguments.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
@@ -445,9 +618,12 @@ void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient)
b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
- const SharedMemoryStatus* stat = cl->processServerStatus();
- return (b3SharedMemoryStatusHandle) stat;
-
+ if (cl && cl->isConnected())
+ {
+ const SharedMemoryStatus* stat = cl->processServerStatus();
+ return (b3SharedMemoryStatusHandle) stat;
+ }
+ return 0;
}
@@ -461,7 +637,34 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
{
return status->m_type;
}
- return 0;
+ return CMD_INVALID_STATUS;
+}
+
+int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity)
+{
+ int numBodies = 0;
+ const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
+ b3Assert(status);
+
+ if (status)
+ {
+ switch (status->m_type)
+ {
+ case CMD_SDF_LOADING_COMPLETED:
+ {
+ int i,maxBodies;
+ numBodies = status->m_sdfLoadedArgs.m_numBodies;
+ maxBodies = btMin(bodyIndicesCapacity, numBodies);
+ for (i=0;i<maxBodies;i++)
+ {
+ bodyIndicesOut[i] = status->m_sdfLoadedArgs.m_bodyUniqueIds[i];
+ }
+ break;
+ }
+ }
+ }
+
+ return numBodies;
}
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
@@ -493,6 +696,7 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
return bodyId;
}
+
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numDegreeOfFreedomQ,
@@ -503,6 +707,10 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* jointReactionForces[]) {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
const SendActualStateArgs &args = status->m_sendActualStateArgs;
+ btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED);
+ if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+ return false;
+
if (bodyUniqueId) {
*bodyUniqueId = args.m_bodyUniqueId;
}
@@ -560,6 +768,29 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
}
+///return the total number of bodies in the simulation
+int b3GetNumBodies(b3PhysicsClientHandle physClient)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ return cl->getNumBodies();
+}
+
+/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
+int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ return cl->getBodyUniqueId(serialIndex);
+}
+
+///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
+int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ return cl->getBodyInfo(bodyUniqueId,*info);
+}
+
+
+
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -567,10 +798,34 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
}
-void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
+int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
- cl->getJointInfo(bodyIndex, linkIndex,*info);
+ return cl->getJointInfo(bodyIndex, jointIndex, *info);
+}
+
+b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+
+ command->m_type = CMD_CREATE_JOINT;
+ command->m_createJointArguments.m_parentBodyIndex = parentBodyIndex;
+ command->m_createJointArguments.m_parentJointIndex = parentJointIndex;
+ command->m_createJointArguments.m_childBodyIndex = childBodyIndex;
+ command->m_createJointArguments.m_childJointIndex = childJointIndex;
+ for (int i = 0; i < 7; ++i) {
+ command->m_createJointArguments.m_parentFrame[i] = info->m_parentFrame[i];
+ command->m_createJointArguments.m_childFrame[i] = info->m_childFrame[i];
+ }
+ for (int i = 0; i < 3; ++i) {
+ command->m_createJointArguments.m_jointAxis[i] = info->m_jointAxis[i];
+ }
+ command->m_createJointArguments.m_jointType = info->m_jointType;
+ return (b3SharedMemoryCommandHandle)command;
}
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
@@ -651,3 +906,594 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
}
}
+
+///request an image from a simulated camera, using a software renderer.
+b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+ command->m_type =CMD_REQUEST_CAMERA_IMAGE_DATA;
+ command->m_requestPixelDataArguments.m_startPixelIndex = 0;
+ command->m_updateFlags = 0;//REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL;
+ return (b3SharedMemoryCommandHandle) command;
+}
+
+void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+ b3Assert(renderer>(1<<15));
+ command->m_updateFlags |= renderer;
+}
+
+void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16])
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+ for (int i=0;i<16;i++)
+ {
+ command->m_requestPixelDataArguments.m_projectionMatrix[i] = projectionMatrix[i];
+ command->m_requestPixelDataArguments.m_viewMatrix[i] = viewMatrix[i];
+ }
+ command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
+}
+
+void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+ b3Vector3 camUpVector;
+ b3Vector3 camForward;
+ b3Vector3 camPos;
+ b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0],cameraTargetPosition[1],cameraTargetPosition[2]);
+ b3Vector3 eyePos = b3MakeVector3(0,0,0);
+
+ int forwardAxis(-1);
+
+ {
+
+ switch (upAxis)
+ {
+
+ case 1:
+ {
+
+
+ forwardAxis = 0;
+ eyePos[forwardAxis] = -distance;
+ camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]);
+ if (camForward.length2() < B3_EPSILON)
+ {
+ camForward.setValue(1.f,0.f,0.f);
+ } else
+ {
+ camForward.normalize();
+ }
+ b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547);
+ b3Quaternion rollRot(camForward,rollRad);
+
+ camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,1,0));
+ //gLightPos = b3MakeVector3(-50.f,100,30);
+ break;
+ }
+ case 2:
+ {
+
+
+ forwardAxis = 1;
+ eyePos[forwardAxis] = -distance;
+ camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]);
+ if (camForward.length2() < B3_EPSILON)
+ {
+ camForward.setValue(1.f,0.f,0.f);
+ } else
+ {
+ camForward.normalize();
+ }
+
+ b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547);
+ b3Quaternion rollRot(camForward,rollRad);
+
+ camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,0,1));
+ //gLightPos = b3MakeVector3(-50.f,30,100);
+ break;
+ }
+ default:
+ {
+ //b3Assert(0);
+ return;
+ }
+ };
+ }
+
+
+ b3Scalar yawRad = yaw * b3Scalar(0.01745329251994329547);// rads per deg
+ b3Scalar pitchRad = pitch * b3Scalar(0.01745329251994329547);// rads per deg
+
+ b3Quaternion pitchRot(camUpVector,pitchRad);
+
+ b3Vector3 right = camUpVector.cross(camForward);
+ b3Quaternion yawRot(right,-yawRad);
+
+
+
+ eyePos = b3Matrix3x3(pitchRot) * b3Matrix3x3(yawRot) * eyePos;
+ camPos = eyePos;
+ camPos += camTargetPos;
+
+ float camPosf[4] = {camPos[0],camPos[1],camPos[2],0};
+ float camPosTargetf[4] = {camTargetPos[0],camTargetPos[1],camTargetPos[2],0};
+ float camUpf[4] = {camUpVector[0],camUpVector[1],camUpVector[2],0};
+
+ b3RequestCameraImageSetViewMatrix(commandHandle,camPosf,camPosTargetf,camUpf);
+
+}
+void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3])
+{
+ b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]);
+ b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
+ b3Vector3 up = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]);
+ b3Vector3 f = (center - eye).normalized();
+ b3Vector3 u = up.normalized();
+ b3Vector3 s = (f.cross(u)).normalized();
+ u = s.cross(f);
+
+ float viewMatrix[16];
+
+ viewMatrix[0*4+0] = s.x;
+ viewMatrix[1*4+0] = s.y;
+ viewMatrix[2*4+0] = s.z;
+
+ viewMatrix[0*4+1] = u.x;
+ viewMatrix[1*4+1] = u.y;
+ viewMatrix[2*4+1] = u.z;
+
+ viewMatrix[0*4+2] =-f.x;
+ viewMatrix[1*4+2] =-f.y;
+ viewMatrix[2*4+2] =-f.z;
+
+ viewMatrix[0*4+3] = 0.f;
+ viewMatrix[1*4+3] = 0.f;
+ viewMatrix[2*4+3] = 0.f;
+
+ viewMatrix[3*4+0] = -s.dot(eye);
+ viewMatrix[3*4+1] = -u.dot(eye);
+ viewMatrix[3*4+2] = f.dot(eye);
+ viewMatrix[3*4+3] = 1.f;
+
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+ for (int i=0;i<16;i++)
+ {
+ command->m_requestPixelDataArguments.m_viewMatrix[i] = viewMatrix[i];
+ }
+ command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
+
+}
+
+void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal)
+{
+ float frustum[16];
+
+ frustum[0*4+0] = (float(2) * nearVal) / (right - left);
+ frustum[0*4+1] = float(0);
+ frustum[0*4+2] = float(0);
+ frustum[0*4+3] = float(0);
+
+ frustum[1*4+0] = float(0);
+ frustum[1*4+1] = (float(2) * nearVal) / (top - bottom);
+ frustum[1*4+2] = float(0);
+ frustum[1*4+3] = float(0);
+
+ frustum[2*4+0] = (right + left) / (right - left);
+ frustum[2*4+1] = (top + bottom) / (top - bottom);
+ frustum[2*4+2] = -(farVal + nearVal) / (farVal - nearVal);
+ frustum[2*4+3] = float(-1);
+
+ frustum[3*4+0] = float(0);
+ frustum[3*4+1] = float(0);
+ frustum[3*4+2] = -(float(2) * farVal * nearVal) / (farVal - nearVal);
+ frustum[3*4+3] = float(0);
+
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+ for (int i=0;i<16;i++)
+ {
+ command->m_requestPixelDataArguments.m_projectionMatrix[i] = frustum[i];
+ }
+ command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
+}
+
+void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float fov, float aspect, float nearVal, float farVal)
+{
+ float yScale = 1.0 / tan((3.141592538 / 180.0) * fov / 2);
+ float xScale = yScale / aspect;
+
+ float frustum[16];
+
+ frustum[0*4+0] = xScale;
+ frustum[0*4+1] = float(0);
+ frustum[0*4+2] = float(0);
+ frustum[0*4+3] = float(0);
+
+ frustum[1*4+0] = float(0);
+ frustum[1*4+1] = yScale;
+ frustum[1*4+2] = float(0);
+ frustum[1*4+3] = float(0);
+
+ frustum[2*4+0] = 0;
+ frustum[2*4+1] = 0;
+ frustum[2*4+2] = (nearVal + farVal) / (nearVal - farVal);
+ frustum[2*4+3] = float(-1);
+
+ frustum[3*4+0] = float(0);
+ frustum[3*4+1] = float(0);
+ frustum[3*4+2] = (float(2) * farVal * nearVal) / (nearVal - farVal);
+ frustum[3*4+3] = float(0);
+
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+ for (int i=0;i<16;i++)
+ {
+ command->m_requestPixelDataArguments.m_projectionMatrix[i] = frustum[i];
+ }
+ command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
+}
+
+void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height )
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+ command->m_requestPixelDataArguments.m_pixelWidth = width;
+ command->m_requestPixelDataArguments.m_pixelHeight = height;
+ command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT;
+}
+
+void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ if (cl)
+ {
+ cl->getCachedCameraImage(imageData);
+ }
+}
+
+///request an contact point information
+b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+ command->m_type =CMD_REQUEST_CONTACT_POINT_INFORMATION;
+ command->m_requestContactPointArguments.m_startingContactPointIndex = 0;
+ command->m_requestContactPointArguments.m_objectAIndexFilter = -1;
+ command->m_requestContactPointArguments.m_objectBIndexFilter = -1;
+ command->m_updateFlags = 0;
+ return (b3SharedMemoryCommandHandle) command;
+}
+
+void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+ command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA;
+}
+
+void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+ command->m_requestContactPointArguments.m_objectBIndexFilter = bodyUniqueIdB;
+}
+
+void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
+
+
+void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ if (cl)
+ {
+ cl->getCachedContactPointInformation(contactPointData);
+ }
+}
+
+
+b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+
+ command->m_type = CMD_APPLY_EXTERNAL_FORCE;
+ command->m_updateFlags = 0;
+ command->m_externalForceArguments.m_numForcesAndTorques = 0;
+ return (b3SharedMemoryCommandHandle) command;
+}
+
+void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
+ int index = command->m_externalForceArguments.m_numForcesAndTorques;
+ command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
+ command->m_externalForceArguments.m_linkIds[index] = linkId;
+ command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE+flag;
+ for (int i = 0; i < 3; ++i) {
+ command->m_externalForceArguments.m_forcesAndTorques[index+i] = force[i];
+ command->m_externalForceArguments.m_positions[index+i] = position[i];
+ }
+
+ command->m_externalForceArguments.m_numForcesAndTorques++;
+}
+
+void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
+ int index = command->m_externalForceArguments.m_numForcesAndTorques;
+ command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
+ command->m_externalForceArguments.m_linkIds[index] = linkId;
+ command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE+flag;
+
+ for (int i = 0; i < 3; ++i) {
+ command->m_externalForceArguments.m_forcesAndTorques[index+i] = torque[i];
+ }
+ command->m_externalForceArguments.m_numForcesAndTorques++;
+}
+
+
+
+
+///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
+b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
+ const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
+{
+ PhysicsClient* cl = (PhysicsClient*)physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+
+ command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
+ command->m_updateFlags = 0;
+ command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyIndex;
+ int numJoints = cl->getNumJoints(bodyIndex);
+ for (int i = 0; i < numJoints;i++)
+ {
+ command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
+ command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
+ command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i];
+ }
+
+ return (b3SharedMemoryCommandHandle)command;
+}
+
+int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
+ int* bodyUniqueId,
+ int* dofCount,
+ double* jointForces)
+{
+ const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+ btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED);
+ if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
+ return false;
+
+
+ if (dofCount)
+ {
+ *dofCount = status->m_inverseDynamicsResultArgs.m_dofCount;
+ }
+ if (bodyUniqueId)
+ {
+ *bodyUniqueId = status->m_inverseDynamicsResultArgs.m_bodyUniqueId;
+ }
+ if (jointForces)
+ {
+ for (int i = 0; i < status->m_inverseDynamicsResultArgs.m_dofCount; i++)
+ {
+ jointForces[i] = status->m_inverseDynamicsResultArgs.m_jointForces[i];
+ }
+ }
+
+
+ return true;
+}
+
+b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
+{
+ PhysicsClient* cl = (PhysicsClient*)physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+
+ command->m_type = CMD_CALCULATE_JACOBIAN;
+ command->m_updateFlags = 0;
+ command->m_calculateJacobianArguments.m_bodyUniqueId = bodyIndex;
+ command->m_calculateJacobianArguments.m_linkIndex = linkIndex;
+ command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0];
+ command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1];
+ command->m_calculateJacobianArguments.m_localPosition[2] = localPosition[2];
+ int numJoints = cl->getNumJoints(bodyIndex);
+ for (int i = 0; i < numJoints;i++)
+ {
+ command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
+ command->m_calculateJacobianArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
+ command->m_calculateJacobianArguments.m_jointAccelerations[i] = jointAccelerations[i];
+ }
+
+ return (b3SharedMemoryCommandHandle)command;
+}
+
+int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian)
+{
+ const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+ btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED);
+ if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED)
+ return false;
+
+ if (linearJacobian)
+ {
+ for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount*3; i++)
+ {
+ linearJacobian[i] = status->m_jacobianResultArgs.m_linearJacobian[i];
+ }
+ }
+ if (angularJacobian)
+ {
+ for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount*3; i++)
+ {
+ angularJacobian[i] = status->m_jacobianResultArgs.m_angularJacobian[i];
+ }
+
+ }
+
+ return true;
+}
+
+///compute the joint positions to move the end effector to a desired target using inverse kinematics
+b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
+{
+ PhysicsClient* cl = (PhysicsClient*)physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+
+ command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
+ command->m_updateFlags = 0;
+ command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex;
+
+ return (b3SharedMemoryCommandHandle)command;
+
+}
+
+void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3])
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
+ command->m_updateFlags |= IK_HAS_TARGET_POSITION;
+ command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
+
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
+
+
+}
+void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4])
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
+ command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_TARGET_ORIENTATION;
+ command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
+
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
+
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2];
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3];
+
+}
+
+void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
+ command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_NULL_SPACE_VELOCITY;
+ command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
+
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
+
+ for (int i = 0; i < numDof; ++i)
+ {
+ command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
+ command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
+ command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i];
+ command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i];
+ }
+}
+
+void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
+ command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_TARGET_ORIENTATION+IK_HAS_NULL_SPACE_VELOCITY;
+ command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
+
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
+ command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
+
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2];
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3];
+
+ for (int i = 0; i < numDof; ++i)
+ {
+ command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
+ command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
+ command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i];
+ command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i];
+ }
+
+}
+
+int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
+ int* bodyUniqueId,
+ int* dofCount,
+ double* jointPositions)
+{
+ const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+ btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED);
+ if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED)
+ return false;
+
+
+ if (dofCount)
+ {
+ *dofCount = status->m_inverseKinematicsResultArgs.m_dofCount;
+ }
+ if (bodyUniqueId)
+ {
+ *bodyUniqueId = status->m_inverseKinematicsResultArgs.m_bodyUniqueId;
+ }
+ if (jointPositions)
+ {
+ for (int i = 0; i < status->m_inverseKinematicsResultArgs.m_dofCount; i++)
+ {
+ jointPositions[i] = status->m_inverseKinematicsResultArgs.m_jointPositions[i];
+ }
+ }
+
+ return true;
+}
\ No newline at end of file
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index e4b4f2f..b97a021 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -15,25 +15,33 @@ B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle);
extern "C" {
#endif
-///make sure to start the server first, before connecting client to a physics server over shared memory or UDP
+///b3ConnectSharedMemory will connect to a physics server over shared memory, so
+///make sure to start the server first.
+///and a way to spawn an OpenGL 3D GUI physics server and connect (b3CreateInProcessPhysicsServerAndConnect)
b3PhysicsClientHandle b3ConnectSharedMemory(int key);
+///b3DisconnectSharedMemory will disconnect the client from the server and cleanup memory.
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
-///check if a command can be send
+///There can only be 1 outstanding command. Check if a command can be send.
int b3CanSubmitCommand(b3PhysicsClientHandle physClient);
-//blocking submit command and wait for status
+///blocking submit command and wait for status
b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
-///non-blocking submit command
+///In general it is better to use b3SubmitClientCommandAndWaitStatus. b3SubmitClientCommand is a non-blocking submit
+///command, which requires checking for the status manually, using b3ProcessServerStatus. Also, before sending the
+///next command, make sure to check if you can send a command using 'b3CanSubmitCommand'.
int b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
///non-blocking check status
b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient);
+/// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes.
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
+int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
+
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
@@ -45,38 +53,116 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* actualStateQdot[],
const double* jointReactionForces[]);
+///return the total number of bodies in the simulation
+int b3GetNumBodies(b3PhysicsClientHandle physClient);
+
+/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
+int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex);
+
+///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
+int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
+
+///give a unique body index (after loading the body) return the number of joints.
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
-void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
+///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
+int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
+
+b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
+///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
+///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
-
+
+///Get the pointers to the debug line information, after b3InitRequestDebugLinesCommand returns
+///status CMD_DEBUG_LINES_COMPLETED
void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines);
-
+
+///request an image from a simulated camera, using a software renderer.
+b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
+void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
+void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]);
+void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis);
+void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal);
+void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal);
+void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height );
+void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
+void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
+
+///request an contact point information
+b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
+void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
+void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
+void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData);
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
+int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
+int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
+int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
+int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
+
+
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
+///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED.
+///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle);
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
-///all those commands are optional, except for the *Init
+
int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
-///Set joint control variables such as desired position/angle, desired velocity,
-///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
+
+///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
+b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
+ const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
+
+int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
+ int* bodyUniqueId,
+ int* dofCount,
+ double* jointForces);
+
+b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
+
+int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
+
+///compute the joint positions to move the end effector to a desired target using inverse kinematics
+b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
+void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]);
+void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]);
+void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
+void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
+int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
+ int* bodyUniqueId,
+ int* dofCount,
+ double* jointPositions);
+
+b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
+int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
+
+b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
+
+
+///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
+
+
+///Set joint motor control variables such as desired position/angle, desired velocity,
+///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
+b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
+
///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
-//Only use when controlMode is CONTROL_MODE_VELOCITY
+
+///Only use when controlMode is CONTROL_MODE_VELOCITY
int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///Only use if when controlMode is CONTROL_MODE_TORQUE,
@@ -86,8 +172,8 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
///the creation of collision shapes and rigid bodies etc is likely going to change,
///but good to have a b3CreateBoxShapeCommandInit for now
-//create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1)
-//after that, you can optionally adjust the initial position, orientation and size
+///create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1)
+///after that, you can optionally adjust the initial position, orientation and size
b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient);
int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
@@ -97,20 +183,26 @@ int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandH
int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha);
-///Initialize (teleport) the pose of a body/robot. You can individually set the base position, base orientation and joint angles.
-///This will set all velocities of base and joints to zero.
+///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position,
+///base orientation and joint angles. This will set all velocities of base and joints to zero.
+///This is not a robot control command using actuators/joint motors, but manual repositioning the robot.
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
-b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient);
+///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
+///This is rather inconsistent, to mix programmatical creation with loading from file.
+b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
+///b3CreateSensorEnableIMUForLink is not implemented yet.
+///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
+void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
@@ -121,6 +213,11 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d
double rayToWorldZ);
b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
+/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
+b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
+void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
+void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
+
#ifdef __cplusplus
}
#endif
diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp
index 3073403..95faa3f 100644
--- a/examples/SharedMemory/PhysicsClientExample.cpp
+++ b/examples/SharedMemory/PhysicsClientExample.cpp
@@ -2,23 +2,29 @@
#include "PhysicsClientExample.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
-
+#include "../CommonInterfaces/Common2dCanvasInterface.h"
#include "SharedMemoryCommon.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
+
#include "PhysicsClientC_API.h"
#include "PhysicsClient.h"
//#include "SharedMemoryCommands.h"
#include "PhysicsLoopBackC_API.h"
#include "PhysicsDirectC_API.h"
#include "PhysicsClientC_API.h"
-
+#include "PhysicsServerSharedMemory.h"
struct MyMotorInfo2
{
btScalar m_velTarget;
btScalar m_maxForce;
+ btScalar m_posTarget;
int m_uIndex;
+ int m_qIndex;
};
+int camVisualizerWidth = 320;//1024/3;
+int camVisualizerHeight = 240;//768/3;
+
#define MAX_NUM_MOTORS 128
@@ -26,24 +32,39 @@ class PhysicsClientExample : public SharedMemoryCommon
{
protected:
b3PhysicsClientHandle m_physicsClientHandle;
+
+ //this m_physicsServer is only used when option eCLIENTEXAMPLE_SERVER is enabled
+ PhysicsServerSharedMemory m_physicsServer;
bool m_wantsTermination;
btAlignedObjectArray<int> m_userCommandRequests;
+ btAlignedObjectArray<int> m_bodyUniqueIds;
+
+
int m_sharedMemoryKey;
int m_selectedBody;
int m_prevSelectedBody;
+ struct Common2dCanvasInterface* m_canvas;
+ int m_canvasRGBIndex;
+ int m_canvasDepthIndex;
+ int m_canvasSegMaskIndex;
+
void createButton(const char* name, int id, bool isTrigger );
void createButtons();
-public:
+
//@todo, add accessor methods
- MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS];
+ // MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS];
+ MyMotorInfo2 m_motorTargetPositions[MAX_NUM_MOTORS];
int m_numMotors;
+ int m_options;
+ bool m_isOptionalServerConnected;
-
- PhysicsClientExample(GUIHelperInterface* helper);
+ public:
+
+ PhysicsClientExample(GUIHelperInterface* helper, int options);
virtual ~PhysicsClientExample();
virtual void initPhysics();
@@ -51,7 +72,8 @@ public:
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
- int bodyIndex = comboIndex;
+ int itemIndex = int(atoi(name));
+ int bodyIndex = m_bodyUniqueIds[itemIndex];
if (m_selectedBody != bodyIndex)
{
m_selectedBody = bodyIndex;
@@ -62,10 +84,10 @@ public:
virtual void resetCamera()
{
- float dist = 5;
- float pitch = 50;
- float yaw = 35;
- float targetPos[3]={0,0,0};//-3,2.8,-2.5};
+ float dist = 4;
+ float pitch = 193;
+ float yaw = 25;
+ float targetPos[3]={0,0,0.5};//-3,2.8,-2.5};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
@@ -90,6 +112,11 @@ public:
virtual void exitPhysics(){};
virtual void renderScene()
{
+ if (m_options == eCLIENTEXAMPLE_SERVER)
+ {
+ m_physicsServer.renderScene();
+ }
+
b3DebugLines debugLines;
b3GetDebugLines(m_physicsClientHandle,&debugLines);
int numLines = debugLines.m_numDebugLines;
@@ -134,16 +161,32 @@ public:
void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle)
{
+
for (int i=0;i<m_numMotors;i++)
{
- btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
-
- int uIndex = m_motorTargetVelocities[i].m_uIndex;
- b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
- b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
+
+ btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
+ int qIndex = m_motorTargetPositions[i].m_qIndex;
+ int uIndex = m_motorTargetPositions[i].m_uIndex;
+ static int serial=0;
+ serial++;
+ // b3Printf("# motors = %d, cmd[%d] qIndex = %d, uIndex = %d, targetPos = %f", m_numMotors, serial, qIndex,uIndex,targetPos);
+
+ b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
+ b3JointControlSetDesiredVelocity(commandHandle,uIndex,0);
+ b3JointControlSetKp(commandHandle, qIndex, 0.2);
+ b3JointControlSetKd(commandHandle, uIndex, 1.);
+
+ b3JointControlSetMaximumForce(commandHandle,uIndex,5000);
}
}
- virtual void physicsDebugDraw(int debugFlags){}
+ virtual void physicsDebugDraw(int debugFlags)
+ {
+ if (m_options==eCLIENTEXAMPLE_SERVER)
+ {
+ m_physicsServer.physicsDebugDraw(debugFlags);
+ }
+ }
virtual bool mouseMoveCallback(float x,float y){return false;};
virtual bool mouseButtonCallback(int button, int state, float x, float y){return false;}
virtual bool keyboardCallback(int key, int state){return false;}
@@ -158,7 +201,7 @@ public:
void MyComboBoxCallback (int combobox, const char* item, void* userPointer)
{
- b3Printf("Item selected %s", item);
+ //b3Printf("Item selected %s", item);
PhysicsClientExample* cl = (PhysicsClientExample*) userPointer;
b3Assert(cl);
@@ -194,9 +237,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
case CMD_LOAD_URDF:
{
-
- b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_lwr/kuka.urdf");
-
+ b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
//setting the initial position, orientation and other arguments are optional
double startPosX = 0;
static double startPosY = 0;
@@ -205,13 +246,36 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
startPosY += 2.f;
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
-
break;
}
+
+ case CMD_LOAD_SDF:
+ {
+ b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");//kuka_iiwa/model.sdf");
+ b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+ break;
+ }
+ case CMD_REQUEST_CAMERA_IMAGE_DATA:
+ {
+ ///request an image from a simulated camera, using a software renderer.
+
+ b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
+ //b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL);
+
+ float viewMatrix[16];
+ float projectionMatrix[16];
+ m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
+ m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
+
+ b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
+ b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
+ b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+ break;
+ }
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
- b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3);
+ b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-1.5);
b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
@@ -235,7 +299,15 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
if (m_selectedBody>=0)
{
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody);
+ b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+
+ int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
+ for (int i = 0; i < numJoints; ++i) {
+ struct b3JointSensorState sensorState;
+ b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
+ b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
+ }
}
break;
};
@@ -308,10 +380,18 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
}
case CMD_SEND_DESIRED_STATE:
{
- b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
+ if (m_selectedBody>=0)
+ {
+ // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY);
+ b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
+ // b3Printf("prepare control command for body %d", m_selectedBody);
+
prepareControlCommand(command);
+
+
b3SubmitClientCommand(m_physicsClientHandle, command);
- break;
+ }
+ break;
}
case CMD_RESET_SIMULATION:
{
@@ -330,6 +410,54 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
break;
}
+ case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: {
+ b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
+ b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
+ b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+ break;
+ }
+
+ case CMD_CALCULATE_INVERSE_DYNAMICS:
+ {
+ if (m_selectedBody >= 0)
+ {
+ btAlignedObjectArray<double> jointPositionsQ;
+ btAlignedObjectArray<double> jointVelocitiesQdot;
+ btAlignedObjectArray<double> jointAccelerations;
+ int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
+ if (numJoints)
+ {
+ b3Printf("Compute inverse dynamics for joint accelerations:");
+ jointPositionsQ.resize(numJoints);
+ jointVelocitiesQdot.resize(numJoints);
+ jointAccelerations.resize(numJoints);
+ for (int i = 0; i < numJoints; i++)
+ {
+ jointAccelerations[i] = 100;
+ b3Printf("Desired joint acceleration[%d]=%f", i, jointAccelerations[i]);
+ }
+ b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_physicsClientHandle,
+ m_selectedBody, &jointPositionsQ[0], &jointVelocitiesQdot[0], &jointAccelerations[0]);
+ b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+
+ }
+ }
+ break;
+ }
+ case CMD_REQUEST_CONTACT_POINT_INFORMATION:
+ {
+ b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle);
+ b3SetContactFilterBodyA(commandHandle,0);
+ b3SetContactFilterBodyB(commandHandle,1);
+ b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+ break;
+ }
+ case CMD_SAVE_WORLD:
+ {
+ b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py");
+ b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+ break;
+ }
default:
{
b3Error("Unknown buttonId");
@@ -340,14 +468,21 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
-PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper)
+PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper, int options)
:SharedMemoryCommon(helper),
m_physicsClientHandle(0),
m_wantsTermination(false),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_selectedBody(-1),
m_prevSelectedBody(-1),
-m_numMotors(0)
+m_numMotors(0),
+m_options(options),
+m_isOptionalServerConnected(false),
+m_canvas(0),
+m_canvasRGBIndex(-1),
+m_canvasDepthIndex(-1),
+m_canvasSegMaskIndex(-1)
+
{
b3Printf("Started PhysicsClientExample\n");
}
@@ -359,6 +494,23 @@ PhysicsClientExample::~PhysicsClientExample()
b3ProcessServerStatus(m_physicsClientHandle);
b3DisconnectSharedMemory(m_physicsClientHandle);
}
+
+ if (m_options == eCLIENTEXAMPLE_SERVER)
+ {
+ bool deInitializeSharedMemory = true;
+ m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
+ }
+
+ if (m_canvas)
+ {
+ if (m_canvasRGBIndex>=0)
+ m_canvas->destroyCanvas(m_canvasRGBIndex);
+ if (m_canvasDepthIndex>=0)
+ m_canvas->destroyCanvas(m_canvasDepthIndex);
+ if (m_canvasSegMaskIndex>=0)
+ m_canvas->destroyCanvas(m_canvasSegMaskIndex);
+
+ }
b3Printf("~PhysicsClientExample\n");
}
@@ -379,18 +531,54 @@ void PhysicsClientExample::createButtons()
m_guiHelper->getParameterInterface()->removeAllParameters();
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
+ createButton("Load SDF",CMD_LOAD_SDF, isTrigger);
+ createButton("Save World",CMD_SAVE_WORLD, isTrigger);
+ createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
- createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
+ if (m_options!=eCLIENTEXAMPLE_SERVER)
+ {
+ createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
+ }
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
- createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger);
+ createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger);
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
- createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
+ createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
+ createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
+ createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger);
+ createButton("Get Contact Point Info", CMD_REQUEST_CONTACT_POINT_INFORMATION, isTrigger);
+ if (m_bodyUniqueIds.size())
+ {
+ if (m_selectedBody<0)
+ m_selectedBody = 0;
+
+ ComboBoxParams comboParams;
+ comboParams.m_comboboxId = 0;
+ comboParams.m_numItems = m_bodyUniqueIds.size();
+ comboParams.m_startItem = m_selectedBody;
+ comboParams.m_callback = MyComboBoxCallback;
+ comboParams.m_userPointer = this;
+ //todo: get the real object name
+
+ const char** blarray = new const char*[m_bodyUniqueIds.size()];
+
+ for (int i=0;i<m_bodyUniqueIds.size();i++)
+ {
+ char* bla = new char[16];
+ sprintf(bla,"%d", i);
+ blarray[i] = bla;
+ comboParams.m_items=blarray;//{&bla};
+ }
+ m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
+ }
+
if (m_physicsClientHandle && m_selectedBody>=0)
{
+ m_numMotors = 0;
+
int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
for (int i=0;i<numJoints;i++)
{
@@ -403,14 +591,20 @@ void PhysicsClientExample::createButtons()
if (m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
- sprintf(motorName,"%s q'", info.m_jointName);
- MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
+ sprintf(motorName,"%s q", info.m_jointName);
+ // MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
+ MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors];
motorInfo->m_velTarget = 0.f;
+ motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
+ motorInfo->m_qIndex = info.m_qIndex;
- SliderParams slider(motorName,&motorInfo->m_velTarget);
- slider.m_minVal=-4;
- slider.m_maxVal=4;
+ // SliderParams slider(motorName,&motorInfo->m_velTarget);
+ // slider.m_minVal=-4;
+ // slider.m_maxVal=4;
+ SliderParams slider(motorName,&motorInfo->m_posTarget);
+ slider.m_minVal=-4;
+ slider.m_maxVal=4;
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
@@ -438,17 +632,65 @@ void PhysicsClientExample::initPhysics()
{
MyCallback(CMD_LOAD_URDF, true, this);
MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
- MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
+
MyCallback(CMD_RESET_SIMULATION,true,this);
}
m_selectedBody = -1;
m_prevSelectedBody = -1;
- //m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
- m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
- //m_physicsClientHandle = b3ConnectPhysicsDirect();
+ {
+ m_canvas = m_guiHelper->get2dCanvasInterface();
+ if (m_canvas)
+ {
+
+ m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",camVisualizerWidth, camVisualizerHeight);
+ m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",camVisualizerWidth, camVisualizerHeight);
+ m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",camVisualizerWidth, camVisualizerHeight);
+ for (int i=0;i<camVisualizerWidth;i++)
+ {
+ for (int j=0;j<camVisualizerHeight;j++)
+ {
+ unsigned char red=255;
+ unsigned char green=255;
+ unsigned char blue=255;
+ unsigned char alpha=255;
+ if (i==j)
+ {
+ red = 0;
+ green=0;
+ blue=0;
+ }
+ m_canvas->setPixel(m_canvasRGBIndex,i,j,red,green,blue,alpha);
+ m_canvas->setPixel(m_canvasDepthIndex,i,j,red,green,blue,alpha);
+ m_canvas->setPixel(m_canvasSegMaskIndex,i,j,red,green,blue,alpha);
+
+ }
+ }
+ m_canvas->refreshImageData(m_canvasRGBIndex);
+ m_canvas->refreshImageData(m_canvasDepthIndex);
+ m_canvas->refreshImageData(m_canvasSegMaskIndex);
+
+
+ }
+
+ }
+
+ if (m_options == eCLIENTEXAMPLE_SERVER)
+ {
+ m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
+ }
+
+ if (m_options == eCLIENTEXAMPLE_DIRECT)
+ {
+ m_physicsClientHandle = b3ConnectPhysicsDirect();
+ } else
+ {
+ m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
+ //m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
+ }
+
if (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3Warning("Cannot connect to physics client");
@@ -459,6 +701,15 @@ void PhysicsClientExample::initPhysics()
void PhysicsClientExample::stepSimulation(float deltaTime)
{
+
+ if (m_options == eCLIENTEXAMPLE_SERVER)
+ {
+ for (int i=0;i<100;i++)
+ {
+ m_physicsServer.processClientCommands();
+ }
+ }
+
if (m_prevSelectedBody != m_selectedBody)
{
createButtons();
@@ -477,10 +728,198 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
{
//b3Printf("bla\n");
}
- if (statusType == CMD_URDF_LOADING_COMPLETED)
+ if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
+ {
+ // static int counter=0;
+ // char msg[1024];
+ // sprintf(msg,"Camera image %d OK\n",counter++);
+ b3CameraImageData imageData;
+ b3GetCameraImageData(m_physicsClientHandle,&imageData);
+ if (m_canvas)
+ {
+
+ //compute depth image range
+ float minDepthValue = 1e20f;
+ float maxDepthValue = -1e20f;
+
+ for (int i=0;i<camVisualizerWidth;i++)
+ {
+ for (int j=0;j<camVisualizerHeight;j++)
+ {
+ int xIndex = int(float(i)*(float(imageData.m_pixelWidth)/float(camVisualizerWidth)));
+ int yIndex = int(float(j)*(float(imageData.m_pixelHeight)/float(camVisualizerHeight)));
+ btClamp(yIndex,0,imageData.m_pixelHeight);
+ btClamp(xIndex,0,imageData.m_pixelWidth);
+
+ if (m_canvasDepthIndex >=0)
+ {
+ int depthPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth);
+ float depthValue = imageData.m_depthValues[depthPixelIndex];
+ //todo: rescale the depthValue to [0..255]
+ if (depthValue>-1e20)
+ {
+ maxDepthValue = btMax(maxDepthValue,depthValue);
+ minDepthValue = btMin(minDepthValue,depthValue);
+ }
+ }
+ }
+ }
+
+ for (int i=0;i<camVisualizerWidth;i++)
+ {
+ for (int j=0;j<camVisualizerHeight;j++)
+ {
+ int xIndex = int(float(i)*(float(imageData.m_pixelWidth)/float(camVisualizerWidth)));
+ int yIndex = int(float(j)*(float(imageData.m_pixelHeight)/float(camVisualizerHeight)));
+ btClamp(yIndex,0,imageData.m_pixelHeight);
+ btClamp(xIndex,0,imageData.m_pixelWidth);
+ int bytesPerPixel = 4; //RGBA
+
+ if (m_canvasRGBIndex >=0)
+ {
+ int rgbPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel;
+ m_canvas->setPixel(m_canvasRGBIndex,i,j,
+ imageData.m_rgbColorData[rgbPixelIndex],
+ imageData.m_rgbColorData[rgbPixelIndex+1],
+ imageData.m_rgbColorData[rgbPixelIndex+2],
+ 255); //alpha set to 255
+ }
+
+ if (m_canvasDepthIndex >=0)
+ {
+ int depthPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth);
+ float depthValue = imageData.m_depthValues[depthPixelIndex];
+ //todo: rescale the depthValue to [0..255]
+ if (depthValue>-1e20)
+ {
+ int rgb = (depthValue-minDepthValue)*(255. / (btFabs(maxDepthValue-minDepthValue)));
+ if (rgb<0 || rgb>255)
+ {
+
+ printf("rgb=%d\n",rgb);
+ }
+
+ m_canvas->setPixel(m_canvasDepthIndex,i,j,
+ rgb,
+ rgb,
+ 255, 255); //alpha set to 255
+ } else
+ {
+ m_canvas->setPixel(m_canvasDepthIndex,i,j,
+ 0,
+ 0,
+ 0, 255); //alpha set to 255
+ }
+ }
+ if (m_canvasSegMaskIndex>=0 && (0!=imageData.m_segmentationMaskValues))
+ {
+ int segmentationMaskPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth);
+ int segmentationMask = imageData.m_segmentationMaskValues[segmentationMaskPixelIndex];
+ btVector4 palette[4] = {btVector4(32,255,32,255),
+ btVector4(32,32,255,255),
+ btVector4(255,255,32,255),
+ btVector4(32,255,255,255)};
+ if (segmentationMask>=0)
+ {
+ btVector4 rgb = palette[segmentationMask&3];
+ m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
+ rgb.x(),
+ rgb.y(),
+ rgb.z(), 255); //alpha set to 255
+ } else
+ {
+ m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
+ 0,
+ 0,
+ 0, 255); //alpha set to 255
+ }
+ }
+ }
+ }
+ if (m_canvasRGBIndex >=0)
+ m_canvas->refreshImageData(m_canvasRGBIndex);
+ if (m_canvasDepthIndex >=0)
+ m_canvas->refreshImageData(m_canvasDepthIndex);
+ if (m_canvasSegMaskIndex >=0)
+ m_canvas->refreshImageData(m_canvasSegMaskIndex);
+
+
+
+ }
+
+ // b3Printf(msg);
+ }
+ if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
{
- int bodyIndex = b3GetStatusBodyIndex(status);
- if (bodyIndex>=0)
+ int bodyUniqueId;
+ int dofCount;
+
+ b3GetStatusInverseDynamicsJointForces(status,
+ &bodyUniqueId,
+ &dofCount,
+ 0);
+
+ btAlignedObjectArray<double> jointForces;
+ if (dofCount)
+ {
+ jointForces.resize(dofCount);
+ b3GetStatusInverseDynamicsJointForces(status,
+ 0,
+ 0,
+ &jointForces[0]);
+ for (int i = 0; i < dofCount; i++)
+ {
+ b3Printf("jointForces[%d]=%f", i, jointForces[i]);
+ }
+ }
+
+ }
+ if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_FAILED)
+ {
+ b3Warning("Inverse Dynamics computations failed");
+ }
+
+ if (statusType == CMD_CAMERA_IMAGE_FAILED)
+ {
+ b3Warning("Camera image FAILED\n");
+ }
+
+ if (statusType == CMD_SDF_LOADING_COMPLETED)
+ {
+ int bodyIndicesOut[1024];
+ int bodyCapacity = 1024;
+ int numBodies = b3GetStatusBodyIndices(status, bodyIndicesOut, bodyCapacity);
+ if (numBodies > bodyCapacity)
+ {
+ b3Warning("loadSDF number of bodies (%d) exceeds the internal body capacity (%d)",numBodies, bodyCapacity);
+ } else
+ {
+ for (int i=0;i<numBodies;i++)
+ {
+ int bodyUniqueId = bodyIndicesOut[i];
+ m_bodyUniqueIds.push_back(bodyUniqueId);
+ int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
+ if (numJoints>0)
+ {
+ m_selectedBody = bodyUniqueId;
+ }
+/* int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
+ b3Printf("numJoints = %d", numJoints);
+ for (int i=0;i<numJoints;i++)
+ {
+ b3JointInfo info;
+ b3GetJointInfo(m_physicsClientHandle,bodyUniqueId,i,&info);
+ b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
+ }
+
+ */
+ }
+ }
+
+ //int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
+
+ //int bodyIndex = b3GetStatusBodyIndex(status);
+ /*if (bodyIndex>=0)
{
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
@@ -489,7 +928,6 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
-
}
ComboBoxParams comboParams;
comboParams.m_comboboxId = bodyIndex;
@@ -503,12 +941,40 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
comboParams.m_items=blarray;//{&bla};
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
-
-
}
+ */
+ }
+
+
+ if (statusType == CMD_URDF_LOADING_COMPLETED)
+ {
+ int bodyIndex = b3GetStatusBodyIndex(status);
+ if (bodyIndex>=0)
+ {
+ m_bodyUniqueIds.push_back(bodyIndex);
+ m_selectedBody = bodyIndex;
+ int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
+
+ for (int i=0;i<numJoints;i++)
+ {
+ b3JointInfo info;
+ b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
+ b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
+ }
+
+ }
+ }
+ if (statusType == CMD_CONTACT_POINT_INFORMATION_FAILED)
+ {
+ b3Warning("Cannot get contact information");
+ }
+ if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
+ {
+ b3ContactInformation contactPointData;
+ b3GetContactPointInformation(m_physicsClientHandle, &contactPointData);
+ b3Printf("Num Contacts: %d\n", contactPointData.m_numContactPoints);
}
-
}
}
if (b3CanSubmitCommand(m_physicsClientHandle))
@@ -531,21 +997,43 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
{
m_selectedBody = -1;
m_numMotors=0;
+ m_bodyUniqueIds.clear();
createButtons();
- }
-
-
- prepareAndSubmitCommand(commandId);
+ b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
+ if (m_options == eCLIENTEXAMPLE_SERVER)
+ {
+ b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+ while (!b3CanSubmitCommand(m_physicsClientHandle))
+ {
+ m_physicsServer.processClientCommands();
+ b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
+ bool hasStatus = (status != 0);
+ if (hasStatus)
+ {
+ //int statusType = b3GetStatusType(status);
+ //b3Printf("Status after reset: %d",statusType);
+ }
+ }
+ } else
+ {
+ prepareAndSubmitCommand(commandId);
+ }
+ } else
+ {
+ prepareAndSubmitCommand(commandId);
+ }
} else
{
if (m_numMotors)
{
enqueueCommand(CMD_SEND_DESIRED_STATE);
- enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
- enqueueCommand(CMD_REQUEST_DEBUG_LINES);
- //enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
}
+ enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
+ if (m_options != eCLIENTEXAMPLE_SERVER)
+ {
+ //enqueueCommand(CMD_REQUEST_DEBUG_LINES);
+ }
}
}
@@ -557,10 +1045,11 @@ extern int gSharedMemoryKey;
class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options)
{
- PhysicsClientExample* example = new PhysicsClientExample(options.m_guiHelper);
+ PhysicsClientExample* example = new PhysicsClientExample(options.m_guiHelper, options.m_option);
if (gSharedMemoryKey>=0)
{
example->setSharedMemoryKey(gSharedMemoryKey);
}
return example;
}
+
diff --git a/examples/SharedMemory/PhysicsClientExample.h b/examples/SharedMemory/PhysicsClientExample.h
index 910d8e5..a36a37e 100644
--- a/examples/SharedMemory/PhysicsClientExample.h
+++ b/examples/SharedMemory/PhysicsClientExample.h
@@ -1,6 +1,13 @@
#ifndef PHYSICS_CLIENT_EXAMPLE_H
#define PHYSICS_CLIENT_EXAMPLE_H
+enum ClientExampleOptions
+{
+ eCLIENTEXAMPLE_LOOPBACK=1,
+ eCLIENTEXAMPLE_DIRECT=2,
+ eCLIENTEXAMPLE_SERVER=3,
+};
+
class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options);
#endif//PHYSICS_CLIENT_EXAMPLE_H
diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
index 35d7d82..c9fad2a 100644
--- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
@@ -18,11 +18,13 @@
struct BodyJointInfoCache
{
+ std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
};
struct PhysicsClientSharedMemoryInternalData {
SharedMemoryInterface* m_sharedMemory;
+ bool m_ownsSharedMemory;
SharedMemoryBlock* m_testBlock1;
btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
@@ -31,10 +33,21 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
+ int m_cachedCameraPixelsWidth;
+ int m_cachedCameraPixelsHeight;
+ btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
+ btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
+ btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
+
+ btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
+
+ btAlignedObjectArray<int> m_bodyIdsRequestInfo;
+ SharedMemoryStatus m_tempBackupServerStatus;
+
SharedMemoryStatus m_lastServerStatus;
int m_counter;
- bool m_serverLoadUrdfOK;
+
bool m_isConnected;
bool m_waitingForServer;
bool m_hasLastServerStatus;
@@ -43,9 +56,12 @@ struct PhysicsClientSharedMemoryInternalData {
PhysicsClientSharedMemoryInternalData()
: m_sharedMemory(0),
+ m_ownsSharedMemory(false),
m_testBlock1(0),
- m_counter(0),
- m_serverLoadUrdfOK(false),
+ m_counter(0),
+ m_cachedCameraPixelsWidth(0),
+ m_cachedCameraPixelsHeight(0),
+
m_isConnected(false),
m_waitingForServer(false),
m_hasLastServerStatus(false),
@@ -59,30 +75,57 @@ struct PhysicsClientSharedMemoryInternalData {
+int PhysicsClientSharedMemory::getNumBodies() const
+{
+ return m_data->m_bodyJointMap.size();
+}
+
+int PhysicsClientSharedMemory::getBodyUniqueId(int serialIndex) const
+{
+ if ((serialIndex >= 0) && (serialIndex < getNumBodies()))
+ {
+ return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1();
+ }
+ return -1;
+}
-int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
+bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
{
- BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
+ BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
+ if (bodyJointsPtr && *bodyJointsPtr)
+ {
+ BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
+ info.m_baseName = bodyJoints->m_baseName.c_str();
+ return true;
+ }
+ return false;
+}
+
+
+
+int PhysicsClientSharedMemory::getNumJoints(int bodyUniqueId) const
+{
+ BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
return bodyJoints->m_jointInfo.size();
}
- btAssert(0);
return 0;
}
-void PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
+bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo& info) const
{
- BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
+ BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
info = bodyJoints->m_jointInfo[jointIndex];
+ return true;
}
-
+ return false;
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
@@ -95,23 +138,40 @@ PhysicsClientSharedMemory::PhysicsClientSharedMemory()
#else
m_data->m_sharedMemory = new PosixSharedMemory();
#endif
+ m_data->m_ownsSharedMemory = true;
+
}
PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
if (m_data->m_isConnected) {
disconnectSharedMemory();
}
- delete m_data->m_sharedMemory;
+ if (m_data->m_ownsSharedMemory)
+ {
+ delete m_data->m_sharedMemory;
+ }
delete m_data;
}
void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; }
+
+void PhysicsClientSharedMemory::setSharedMemoryInterface(class SharedMemoryInterface* sharedMem)
+{
+ if (m_data->m_sharedMemory && m_data->m_ownsSharedMemory)
+ {
+ delete m_data->m_sharedMemory;
+ }
+ m_data->m_ownsSharedMemory = false;
+ m_data->m_sharedMemory = sharedMem;
+}
+
void PhysicsClientSharedMemory::disconnectSharedMemory() {
- if (m_data->m_isConnected) {
+ if (m_data->m_isConnected && m_data->m_sharedMemory) {
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
- m_data->m_isConnected = false;
}
+ m_data->m_isConnected = false;
+
}
bool PhysicsClientSharedMemory::isConnected() const { return m_data->m_isConnected; }
@@ -142,17 +202,67 @@ bool PhysicsClientSharedMemory::connect() {
return true;
}
+
+///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo
+void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
+{
+ bParse::btBulletFile bf(
+ &this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],
+ serverCmd.m_dataStreamArguments.m_streamChunkLength);
+ bf.setFileDNAisMemoryDNA();
+ bf.parse(false);
+
+
+ BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
+ m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
+
+ for (int i = 0; i < bf.m_multiBodies.size(); i++)
+ {
+ int flag = bf.getFlags();
+ if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
+ {
+ Bullet::btMultiBodyDoubleData* mb =
+ (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
+
+ bodyJoints->m_baseName = mb->m_baseName;
+ addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
+ } else
+ {
+ Bullet::btMultiBodyFloatData* mb =
+ (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
+ bodyJoints->m_baseName = mb->m_baseName;
+ addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
+ }
+ }
+ if (bf.ok()) {
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("Received robot description ok!\n");
+ }
+ } else
+ {
+ b3Warning("Robot description not received");
+ }
+}
+
const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
SharedMemoryStatus* stat = 0;
if (!m_data->m_testBlock1) {
- return 0;
+ m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
+ return &m_data->m_lastServerStatus;
}
if (!m_data->m_waitingForServer) {
return 0;
}
+ if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER)
+ {
+ m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
+ return &m_data->m_lastServerStatus;
+ }
+
if (m_data->m_testBlock1->m_numServerCommands >
m_data->m_testBlock1->m_numProcessedServerCommands) {
btAssert(m_data->m_testBlock1->m_numServerCommands ==
@@ -171,8 +281,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
break;
}
+ case CMD_SDF_LOADING_COMPLETED: {
+
+ if (m_data->m_verboseOutput) {
+ b3Printf("Server loading the SDF OK\n");
+ }
+
+ break;
+ }
case CMD_URDF_LOADING_COMPLETED: {
- m_data->m_serverLoadUrdfOK = true;
+
if (m_data->m_verboseOutput) {
b3Printf("Server loading the URDF OK\n");
}
@@ -183,10 +301,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
- int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
+ int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
- m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints);
+ m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++) {
@@ -232,7 +350,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_verboseOutput) {
b3Printf("Server failed loading the URDF...\n");
}
- m_data->m_serverLoadUrdfOK = false;
+
+ break;
+ }
+
+ case CMD_BODY_INFO_COMPLETED:
+ {
+ if (m_data->m_verboseOutput) {
+ b3Printf("Received body info\n");
+ }
+ int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
+ processBodyJointInfo(bodyUniqueId, serverCmd);
+
+ break;
+ }
+ case CMD_SDF_LOADING_FAILED: {
+ if (m_data->m_verboseOutput) {
+ b3Printf("Server failed loading the SDF...\n");
+ }
+
break;
}
@@ -390,9 +526,116 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
+
+ case CMD_CAMERA_IMAGE_COMPLETED:
+ {
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("Camera image OK\n");
+ }
+
+ int numBytesPerPixel = 4;//RGBA
+ int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+
+ serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+
+ serverCmd.m_sendPixelDataArguments.m_numRemainingPixels;
+
+ m_data->m_cachedCameraPixelsWidth = 0;
+ m_data->m_cachedCameraPixelsHeight = 0;
+
+ int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight;
+
+ m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
+ m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
+ m_data->m_cachedSegmentationMaskBuffer.resize(numTotalPixels);
+ m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
+
+
+ unsigned char* rgbaPixelsReceived =
+ (unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
+ // printf("pixel = %d\n", rgbaPixelsReceived[0]);
+
+ float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
+ int* segmentationMaskBuffer = (int*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]);
+
+ for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
+ {
+ m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
+ }
+
+ for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
+ {
+ m_data->m_cachedSegmentationMaskBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i];
+ }
+
+ for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
+ {
+ m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
+ = rgbaPixelsReceived[i];
+ }
+
+ break;
+ }
+
+ case CMD_CAMERA_IMAGE_FAILED:
+ {
+ b3Warning("Camera image FAILED\n");
+ break;
+ }
+ case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED:
+ {
+ break;
+ }
+ case CMD_CALCULATED_INVERSE_DYNAMICS_FAILED:
+ {
+ b3Warning("Inverse Dynamics computations failed");
+ break;
+ }
+ case CMD_CONTACT_POINT_INFORMATION_COMPLETED:
+ {
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("Contact Point Information Request OK\n");
+ }
+ int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex;
+ int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
+
+ m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied);
+
+ b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
+
+ for (int i=0;i<numContactsCopied;i++)
+ {
+ m_data->m_cachedContactPoints[startContactIndex+i] = contactData[i];
+ }
+
+ break;
+ }
+ case CMD_CONTACT_POINT_INFORMATION_FAILED:
+ {
+ b3Warning("Contact Point Information Request failed");
+ break;
+ }
+
+ case CMD_SAVE_WORLD_COMPLETED:
+ break;
+
+ case CMD_SAVE_WORLD_FAILED:
+ {
+ b3Warning("Saving world failed");
+ break;
+ }
+ case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
+ {
+ break;
+ }
+ case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED:
+ {
+ b3Warning("Calculate Inverse Kinematics Request failed");
+ break;
+ }
default: {
- b3Error("Unknown server status\n");
+ b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);
}
};
@@ -409,6 +652,88 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
m_data->m_waitingForServer = true;
}
+ if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED)
+ {
+ int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
+ if (numBodies>0)
+ {
+ m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
+
+ for (int i=0;i<numBodies;i++)
+ {
+ m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]);
+ }
+
+ int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
+ m_data->m_bodyIdsRequestInfo.pop_back();
+
+ SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
+ //now transfer the information of the individual objects etc.
+ command.m_type = CMD_REQUEST_BODY_INFO;
+ command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
+ submitClientCommand(command);
+ return 0;
+ }
+ }
+
+ if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
+ {
+ //are there any bodies left to be processed?
+ if (m_data->m_bodyIdsRequestInfo.size())
+ {
+ int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
+ m_data->m_bodyIdsRequestInfo.pop_back();
+
+ SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
+ //now transfer the information of the individual objects etc.
+ command.m_type = CMD_REQUEST_BODY_INFO;
+ command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
+ submitClientCommand(command);
+ return 0;
+ } else
+ {
+ m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
+ }
+ }
+
+ if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
+ {
+ SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
+ if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
+ {
+ command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
+ command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
+ command.m_requestContactPointArguments.m_objectAIndexFilter = -1;
+ command.m_requestContactPointArguments.m_objectBIndexFilter = -1;
+ submitClientCommand(command);
+ return 0;
+ }
+ }
+
+ if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
+ {
+ SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
+
+ if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied)
+ {
+
+
+ // continue requesting remaining pixels
+ command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
+ command.m_requestPixelDataArguments.m_startPixelIndex =
+ serverCmd.m_sendPixelDataArguments.m_startingPixelIndex +
+ serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
+ submitClientCommand(command);
+ return 0;
+ } else
+ {
+ m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
+ m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
+ }
+
+
+ }
+
if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) &&
(serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)) {
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
@@ -439,6 +764,8 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const {
}
struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() {
+ static int sequence = 0;
+ m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++;
return &m_data->m_testBlock1->m_clientCommands[0];
}
@@ -446,7 +773,6 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
/// at the moment we allow a maximum of 1 outstanding command, so we check for this
// once the server processed the command and returns a status, we clear the flag
// "m_data->m_waitingForServer" and allow submitting the next command
- btAssert(!m_data->m_waitingForServer);
if (!m_data->m_waitingForServer) {
if (&m_data->m_testBlock1->m_clientCommands[0] != &command) {
@@ -471,6 +797,22 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data,
}
}
+void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* cameraData)
+{
+ cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
+ cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
+ cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
+ cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
+ cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size()?&m_data->m_cachedSegmentationMaskBuffer[0] : 0;
+}
+
+void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
+{
+ contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
+ contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0;
+
+}
+
const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
if (m_data->m_debugLinesFrom.size()) {
return &m_data->m_debugLinesFrom[0].m_x;
diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h
index 05a4173..1d0047b 100644
--- a/examples/SharedMemory/PhysicsClientSharedMemory.h
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.h
@@ -10,6 +10,10 @@ class PhysicsClientSharedMemory : public PhysicsClient {
struct PhysicsClientSharedMemoryInternalData* m_data;
protected:
+ virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
+ void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
+
+
public:
PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory();
@@ -30,9 +34,15 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
- virtual int getNumJoints(int bodyIndex) const;
+ virtual int getNumBodies() const;
+
+ virtual int getBodyUniqueId(int serialIndex) const;
+
+ virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
+
+ virtual int getNumJoints(int bodyUniqueId) const;
- virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
+ virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key);
@@ -43,6 +53,9 @@ public:
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
+ virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
+
+ virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
};
#endif // BT_PHYSICS_CLIENT_API_H
diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp
index f6f0adf..3367b83 100644
--- a/examples/SharedMemory/PhysicsDirect.cpp
+++ b/examples/SharedMemory/PhysicsDirect.cpp
@@ -9,10 +9,11 @@
#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
#include "BodyJointInfoUtility.h"
-
+#include <string>
struct BodyJointInfoCache2
{
+ std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
};
@@ -33,6 +34,16 @@ struct PhysicsDirectInternalData
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
+ int m_cachedCameraPixelsWidth;
+ int m_cachedCameraPixelsHeight;
+ btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
+ btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
+ btAlignedObjectArray<int> m_cachedSegmentationMask;
+
+ btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
+
+
+
PhysicsServerCommandProcessor* m_commandProcessor;
PhysicsDirectInternalData()
@@ -61,9 +72,27 @@ PhysicsDirect::~PhysicsDirect()
bool PhysicsDirect::connect()
{
m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx);
+
return true;
}
+// return true if connection succesfull, can also check 'isConnected'
+bool PhysicsDirect::connect(struct GUIHelperInterface* guiHelper)
+{
+ m_data->m_commandProcessor->setGuiHelper(guiHelper);
+
+ return true;
+}
+
+void PhysicsDirect::renderScene()
+{
+ m_data->m_commandProcessor->renderScene();
+}
+void PhysicsDirect::debugDraw(int debugDrawMode)
+{
+ m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode);
+}
+
////todo: rename to 'disconnect'
void PhysicsDirect::disconnectSharedMemory()
{
@@ -167,6 +196,174 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
return m_data->m_hasStatus;
}
+bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand)
+{
+ SharedMemoryCommand command = orgCommand;
+
+ const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+
+ do
+ {
+ bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+ m_data->m_hasStatus = hasStatus;
+ if (hasStatus)
+ {
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("Contact Point Information Request OK\n");
+ }
+ int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex;
+ int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
+
+ m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied);
+
+ b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0];
+
+ for (int i=0;i<numContactsCopied;i++)
+ {
+ m_data->m_cachedContactPoints[startContactIndex+i] = contactData[i];
+ }
+
+ if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
+ {
+ command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
+ command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
+ command.m_requestContactPointArguments.m_objectAIndexFilter = -1;
+ command.m_requestContactPointArguments.m_objectBIndexFilter = -1;
+
+ }
+
+ }
+ } while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied);
+
+ return m_data->m_hasStatus;
+
+}
+
+
+bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
+{
+ SharedMemoryCommand command = orgCommand;
+
+ const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+
+ do
+ {
+
+ bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+ m_data->m_hasStatus = hasStatus;
+ if (hasStatus)
+ {
+ btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED);
+
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("Camera image OK\n");
+ }
+
+ int numBytesPerPixel = 4;//RGBA
+ int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+
+ serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+
+ serverCmd.m_sendPixelDataArguments.m_numRemainingPixels;
+
+ m_data->m_cachedCameraPixelsWidth = 0;
+ m_data->m_cachedCameraPixelsHeight = 0;
+
+ int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight;
+
+ m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
+ m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
+ m_data->m_cachedSegmentationMask.resize(numTotalPixels);
+ m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
+
+
+ unsigned char* rgbaPixelsReceived =
+ (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0];
+
+ float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
+ int* segmentationMaskBuffer = (int*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]);
+
+ // printf("pixel = %d\n", rgbaPixelsReceived[0]);
+
+ for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
+ {
+ m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
+ }
+ for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
+ {
+ m_data->m_cachedSegmentationMask[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i];
+ }
+ for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
+ {
+ m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
+ = rgbaPixelsReceived[i];
+ }
+
+ if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied)
+ {
+
+
+ // continue requesting remaining pixels
+ command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
+ command.m_requestPixelDataArguments.m_startPixelIndex =
+ serverCmd.m_sendPixelDataArguments.m_startingPixelIndex +
+ serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
+
+ } else
+ {
+ m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
+ m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
+ }
+ }
+ } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied);
+
+ return m_data->m_hasStatus;
+
+
+}
+
+
+void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
+{
+ bParse::btBulletFile bf(
+ &m_data->m_bulletStreamDataServerToClient[0],
+ serverCmd.m_dataStreamArguments.m_streamChunkLength);
+ bf.setFileDNAisMemoryDNA();
+ bf.parse(false);
+
+ BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
+ m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
+
+ for (int i = 0; i < bf.m_multiBodies.size(); i++)
+ {
+ int flag = bf.getFlags();
+ if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
+ {
+ Bullet::btMultiBodyDoubleData* mb =
+ (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
+ bodyJoints->m_baseName = mb->m_baseName;
+
+ addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
+ } else
+ {
+ Bullet::btMultiBodyFloatData* mb =
+ (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
+
+ bodyJoints->m_baseName = mb->m_baseName;
+ addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
+ }
+ }
+ if (bf.ok()) {
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("Received robot description ok!\n");
+ }
+ } else
+ {
+ b3Warning("Robot description not received");
+ }
+}
+
bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
{
if (command.m_type==CMD_REQUEST_DEBUG_LINES)
@@ -174,6 +371,15 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
return processDebugLines(command);
}
+ if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA)
+ {
+ return processCamera(command);
+ }
+ if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION)
+ {
+ return processContactPointData(command);
+ }
+
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
@@ -210,46 +416,35 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
break;
}
+ case CMD_SDF_LOADING_COMPLETED:
+ {
+ //we'll stream further info from the physics server
+ //so serverCmd will be invalid, make a copy
+
+
+ int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
+ for (int i=0;i<numBodies;i++)
+ {
+ int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
+ SharedMemoryCommand infoRequestCommand;
+ infoRequestCommand.m_type = CMD_REQUEST_BODY_INFO;
+ infoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
+ SharedMemoryStatus infoStatus;
+ bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand,infoStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+ if (hasStatus)
+ {
+ processBodyJointInfo(bodyUniqueId, infoStatus);
+ }
+ }
+ break;
+ }
case CMD_URDF_LOADING_COMPLETED:
{
+
if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0)
{
- bParse::btBulletFile bf(
- &m_data->m_bulletStreamDataServerToClient[0],
- serverCmd.m_dataStreamArguments.m_streamChunkLength);
- bf.setFileDNAisMemoryDNA();
- bf.parse(false);
- int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
-
- BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
- m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints);
-
- for (int i = 0; i < bf.m_multiBodies.size(); i++)
- {
- int flag = bf.getFlags();
- if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
- {
- Bullet::btMultiBodyDoubleData* mb =
- (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
-
- addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
- } else
- {
- Bullet::btMultiBodyFloatData* mb =
- (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
-
- addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
- }
- }
- if (bf.ok()) {
- if (m_data->m_verboseOutput)
- {
- b3Printf("Received robot description ok!\n");
- }
- } else
- {
- b3Warning("Robot description not received");
- }
+ int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
+ processBodyJointInfo(bodyIndex,serverCmd);
}
break;
}
@@ -265,6 +460,34 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
return hasStatus;
}
+int PhysicsDirect::getNumBodies() const
+{
+ return m_data->m_bodyJointMap.size();
+}
+
+
+int PhysicsDirect::getBodyUniqueId(int serialIndex) const
+{
+ if ((serialIndex >= 0) && (serialIndex < getNumBodies()))
+ {
+ return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1();
+ }
+ return -1;
+}
+
+bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
+{
+ BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
+ if (bodyJointsPtr && *bodyJointsPtr)
+ {
+ BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
+ info.m_baseName = bodyJoints->m_baseName.c_str();
+ return true;
+ }
+
+ return false;
+}
+
int PhysicsDirect::getNumJoints(int bodyIndex) const
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
@@ -277,14 +500,19 @@ int PhysicsDirect::getNumJoints(int bodyIndex) const
return 0;
}
-void PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
+bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
- info = bodyJoints->m_jointInfo[jointIndex];
+ if (jointIndex < bodyJoints->m_jointInfo.size())
+ {
+ info = bodyJoints->m_jointInfo[jointIndex];
+ return true;
+ }
}
+ return false;
}
///todo: move this out of the
@@ -328,3 +556,23 @@ const float* PhysicsDirect::getDebugLinesColor() const
}
return 0;
}
+
+void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
+{
+ if (cameraData)
+ {
+ cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
+ cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
+ cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
+ cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
+ cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0;
+ }
+}
+
+void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
+{
+ contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
+ contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0;
+
+}
+
diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h
index bb84aff..e312823 100644
--- a/examples/SharedMemory/PhysicsDirect.h
+++ b/examples/SharedMemory/PhysicsDirect.h
@@ -19,15 +19,22 @@ protected:
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
+ bool processCamera(const struct SharedMemoryCommand& orgCommand);
+
+ bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
+
+ void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
+
public:
PhysicsDirect();
virtual ~PhysicsDirect();
- // return true if connection succesfull, can also check 'isConnected'
+ // return true if connection succesfull, can also check 'isConnected'
+ //it is OK to pass a null pointer for the gui helper
virtual bool connect();
-
+
////todo: rename to 'disconnect'
virtual void disconnectSharedMemory();
@@ -42,9 +49,15 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
+ virtual int getNumBodies() const;
+
+ virtual int getBodyUniqueId(int serialIndex) const;
+
+ virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
+
virtual int getNumJoints(int bodyIndex) const;
- virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
+ virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);
@@ -57,6 +70,15 @@ public:
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
+ virtual void getCachedCameraImage(b3CameraImageData* cameraData);
+
+ virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
+
+ //those 2 APIs are for internal use for visualization
+ virtual bool connect(struct GUIHelperInterface* guiHelper);
+ virtual void renderScene();
+ virtual void debugDraw(int debugDrawMode);
+
};
#endif //PHYSICS_DIRECT_H
diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp
index 90f7a34..eeb6410 100644
--- a/examples/SharedMemory/PhysicsLoopBack.cpp
+++ b/examples/SharedMemory/PhysicsLoopBack.cpp
@@ -74,14 +74,29 @@ bool PhysicsLoopBack::submitClientCommand(const struct SharedMemoryCommand& comm
return m_data->m_physicsClient->submitClientCommand(command);
}
+int PhysicsLoopBack::getNumBodies() const
+{
+ return m_data->m_physicsClient->getNumBodies();
+}
+
+int PhysicsLoopBack::getBodyUniqueId(int serialIndex) const
+{
+ return m_data->m_physicsClient->getBodyUniqueId(serialIndex);
+}
+
+bool PhysicsLoopBack::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
+{
+ return m_data->m_physicsClient->getBodyInfo(bodyUniqueId, info);
+}
+
int PhysicsLoopBack::getNumJoints(int bodyIndex) const
{
return m_data->m_physicsClient->getNumJoints(bodyIndex);
}
-void PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
+bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
{
- m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
+ return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
}
///todo: move this out of the
@@ -105,11 +120,23 @@ const float* PhysicsLoopBack::getDebugLinesFrom() const
{
return m_data->m_physicsClient->getDebugLinesFrom();
}
+
const float* PhysicsLoopBack::getDebugLinesTo() const
{
return m_data->m_physicsClient->getDebugLinesTo();
}
+
const float* PhysicsLoopBack::getDebugLinesColor() const
{
return m_data->m_physicsClient->getDebugLinesColor();
}
+
+void PhysicsLoopBack::getCachedCameraImage(struct b3CameraImageData* cameraData)
+{
+ return m_data->m_physicsClient->getCachedCameraImage(cameraData);
+}
+
+void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
+{
+ return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData);
+}
diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h
index ea66ad1..baa7939 100644
--- a/examples/SharedMemory/PhysicsLoopBack.h
+++ b/examples/SharedMemory/PhysicsLoopBack.h
@@ -38,9 +38,15 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
+ virtual int getNumBodies() const;
+
+ virtual int getBodyUniqueId(int serialIndex) const;
+
+ virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
+
virtual int getNumJoints(int bodyIndex) const;
- virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
+ virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);
@@ -52,6 +58,9 @@ public:
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
+ virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
+
+ virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
};
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index b9df7b9..6f2de6b 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -1,16 +1,21 @@
#include "PhysicsServerCommandProcessor.h"
-
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
+#include "TinyRendererVisualShapeConverter.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
-#include "../CommonInterfaces/CommonRenderInterface.h"
-
+#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+#include "LinearMath/btHashMap.h"
+#include "BulletInverseDynamics/MultiBodyTree.hpp"
+#include "IKTrajectoryHelper.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btTransform.h"
@@ -22,6 +27,17 @@
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "SharedMemoryCommands.h"
+
+//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
+btVector3 gLastPickPos(0, 0, 0);
+bool gCloseToKuka=false;
+bool gEnableRealTimeSimVR=false;
+bool gCreateSamuraiRobotAssets = true;
+
+int gCreateObjectSimVR = -1;
+btScalar simTimeScalingFactor = 1;
+btScalar gRhsClamp = 1.f;
+
struct UrdfLinkNameMapUtil
{
btMultiBody* m_mb;
@@ -30,6 +46,10 @@ struct UrdfLinkNameMapUtil
UrdfLinkNameMapUtil():m_mb(0),m_memSerializer(0)
{
}
+ virtual ~UrdfLinkNameMapUtil()
+ {
+ delete m_memSerializer;
+ }
};
@@ -54,12 +74,12 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
virtual void draw3dText(const btVector3& location,const char* textString)
{
}
-
+
virtual void setDebugMode(int debugMode)
{
m_debugMode = debugMode;
}
-
+
virtual int getDebugMode() const
{
return m_debugMode;
@@ -82,6 +102,7 @@ struct InteralBodyData
int m_testData;
btTransform m_rootLocalInertialFrame;
+ btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
InteralBodyData()
:m_multiBody(0),
@@ -98,11 +119,11 @@ struct InternalBodyHandle : public InteralBodyData
BT_DECLARE_ALIGNED_ALLOCATOR();
int m_nextFreeHandle;
- void SetNextFree(int next)
+ void SetNextFree(int next)
{
m_nextFreeHandle = next;
}
- int GetNextFree() const
+ int GetNextFree() const
{
return m_nextFreeHandle;
}
@@ -151,7 +172,7 @@ public:
struct CommandLogger
{
FILE* m_file;
-
+
void writeHeader(unsigned char* buffer) const
{
@@ -183,15 +204,15 @@ struct CommandLogger
buffer[9] = 0;
buffer[10] = 0;
buffer[11] = 0;
-
+
int ver = btGetVersion();
if (ver>=0 && ver<999)
{
sprintf((char*)&buffer[9],"%d",ver);
}
-
+
}
-
+
void logCommand(const SharedMemoryCommand& command)
{
btCommandChunk chunk;
@@ -238,10 +259,10 @@ struct CommandLogPlayback
}
unsigned char c = m_header[7];
m_fileIs64bit = (c=='-');
-
+
const bool VOID_IS_8 = ((sizeof(void*)==8));
m_bitsVary = (VOID_IS_8 != m_fileIs64bit);
-
+
}
@@ -258,7 +279,7 @@ struct CommandLogPlayback
if (m_file)
{
size_t s = 0;
-
+
if (m_fileIs64bit)
{
@@ -269,7 +290,7 @@ struct CommandLogPlayback
bCommandChunkPtr4 chunk4;
s = fread((void*)&chunk4,sizeof(bCommandChunkPtr4),1,m_file);
}
-
+
if (s==1)
{
s = fread(cmd,sizeof(SharedMemoryCommand),1,m_file);
@@ -281,12 +302,17 @@ struct CommandLogPlayback
}
};
+struct SaveWorldObjectData
+{
+ b3AlignedObjectArray<int> m_bodyUniqueIds;
+ std::string m_fileName;
+};
+
struct PhysicsServerCommandProcessorInternalData
{
///handle management
btAlignedObjectArray<InternalBodyHandle> m_bodyHandles;
int m_numUsedHandles; // number of active handles
-
int m_firstFreeHandle; // free handles list
InternalBodyHandle* getHandle(int handle)
{
@@ -323,7 +349,7 @@ struct PhysicsServerCommandProcessorInternalData
{
m_numUsedHandles = 0;
m_firstFreeHandle = -1;
-
+
increaseHandleCapacity(1);
}
@@ -348,10 +374,10 @@ struct PhysicsServerCommandProcessorInternalData
int additionalCapacity= m_bodyHandles.size();
increaseHandleCapacity(additionalCapacity);
-
+
getHandle(handle)->SetNextFree(m_firstFreeHandle);
}
-
+
return handle;
}
@@ -368,20 +394,36 @@ struct PhysicsServerCommandProcessorInternalData
}
///end handle management
+
+ bool m_allowRealTimeSimulation;
+ bool m_hasGround;
+
+ btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
+ btMultiBody* m_gripperMultiBody;
+ btMultiBodyFixedConstraint* m_kukaGripperFixed;
+ btMultiBody* m_kukaGripperMultiBody;
+ btMultiBodyPoint2Point* m_kukaGripperRevolute1;
+ btMultiBodyPoint2Point* m_kukaGripperRevolute2;
-
+
+ int m_huskyId;
+ int m_KukaId;
+ int m_sphereId;
+ int m_gripperId;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
-
+
btScalar m_physicsDeltaTime;
+ btScalar m_numSimulationSubSteps;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
+ btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
+ btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
+ b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
-
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
- btHashMap<btHashInt, btMultiBodyJointMotor*> m_multiBodyJointMotorMap;
btAlignedObjectArray<std::string*> m_strings;
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
@@ -392,15 +434,18 @@ struct PhysicsServerCommandProcessorInternalData
btMultiBodyDynamicsWorld* m_dynamicsWorld;
SharedMemoryDebugDrawer* m_remoteDebugDrawer;
-
+ btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
+
+ btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
+
+
-
struct GUIHelperInterface* m_guiHelper;
int m_sharedMemoryKey;
bool m_verboseOutput;
-
-
+
+
//data for picking objects
class btRigidBody* m_pickedBody;
class btTypedConstraint* m_pickedConstraint;
@@ -409,12 +454,25 @@ struct PhysicsServerCommandProcessorInternalData
btVector3 m_hitPos;
btScalar m_oldPickingDist;
bool m_prevCanSleep;
+ TinyRendererVisualShapeConverter m_visualConverter;
PhysicsServerCommandProcessorInternalData()
- :
+ :m_hasGround(false),
+ m_gripperRigidbodyFixed(0),
+ m_gripperMultiBody(0),
+ m_kukaGripperFixed(0),
+ m_kukaGripperMultiBody(0),
+ m_kukaGripperRevolute1(0),
+ m_kukaGripperRevolute2(0),
+ m_allowRealTimeSimulation(false),
+ m_huskyId(-1),
+ m_KukaId(-1),
+ m_sphereId(-1),
+ m_gripperId(-1),
m_commandLogger(0),
m_logPlayback(0),
m_physicsDeltaTime(1./240.),
+ m_numSimulationSubSteps(0),
m_dynamicsWorld(0),
m_remoteDebugDrawer(0),
m_guiHelper(0),
@@ -424,7 +482,7 @@ struct PhysicsServerCommandProcessorInternalData
m_pickedConstraint(0),
m_pickingMultiBodyPoint2Point(0)
{
-
+
initHandles();
#if 0
btAlignedObjectArray<int> bla;
@@ -471,25 +529,56 @@ struct PhysicsServerCommandProcessorInternalData
}
+ btInverseDynamics::MultiBodyTree* findOrCreateTree(btMultiBody* multiBody)
+ {
+ btInverseDynamics::MultiBodyTree* tree = 0;
+
+ btInverseDynamics::MultiBodyTree** treePtrPtr =
+ m_inverseDynamicsBodies.find(multiBody);
+
+ if (treePtrPtr)
+ {
+ tree = *treePtrPtr;
+ }
+ else
+ {
+ btInverseDynamics::btMultiBodyTreeCreator id_creator;
+ if (-1 == id_creator.createFromBtMultiBody(multiBody, false))
+ {
+
+ }
+ else
+ {
+ tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
+ m_inverseDynamicsBodies.insert(multiBody, tree);
+ }
+ }
+
+ return tree;
+ }
+
};
void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiHelper)
{
-
+
if (guiHelper)
{
-
+
guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld);
} else
{
if (m_data->m_guiHelper && m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getDebugDrawer())
{
-
+
m_data->m_dynamicsWorld->setDebugDrawer(0);
}
}
m_data->m_guiHelper = guiHelper;
+
+
+
}
@@ -498,6 +587,9 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
m_data = new PhysicsServerCommandProcessorInternalData();
createEmptyDynamicsWorld();
+ m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
+ m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
+
}
PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
@@ -519,61 +611,94 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
///collision configuration contains default setup for memory, collision setup
m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
//m_collisionConfiguration->setConvexConvexMultipointIterations();
-
+
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration);
-
+
m_data->m_broadphase = new btDbvtBroadphase();
-
+
m_data->m_solver = new btMultiBodyConstraintSolver;
-
+
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
+ //Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
+ m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
+
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
-
-
+
+
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
+ m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
+
+}
+
+void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
+{
+ for (int i = 0; i < m_data->m_inverseDynamicsBodies.size(); i++)
+ {
+ btInverseDynamics::MultiBodyTree** treePtrPtr = m_data->m_inverseDynamicsBodies.getAtIndex(i);
+ if (treePtrPtr)
+ {
+ btInverseDynamics::MultiBodyTree* tree = *treePtrPtr;
+ delete tree;
+ }
+
+ }
+ m_data->m_inverseDynamicsBodies.clear();
}
void PhysicsServerCommandProcessor::deleteDynamicsWorld()
{
-
+ deleteCachedInverseDynamicsBodies();
+
+
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
{
delete m_data->m_multiBodyJointFeedbacks[i];
}
m_data->m_multiBodyJointFeedbacks.clear();
-
-
+
+
for (int i=0;i<m_data->m_worldImporters.size();i++)
{
delete m_data->m_worldImporters[i];
}
m_data->m_worldImporters.clear();
-
+
for (int i=0;i<m_data->m_urdfLinkNameMapper.size();i++)
{
delete m_data->m_urdfLinkNameMapper[i];
}
m_data->m_urdfLinkNameMapper.clear();
-
- m_data->m_multiBodyJointMotorMap.clear();
-
+
+
for (int i=0;i<m_data->m_strings.size();i++)
{
delete m_data->m_strings[i];
}
m_data->m_strings.clear();
-
+ btAlignedObjectArray<btTypedConstraint*> constraints;
+ btAlignedObjectArray<btMultiBodyConstraint*> mbconstraints;
+
+
if (m_data->m_dynamicsWorld)
{
-
+
int i;
for (i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
{
- m_data->m_dynamicsWorld->removeConstraint(m_data->m_dynamicsWorld->getConstraint(i));
+ btTypedConstraint* constraint =m_data->m_dynamicsWorld->getConstraint(i);
+ constraints.push_back(constraint);
+ m_data->m_dynamicsWorld->removeConstraint(constraint);
}
+ for (i=m_data->m_dynamicsWorld->getNumMultiBodyConstraints()-1;i>=0;i--)
+ {
+ btMultiBodyConstraint* mbconstraint = m_data->m_dynamicsWorld->getMultiBodyConstraint(i);
+ mbconstraints.push_back(mbconstraint);
+ m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbconstraint);
+ }
+
for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i];
@@ -585,8 +710,25 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
m_data->m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
+ for (i=m_data->m_dynamicsWorld->getNumMultibodies()-1;i>=0;i--)
+ {
+ btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i);
+ m_data->m_dynamicsWorld->removeMultiBody(mb);
+ delete mb;
+ }
}
- //delete collision shapes
+
+ for (int i=0;i<constraints.size();i++)
+ {
+ delete constraints[i];
+ }
+ constraints.clear();
+ for (int i=0;i<mbconstraints.size();i++)
+ {
+ delete mbconstraints[i];
+ }
+ mbconstraints.clear();
+ //delete collision shapes
for (int j = 0; j<m_data->m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_data->m_collisionShapes[j];
@@ -632,22 +774,157 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
for (int i=0;i<numLinks;i++)
{
int mbLinkIndex = i;
-
+
if (supportsJointMotor(mb,mbLinkIndex))
{
- float maxMotorImpulse = 0.f;
+ float maxMotorImpulse = 1.f;
int dof = 0;
btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
+ motor->setPositionTarget(0, 0);
+ motor->setVelocityTarget(0, 1);
+ //motor->setRhsClamp(gRhsClamp);
//motor->setMaxAppliedImpulse(0);
- m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,motor);
+ mb->getLink(mbLinkIndex).m_userPtr = motor;
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
+ motor->finalizeMultiDof();
+
}
}
}
+bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody)
+{
+ btAssert(m_data->m_dynamicsWorld);
+ if (!m_data->m_dynamicsWorld)
+ {
+ b3Error("loadSdf: No valid m_dynamicsWorld");
+ return false;
+ }
+
+ m_data->m_sdfRecentLoadedBodies.clear();
+
+ BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
+
+ bool useFixedBase = false;
+ bool loadOk = u2b.loadSDF(fileName, useFixedBase);
+ if (loadOk)
+ {
+ for (int i=0;i<u2b.getNumAllocatedCollisionShapes();i++)
+ {
+ btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
+ m_data->m_collisionShapes.push_back(shape);
+ }
+
+ btTransform rootTrans;
+ rootTrans.setIdentity();
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("loaded %s OK!", fileName);
+ }
+ SaveWorldObjectData sd;
+ sd.m_fileName = fileName;
+
+
+ for (int m =0; m<u2b.getNumModels();m++)
+ {
+
+ u2b.activateModel(m);
+ btMultiBody* mb = 0;
+ btRigidBody* rb = 0;
+
+ //get a body index
+ int bodyUniqueId = m_data->allocHandle();
+
+ InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
+
+ sd.m_bodyUniqueIds.push_back(bodyUniqueId);
+
+ u2b.setBodyUniqueId(bodyUniqueId);
+ {
+ btScalar mass = 0;
+ bodyHandle->m_rootLocalInertialFrame.setIdentity();
+ btVector3 localInertiaDiagonal(0,0,0);
+ int urdfLinkIndex = u2b.getRootLinkIndex();
+ u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame);
+ }
+
+
+
+ //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
+ int rootLinkIndex = u2b.getRootLinkIndex();
+ b3Printf("urdf root link index = %d\n",rootLinkIndex);
+ MyMultiBodyCreator creation(m_data->m_guiHelper);
+
+ u2b.getRootTransformInWorld(rootTrans);
+ ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),CUF_USE_SDF);
+
+
+
+ mb = creation.getBulletMultiBody();
+ rb = creation.getRigidBody();
+ if (rb)
+ rb->setUserIndex2(bodyUniqueId);
+
+ if (mb)
+ mb->setUserIndex2(bodyUniqueId);
+
+
+ if (mb)
+ {
+ bodyHandle->m_multiBody = mb;
+
+
+ m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId);
+
+ createJointMotors(mb);
+
+
+ //disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
+
+ bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
+ for (int i=0;i<mb->getNumLinks();i++)
+ {
+ //disable serialization of the collision objects
+
+ int urdfLinkIndex = creation.m_mb2urdfLink[i];
+ btScalar mass;
+ btVector3 localInertiaDiagonal(0,0,0);
+ btTransform localInertialFrame;
+ u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
+ bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame);
+
+ std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
+ m_data->m_strings.push_back(linkName);
+
+ mb->getLink(i).m_linkName = linkName->c_str();
+
+ std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str());
+ m_data->m_strings.push_back(jointName);
+
+ mb->getLink(i).m_jointName = jointName->c_str();
+ }
+ std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
+ m_data->m_strings.push_back(baseName);
+ mb->setBaseName(baseName->c_str());
+ } else
+ {
+ b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
+ bodyHandle->m_rigidBody = rb;
+ }
+
+ }
+
+ m_data->m_saveWorldBodyData.push_back(sd);
+
+ }
+ return loadOk;
+}
+
+
+
bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes)
@@ -659,10 +936,14 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
return false;
}
- BulletURDFImporter u2b(m_data->m_guiHelper);
-
+
+ BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
+
+
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
+
+
if (loadOk)
{
//get a body index
@@ -670,7 +951,18 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (bodyUniqueIdPtr)
*bodyUniqueIdPtr= bodyUniqueId;
+ //quick prototype of 'save world' for crude world editing
+ {
+ SaveWorldObjectData sd;
+ sd.m_fileName = fileName;
+ sd.m_bodyUniqueIds.push_back(bodyUniqueId);
+ m_data->m_saveWorldBodyData.push_back(sd);
+ }
+
+ u2b.setBodyUniqueId(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
+
+
{
btScalar mass = 0;
@@ -683,7 +975,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
{
b3Printf("loaded %s OK!", fileName);
}
-
+
btTransform tr;
tr.setIdentity();
tr.setOrigin(pos);
@@ -691,14 +983,27 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
//int rootLinkIndex = u2b.getRootLinkIndex();
// printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_data->m_guiHelper);
-
+
ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
+
+ for (int i=0;i<u2b.getNumAllocatedCollisionShapes();i++)
+ {
+ btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
+ m_data->m_collisionShapes.push_back(shape);
+ }
+
btMultiBody* mb = creation.getBulletMultiBody();
+ btRigidBody* rb = creation.getRigidBody();
+
if (useMultiBody)
{
+
+
if (mb)
{
+ mb->setUserIndex2(bodyUniqueId);
bodyHandle->m_multiBody = mb;
+
createJointMotors(mb);
@@ -710,11 +1015,18 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0);
+ bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
for (int i=0;i<mb->getNumLinks();i++)
{
//disable serialization of the collision objects
util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0);
int urdfLinkIndex = creation.m_mb2urdfLink[i];
+ btScalar mass;
+ btVector3 localInertiaDiagonal(0,0,0);
+ btTransform localInertialFrame;
+ u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
+ bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame);
+
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
m_data->m_strings.push_back(linkName);
util->m_memSerializer->registerNameForPointer(linkName->c_str(),linkName->c_str());
@@ -725,85 +1037,131 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str());
mb->getLink(i).m_jointName = jointName->c_str();
}
-
+
std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_data->m_strings.push_back(baseName);
-
+
util->m_memSerializer->registerNameForPointer(baseName->c_str(),baseName->c_str());
mb->setBaseName(baseName->c_str());
-
-
+
+
util->m_memSerializer->insertHeader();
-
+
int len = mb->calculateSerializeBufferSize();
btChunk* chunk = util->m_memSerializer->allocate(len,1);
const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer);
util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
- return true;
+ return true;
} else
{
b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
return false;
}
+
} else
{
- btAssert(0);
-
- return true;
+ if (rb)
+ {
+ bodyHandle->m_rigidBody = rb;
+ rb->setUserIndex2(bodyUniqueId);
+ return true;
+ }
}
-
+
}
-
+
return false;
}
+void PhysicsServerCommandProcessor::replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes)
+{
+ if (m_data->m_logPlayback)
+ {
+ SharedMemoryCommand clientCmd;
+ SharedMemoryStatus serverStatus;
+ bool hasCommand = m_data->m_logPlayback->processNextCommand(&clientCmd);
+ if (hasCommand)
+ {
+ processCommand(clientCmd,serverStatus,bufferServerToClient,bufferSizeInBytes);
+ }
+ }
+}
-
-bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
+int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes)
{
+ int streamSizeInBytes = 0;
+ //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
- bool hasStatus = false;
-
+ InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
+ btMultiBody* mb = bodyHandle->m_multiBody;
+ if (mb)
{
-#if 0
- if (m_data->m_logPlayback)
+ UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
+ m_data->m_urdfLinkNameMapper.push_back(util);
+ util->m_mb = mb;
+ util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
+ //disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
+ util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0);
+ if (mb->getBaseName())
{
- if (m_data->m_testBlock1->m_numServerCommands>m_data->m_testBlock1->m_numProcessedServerCommands)
- {
- m_data->m_testBlock1->m_numProcessedServerCommands++;
- }
- //push a command from log file
- bool hasCommand = m_data->m_logPlayback->processNextCommand(&m_data->m_testBlock1->m_clientCommands[0]);
- if (hasCommand)
- {
- m_data->m_testBlock1->m_numClientCommands++;
- }
+ util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName());
}
-#endif
+ bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
+ for (int i=0;i<mb->getNumLinks();i++)
+ {
+ //disable serialization of the collision objects
+ util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0);
+ util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_linkName,mb->getLink(i).m_linkName);
+ util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_jointName,mb->getLink(i).m_jointName);
+ }
+
+ util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName());
+
+ util->m_memSerializer->insertHeader();
+
+ int len = mb->calculateSerializeBufferSize();
+ btChunk* chunk = util->m_memSerializer->allocate(len,1);
+ const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer);
+ util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
+ streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize();
+
+ }
+ return streamSizeInBytes;
+}
+
+bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
+{
+
+ bool hasStatus = false;
+
+ {
///we ignore overflow of integer for now
-
+
{
-
+
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
-
-
+
+
//const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
-#if 0
+#if 1
if (m_data->m_commandLogger)
{
- m_data->m_commandLogger->logCommand(m_data->m_testBlock1);
+ m_data->m_commandLogger->logCommand(clientCmd);
}
#endif
//m_data->m_testBlock1->m_numProcessedClientCommands++;
-
+
//no timestamp yet
int timeStamp = 0;
+
+ //catch uninitialized cases
+ serverStatusOut.m_type = CMD_INVALID_STATUS;
//consume the command
switch (clientCmd.m_type)
@@ -815,11 +1173,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength);
}
-
+
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter);
bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength);
-
+
if (completedOk)
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
@@ -830,7 +1188,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
}
-
+
break;
}
#endif
@@ -838,7 +1196,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_DEBUG_LINES:
{
int curFlags =m_data->m_remoteDebugDrawer->getDebugMode();
-
+
int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb;
int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex;
if (startingLineIndex<0)
@@ -846,7 +1204,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
b3Warning("startingLineIndex should be non-negative");
startingLineIndex = 0;
}
-
+
if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex==0)
{
m_data->m_remoteDebugDrawer->m_lines2.resize(0);
@@ -867,9 +1225,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
b3Warning("m_startingLineIndex exceeds total number of debug lines");
startingLineIndex =m_data->m_remoteDebugDrawer->m_lines2.size();
}
-
+
int numLines = btMin(maxNumLines,m_data->m_remoteDebugDrawer->m_lines2.size()-startingLineIndex);
-
+
if (numLines)
{
@@ -892,20 +1250,300 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
linesColor[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.z();
}
}
-
+
serverStatusOut.m_type = CMD_DEBUG_LINES_COMPLETED;
serverStatusOut.m_sendDebugLinesArgs.m_numDebugLines = numLines;
serverStatusOut.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex;
serverStatusOut.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size()-(startingLineIndex+numLines);
hasStatus = true;
-
+
+ break;
+ }
+
+ case CMD_REQUEST_CAMERA_IMAGE_DATA:
+ {
+
+ int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex;
+ int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth;
+ int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
+ int numPixelsCopied = 0;
+
+
+
+ if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0)
+ {
+ //m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,clientCmd.m_requestPixelDataArguments.m_projectionMatrix,0,0,0,0,0,width,height,0);
+ }
+ else
+ {
+ if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) &&
+ (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0)
+ {
+ m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth,
+ clientCmd.m_requestPixelDataArguments.m_pixelHeight);
+ }
+ m_data->m_visualConverter.getWidthAndHeight(width,height);
+ }
+
+
+
+ int numTotalPixels = width*height;
+ int numRemainingPixels = numTotalPixels - startPixelIndex;
+
+
+ if (numRemainingPixels>0)
+ {
+ int totalBytesPerPixel = 4+4+4;//4 for rgb, 4 for depth, 4 for segmentation mask
+ int maxNumPixels = bufferSizeInBytes/totalBytesPerPixel-1;
+ unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient;
+ int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels);
+
+ float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4);
+ int* segmentationMaskBuffer = (int*)(bufferServerToClient+numRequestedPixels*8);
+
+ if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0)
+ {
+ m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,
+ clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels,
+ depthBuffer,numRequestedPixels,
+ segmentationMaskBuffer, numRequestedPixels,
+ startPixelIndex,width,height,&numPixelsCopied);
+ } else
+ {
+
+ if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0)
+ {
+ // printf("-------------------------------\nRendering\n");
+
+
+ if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0)
+ {
+ m_data->m_visualConverter.render(
+ clientCmd.m_requestPixelDataArguments.m_viewMatrix,
+ clientCmd.m_requestPixelDataArguments.m_projectionMatrix);
+ } else
+ {
+ m_data->m_visualConverter.render();
+ }
+
+ }
+
+ m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels,
+ depthBuffer,numRequestedPixels,
+ segmentationMaskBuffer, numRequestedPixels,
+ startPixelIndex,&width,&height,&numPixelsCopied);
+ }
+
+ //each pixel takes 4 RGBA values and 1 float = 8 bytes
+
+ } else
+ {
+
+ }
+
+ serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED;
+ serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied;
+ serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied;
+ serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex;
+ serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width;
+ serverStatusOut.m_sendPixelDataArguments.m_imageHeight= height;
+ hasStatus = true;
+
+ break;
+ }
+
+ case CMD_REQUEST_BODY_INFO:
+ {
+ const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
+ //stream info into memory
+ int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
+
+ serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED;
+ serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId;
+ serverStatusOut.m_dataStreamArguments.m_streamChunkLength = streamSizeInBytes;
+ hasStatus = true;
+ break;
+ }
+ case CMD_SAVE_WORLD:
+ {
+ ///this is a very rudimentary way to save the state of the world, for scene authoring
+ ///many todo's, for example save the state of motor controllers etc.
+
+
+ {
+ //saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
+
+ FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w");
+ if (f)
+ {
+ char line[1024];
+ {
+ sprintf(line,"import pybullet as p\n");
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ }
+ {
+ sprintf(line,"p.connect(p.SHARED_MEMORY)\n");
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ }
+ //for each objects ...
+ for (int i=0;i<m_data->m_saveWorldBodyData.size();i++)
+ {
+ SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i];
+
+ for (int i=0;i<sd.m_bodyUniqueIds.size();i++)
+ {
+ {
+ int bodyUniqueId = sd.m_bodyUniqueIds[i];
+ InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+ if (body)
+ {
+ if (body->m_multiBody)
+ {
+ btMultiBody* mb = body->m_multiBody;
+
+ btTransform comTr = mb->getBaseWorldTransform();
+ btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse();
+
+ if (strstr(sd.m_fileName.c_str(),".urdf"))
+ {
+ sprintf(line,"objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n",sd.m_fileName.c_str(),
+ tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2],
+ tr.getRotation()[0],tr.getRotation()[1],tr.getRotation()[2],tr.getRotation()[3]);
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ }
+
+ if (strstr(sd.m_fileName.c_str(),".sdf") && i==0)
+ {
+ sprintf(line,"objects = p.loadSDF(\"%s\")\n",sd.m_fileName.c_str());
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ }
+
+ if (strstr(sd.m_fileName.c_str(),".sdf") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) )
+ {
+ sprintf(line,"ob = objects[%d]\n",i);
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ }
+
+ if (strstr(sd.m_fileName.c_str(),".sdf"))
+ {
+ sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n",
+ comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2],
+ comTr.getRotation()[0],comTr.getRotation()[1],comTr.getRotation()[2],comTr.getRotation()[3]);
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ }
+
+ if (mb->getNumLinks())
+ {
+ {
+ sprintf(line,"jointPositions=[");
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ }
+
+ for (int i=0;i<mb->getNumLinks();i++)
+ {
+ btScalar jointPos = mb->getJointPosMultiDof(i)[0];
+ if (i<mb->getNumLinks()-1)
+ {
+ sprintf(line," %f,",jointPos);
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ } else
+ {
+ sprintf(line," %f ",jointPos);
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ }
+ }
+
+ {
+ sprintf(line,"]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n");
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ }
+ }
+ } else
+ {
+ //todo: btRigidBody/btSoftBody etc case
+ }
+ }
+ }
+
+ }
+
+ //for URDF, load at origin, then reposition...
+
+
+ struct SaveWorldObjectData
+ {
+ b3AlignedObjectArray<int> m_bodyUniqueIds;
+ std::string m_fileName;
+ };
+ }
+
+ {
+ btVector3 grav=this->m_data->m_dynamicsWorld->getGravity();
+ sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]);
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ }
+
+
+ {
+ sprintf(line,"p.stepSimulation()\np.disconnect()\n");
+ int len = strlen(line);
+ fwrite(line,len,1,f);
+ }
+ fclose(f);
+ }
+
+
+ serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED;
+ hasStatus = true;
break;
}
+ serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED;
+ hasStatus = true;
+ break;
+ }
+ case CMD_LOAD_SDF:
+ {
+ const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName);
+ }
+ bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? sdfArgs.m_useMultiBody : true;
+
+ bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody);
+ if (completedOk)
+ {
+ //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
+ serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
+ int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
+ for (int i=0;i<maxBodies;i++)
+ {
+ serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = m_data->m_sdfRecentLoadedBodies[i];
+ }
+ serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED;
+ } else
+ {
+ serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
+ }
+ hasStatus = true;
+ break;
+ }
case CMD_LOAD_URDF:
{
-
+
const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
if (m_data->m_verboseOutput)
{
@@ -935,30 +1573,32 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
initialPos,initialOrn,
useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
-
-
+
+
if (completedOk)
{
-
+
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
-
+
serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
+ serverStatusOut.m_dataStreamArguments.m_streamChunkLength = 0;
+
if (m_data->m_urdfLinkNameMapper.size())
{
serverStatusOut.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
hasStatus = true;
-
+
} else
{
serverStatusOut.m_type = CMD_URDF_LOADING_FAILED;
hasStatus = true;
}
-
-
-
+
+
+
break;
}
case CMD_CREATE_SENSOR:
@@ -988,7 +1628,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
mb->getLink(jointIndex).m_jointFeedback = fb;
m_data->m_multiBodyJointFeedbacks.push_back(fb);
};
-
+
} else
{
if (mb->getLink(jointIndex).m_jointFeedback)
@@ -1003,7 +1643,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
-
+
} else
{
b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet");
@@ -1018,24 +1658,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btJointFeedback* fb = new btJointFeedback();
m_data->m_jointFeedbacks.push_back(fb);
c->setJointFeedback(fb);
-
-
+
+
}
*/
#endif
-
+
serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
-
+
break;
}
- case CMD_SEND_DESIRED_STATE:
+ case CMD_SEND_DESIRED_STATE:
{
if (m_data->m_verboseOutput)
{
b3Printf("Processed CMD_SEND_DESIRED_STATE");
}
-
+
int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
@@ -1043,7 +1683,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
btMultiBody* mb = body->m_multiBody;
btAssert(mb);
-
+
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
{
case CONTROL_MODE_TORQUE:
@@ -1052,26 +1692,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
b3Printf("Using CONTROL_MODE_TORQUE");
}
- mb->clearForcesAndTorques();
-
- int torqueIndex = 0;
- btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
- clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
- clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
- btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
- clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
- clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
- torqueIndex+=6;
- mb->addBaseForce(f);
- mb->addBaseTorque(t);
- for (int link=0;link<mb->getNumLinks();link++)
+ // mb->clearForcesAndTorques();
+ int torqueIndex = 6;
+ if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
-
- for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
- {
- double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
- mb->addJointTorqueMultiDof(link,dof,torque);
- torqueIndex++;
+ for (int link=0;link<mb->getNumLinks();link++)
+ {
+
+ for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
+ {
+ double torque = 0.f;
+ if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+ {
+ torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
+ mb->addJointTorqueMultiDof(link,dof,torque);
+ }
+ torqueIndex++;
+ }
}
}
break;
@@ -1082,7 +1719,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
b3Printf("Using CONTROL_MODE_VELOCITY");
}
-
+
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
{
@@ -1091,16 +1728,40 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
if (supportsJointMotor(mb,link))
{
-
- btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link];
- if (motorPtr)
- {
- btMultiBodyJointMotor* motor = *motorPtr;
- btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
- motor->setVelocityTarget(desiredVelocity);
- btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
- motor->setMaxAppliedImpulse(maxImp);
+ btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
+
+
+ if (motor)
+ {
+ btScalar desiredVelocity = 0.f;
+ bool hasDesiredVelocity = false;
+
+
+ if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0)
+ {
+ desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
+ btScalar kd = 0.1f;
+ if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0)
+ {
+ kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
+ }
+
+ motor->setVelocityTarget(desiredVelocity,kd);
+
+ btScalar kp = 0.f;
+ motor->setPositionTarget(0,kp);
+ hasDesiredVelocity = true;
+ }
+ if (hasDesiredVelocity)
+ {
+ btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
+ if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+ {
+ maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
+ }
+ motor->setMaxAppliedImpulse(maxImp);
+ }
numMotors++;
}
@@ -1110,6 +1771,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
break;
}
+
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
if (m_data->m_verboseOutput)
@@ -1117,7 +1779,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD");
}
//compute the force base on PD control
- mb->clearForcesAndTorques();
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
@@ -1128,32 +1789,54 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
if (supportsJointMotor(mb,link))
{
-
- btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link];
- if (motorPtr)
+
+
+ btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
+
+ if (motor)
{
- btMultiBodyJointMotor* motor = *motorPtr;
-
- btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
- btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
-
- btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
- btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
-
- int dof1 = 0;
- btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1];
- btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1];
- btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime;
- btScalar velocityError = (desiredVelocity - currentVelocity);
-
- desiredVelocity = kp * positionStabiliationTerm +
- kd * velocityError;
-
- motor->setVelocityTarget(desiredVelocity);
-
- btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
-
- motor->setMaxAppliedImpulse(1000);//maxImp);
+
+ bool hasDesiredPosOrVel = false;
+ btScalar kp = 0.f;
+ btScalar kd = 0.f;
+ btScalar desiredVelocity = 0.f;
+ if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0)
+ {
+ hasDesiredPosOrVel = true;
+ desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
+ kd = 0.1;
+ }
+ btScalar desiredPosition = 0.f;
+ if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0)
+ {
+ hasDesiredPosOrVel = true;
+ desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
+ kp = 0.1;
+ }
+
+ if (hasDesiredPosOrVel)
+ {
+
+ if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP)!=0)
+ {
+ kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
+ }
+
+ if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD)!=0)
+ {
+ kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
+ }
+
+ motor->setVelocityTarget(desiredVelocity,kd);
+ motor->setPositionTarget(desiredPosition,kp);
+
+ btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
+
+ if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+ maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
+
+ motor->setMaxAppliedImpulse(maxImp);
+ }
numMotors++;
}
@@ -1166,18 +1849,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
}
default:
- {
- b3Warning("m_controlMode not implemented yet");
- break;
- }
-
- }
- }
-
- serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED;
- hasStatus = true;
- break;
- }
+ {
+ b3Warning("m_controlMode not implemented yet");
+ break;
+ }
+
+ }
+ }
+
+ serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED;
+ hasStatus = true;
+ break;
+ }
case CMD_REQUEST_ACTUAL_STATE:
{
if (m_data->m_verboseOutput)
@@ -1186,7 +1869,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
-
+
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
@@ -1195,9 +1878,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
int totalDegreeOfFreedomQ = 0;
- int totalDegreeOfFreedomU = 0;
-
-
+ int totalDegreeOfFreedomU = 0;
+
+ if (mb->getNumLinks()>= MAX_DEGREE_OF_FREEDOM)
+ {
+ serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
+ hasStatus = true;
+ break;
+ }
+
//always add the base, even for static (non-moving objects)
//so that we can easily move the 'fixed' base when needed
//do we don't use this conditional "if (!mb->hasFixedBase())"
@@ -1205,7 +1894,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btTransform tr;
tr.setOrigin(mb->getBasePos());
tr.setRotation(mb->getWorldToBaseRot().inverse());
-
+
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
body->m_rootLocalInertialFrame.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
@@ -1223,14 +1912,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
body->m_rootLocalInertialFrame.getRotation()[3];
-
+
//base position in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian
- serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
+ serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3];
@@ -1240,7 +1929,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2];
-
+
//base angular velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1];
@@ -1257,7 +1946,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
}
-
+
if (0 == mb->getLink(l).m_jointFeedback)
{
for (int d=0;d<6;d++)
@@ -1268,22 +1957,65 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear();
btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular();
-
+
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2];
-
+
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2];
}
- }
+
+ serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0;
+
+ if (supportsJointMotor(mb,l))
+ {
+
+ btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr;
+
+ if (motor && m_data->m_physicsDeltaTime>btScalar(0))
+ {
+ btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime;
+ serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] =
+ force;
+ //if (force>0)
+ //{
+ // b3Printf("force = %f\n", force);
+ //}
+ }
+ }
+ btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
+ btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
+
+ btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
+ btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
+
+ serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX();
+ serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY();
+ serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ();
+ serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x();
+ serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y();
+ serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z();
+ serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w();
+
+ serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX();
+ serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY();
+ serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ();
+
+ serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x();
+ serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y();
+ serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z();
+ serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w();
+
+ }
+
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
-
+
hasStatus = true;
-
+
} else
{
if (body && body->m_rigidBody)
@@ -1303,7 +2035,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian
- serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
+ serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3];
@@ -1313,7 +2045,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2];
-
+
//base angular velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1];
@@ -1337,25 +2069,48 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_STEP_FORWARD_SIMULATION:
{
-
+
if (m_data->m_verboseOutput)
{
b3Printf("Step simulation request");
+ b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber);
}
- m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0);
-
+ ///todo(erwincoumans) move this damping inside Bullet
+ for (int i=0;i<m_data->m_bodyHandles.size();i++)
+ {
+ applyJointDamping(i);
+ }
+
+
+
+
+ btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor;
+
+ if (m_data->m_numSimulationSubSteps > 0)
+ {
+ m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
+ }
+ else
+ {
+ m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
+ }
+
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
hasStatus = true;
break;
}
-
+
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME)
{
m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;
}
+ if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION)
+ {
+ m_data->m_allowRealTimeSimulation = clientCmd.m_physSimParamArgs.m_allowRealTimeSimulation;
+ }
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY)
{
btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0],
@@ -1368,12 +2123,25 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
-
+ if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS)
+ {
+ m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
+ }
+ if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS)
+ {
+ m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps;
+ }
+
+ if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP)
+ {
+ m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP;
+ }
+
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
break;
-
+
};
case CMD_INIT_POSE:
{
@@ -1390,6 +2158,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
{
btVector3 zero(0,0,0);
+ btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[0] &&
+ clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] &&
+ clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]);
+
mb->setBaseVel(zero);
mb->setBasePos(btVector3(
clientCmd.m_initPoseArgs.m_initialStateQ[0],
@@ -1398,21 +2170,26 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
{
+ btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] &&
+ clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] &&
+ clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] &&
+ clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]);
+
mb->setBaseOmega(btVector3(0,0,0));
- mb->setWorldToBaseRot(btQuaternion(
- clientCmd.m_initPoseArgs.m_initialStateQ[3],
+ btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3],
clientCmd.m_initPoseArgs.m_initialStateQ[4],
clientCmd.m_initPoseArgs.m_initialStateQ[5],
- clientCmd.m_initPoseArgs.m_initialStateQ[6]));
+ clientCmd.m_initPoseArgs.m_initialStateQ[6]);
+
+ mb->setWorldToBaseRot(invOrn.inverse());
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
{
int dofIndex = 7;
for (int i=0;i<mb->getNumLinks();i++)
{
- if (mb->getLink(i).m_dofCount==1)
+ if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[dofIndex]) && (mb->getLink(i).m_dofCount==1))
{
-
mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]);
mb->setJointVel(i,0);
}
@@ -1420,31 +2197,40 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
}
-
+
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
break;
}
-
+
case CMD_RESET_SIMULATION:
{
//clean up all data
- if (m_data && m_data->m_guiHelper && m_data->m_guiHelper->getRenderInterface())
- {
- m_data->m_guiHelper->getRenderInterface()->removeAllInstances();
- }
+ deleteCachedInverseDynamicsBodies();
+
+ if (m_data && m_data->m_guiHelper)
+ {
+ m_data->m_guiHelper->removeAllGraphicsInstances();
+ }
+ if (m_data)
+ {
+ m_data->m_visualConverter.resetAll();
+ }
+
deleteDynamicsWorld();
createEmptyDynamicsWorld();
+
m_data->exitHandles();
m_data->initHandles();
-
+
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
hasStatus = true;
-
+ m_data->m_hasGround = false;
+ m_data->m_gripperRigidbodyFixed = 0;
break;
}
case CMD_CREATE_RIGID_BODY:
@@ -1470,7 +2256,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION)
{
-
+
startTrans.setRotation(btQuaternion(
clientCmd.m_createBoxShapeArguments.m_initialOrientation[0],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[1],
@@ -1495,7 +2281,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
m_data->m_worldImporters.push_back(worldImporter);
btCollisionShape* shape = 0;
-
+
switch (shapeType)
{
case COLLISION_SHAPE_TYPE_CYLINDER_X:
@@ -1552,11 +2338,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
shape = worldImporter->createBoxShape(halfExtents);
}
}
-
-
+
+
bool isDynamic = (mass>0);
btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0);
- rb->setRollingFriction(0.2);
//m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
btVector4 colorRGBA(1,0,0,1);
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLOR)
@@ -1568,15 +2353,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape());
m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA);
-
-
+
+
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED;
-
+
int bodyUniqueId = m_data->allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId;
+ rb->setUserIndex2(bodyUniqueId);
bodyHandle->m_rootLocalInertialFrame.setIdentity();
bodyHandle->m_rigidBody = rb;
hasStatus = true;
@@ -1591,7 +2377,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_pickBodyArguments.m_rayToWorld[1],
clientCmd.m_pickBodyArguments.m_rayToWorld[2]));
-
+
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
@@ -1620,10 +2406,522 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
+ case CMD_REQUEST_CONTACT_POINT_INFORMATION:
+ {
+ SharedMemoryStatus& serverCmd =serverStatusOut;
+ serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
+
+ //make a snapshot of the contact manifolds into individual contact points
+ if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex==0)
+ {
+ int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds();
+ m_data->m_cachedContactPoints.resize(0);
+ m_data->m_cachedContactPoints.reserve(numContactManifolds*4);
+ for (int i=0;i<numContactManifolds;i++)
+ {
+ const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
+ int linkIndexA = -1;
+ int linkIndexB = -1;
+
+ int objectIndexB = -1;
+
+ const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
+ if (bodyB)
+ {
+ objectIndexB = bodyB->getUserIndex2();
+ }
+ const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
+ if (mblB && mblB->m_multiBody)
+ {
+ linkIndexB = mblB->m_link;
+ objectIndexB = mblB->m_multiBody->getUserIndex2();
+ }
+
+ int objectIndexA = -1;
+ const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0());
+ if (bodyA)
+ {
+ objectIndexA = bodyA->getUserIndex2();
+ }
+ const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
+ if (mblA && mblA->m_multiBody)
+ {
+ linkIndexA = mblA->m_link;
+
+ objectIndexA = mblA->m_multiBody->getUserIndex2();
+ }
+
+ btAssert(bodyA || mblA);
+
+ //apply the filter, if the user provides it
+ if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0)
+ {
+ if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) &&
+ (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexB))
+ continue;
+ }
+
+ //apply the second object filter, if the user provides it
+ if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter>=0)
+ {
+ if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) &&
+ (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB))
+ continue;
+ }
+
+ for (int p=0;p<manifold->getNumContacts();p++)
+ {
+
+ b3ContactPointData pt;
+ pt.m_bodyUniqueIdA = objectIndexA;
+ pt.m_bodyUniqueIdB = objectIndexB;
+ const btManifoldPoint& srcPt = manifold->getContactPoint(p);
+ pt.m_contactDistance = srcPt.getDistance();
+ pt.m_contactFlags = 0;
+ pt.m_linkIndexA = linkIndexA;
+ pt.m_linkIndexB = linkIndexB;
+ for (int j=0;j<3;j++)
+ {
+ pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
+ pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
+ pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
+ }
+ pt.m_normalForce = srcPt.getAppliedImpulse()/m_data->m_physicsDeltaTime;
+// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
+ m_data->m_cachedContactPoints.push_back (pt);
+ }
+ }
+ }
+
+ int numContactPoints = m_data->m_cachedContactPoints.size();
+
+
+ //b3ContactPoint
+ //struct b3ContactPointDynamics
+
+ int totalBytesPerContact = sizeof(b3ContactPointData);
+ int contactPointStorage = bufferSizeInBytes/totalBytesPerContact-1;
+
+ b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient;
+
+ int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
+ int numContactPointBatch = btMin(numContactPoints,contactPointStorage);
+
+ int endContactPointIndex = startContactPointIndex+numContactPointBatch;
+
+ for (int i=startContactPointIndex;i<endContactPointIndex ;i++)
+ {
+ const b3ContactPointData& srcPt = m_data->m_cachedContactPoints[i];
+ b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied];
+ destPt = srcPt;
+ serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++;
+ }
+
+ serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
+ serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
+
+ serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED,
+ hasStatus = true;
+ break;
+ }
+ case CMD_CALCULATE_INVERSE_DYNAMICS:
+ {
+ SharedMemoryStatus& serverCmd = serverStatusOut;
+ InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
+ if (bodyHandle && bodyHandle->m_multiBody)
+ {
+ serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
+
+ btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
+
+ if (tree)
+ {
+ int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+ const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
+ btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs);
+ for (int i = 0; i < num_dofs; i++)
+ {
+ q[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
+ qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
+ nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
+ }
+ // Set the gravity to correspond to the world gravity
+ btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
+
+ if (-1 != tree->setGravityInWorldFrame(id_grav) &&
+ -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
+ {
+ serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
+ serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs;
+ for (int i = 0; i < num_dofs; i++)
+ {
+ serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i+baseDofs];
+ }
+ serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
+ }
+ else
+ {
+ serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
+ }
+ }
+
+ }
+ else
+ {
+ serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
+ }
+
+ hasStatus = true;
+ break;
+ }
+ case CMD_CALCULATE_JACOBIAN:
+ {
+ SharedMemoryStatus& serverCmd = serverStatusOut;
+ InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId);
+ if (bodyHandle && bodyHandle->m_multiBody)
+ {
+ serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
+
+ btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
+
+ if (tree)
+ {
+ int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+ const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
+ btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs);
+ for (int i = 0; i < num_dofs; i++)
+ {
+ q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i];
+ qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i];
+ nu[i+baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i];
+ }
+ // Set the gravity to correspond to the world gravity
+ btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
+
+ if (-1 != tree->setGravityInWorldFrame(id_grav) &&
+ -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
+ {
+ serverCmd.m_jacobianResultArgs.m_dofCount = num_dofs;
+ // Set jacobian value
+ tree->calculateJacobians(q);
+ btInverseDynamics::mat3x jac_t(3, num_dofs);
+ tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex, &jac_t);
+ for (int i = 0; i < 3; ++i)
+ {
+ for (int j = 0; j < num_dofs; ++j)
+ {
+ serverCmd.m_jacobianResultArgs.m_linearJacobian[i*num_dofs+j] = jac_t(i,j);
+ }
+ }
+ serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED;
+ }
+ else
+ {
+ serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
+ }
+ }
+
+ }
+ else
+ {
+ serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
+ }
+
+ hasStatus = true;
+ break;
+ }
+ case CMD_APPLY_EXTERNAL_FORCE:
+ {
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber);
+ }
+ for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
+ {
+ InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
+ if (body && body->m_multiBody)
+ {
+ btMultiBody* mb = body->m_multiBody;
+ bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0);
+
+ if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0)
+ {
+ btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
+ clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1],
+ clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
+ btVector3 positionLocal(
+ clientCmd.m_externalForceArguments.m_positions[i*3+0],
+ clientCmd.m_externalForceArguments.m_positions[i*3+1],
+ clientCmd.m_externalForceArguments.m_positions[i*3+2]);
+
+ if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
+ {
+ btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal;
+ btVector3 relPosWorld = isLinkFrame ? positionLocal : mb->getBaseWorldTransform().getBasis()*positionLocal;
+ mb->addBaseForce(forceWorld);
+ mb->addBaseTorque(relPosWorld.cross(forceWorld));
+ //b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]);
+ } else
+ {
+ int link = clientCmd.m_externalForceArguments.m_linkIds[i];
+ btVector3 forceWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*forceLocal;
+ btVector3 relPosWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*positionLocal;
+ mb->addLinkForce(link, forceWorld);
+ mb->addLinkTorque(link,relPosWorld.cross(forceWorld));
+ //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]);
+ }
+ }
+ if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE)!=0)
+ {
+ btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
+ clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1],
+ clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
+
+ if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
+ {
+ btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal;
+ mb->addBaseTorque(torqueWorld);
+ //b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
+ } else
+ {
+ int link = clientCmd.m_externalForceArguments.m_linkIds[i];
+ btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*torqueLocal;
+ mb->addLinkTorque(link, torqueWorld);
+ //b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
+ }
+ }
+ }
+ }
+
+ SharedMemoryStatus& serverCmd =serverStatusOut;
+ serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
+ hasStatus = true;
+ break;
+ }
+ case CMD_CREATE_JOINT:
+ {
+ InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_createJointArguments.m_parentBodyIndex);
+ if (parentBody && parentBody->m_multiBody)
+ {
+ InteralBodyData* childBody = m_data->getHandle(clientCmd.m_createJointArguments.m_childBodyIndex);
+ if (childBody)
+ {
+ btVector3 pivotInParent(clientCmd.m_createJointArguments.m_parentFrame[0], clientCmd.m_createJointArguments.m_parentFrame[1], clientCmd.m_createJointArguments.m_parentFrame[2]);
+ btVector3 pivotInChild(clientCmd.m_createJointArguments.m_childFrame[0], clientCmd.m_createJointArguments.m_childFrame[1], clientCmd.m_createJointArguments.m_childFrame[2]);
+ btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_createJointArguments.m_parentFrame[3], clientCmd.m_createJointArguments.m_parentFrame[4], clientCmd.m_createJointArguments.m_parentFrame[5], clientCmd.m_createJointArguments.m_parentFrame[6]));
+ btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_createJointArguments.m_childFrame[3], clientCmd.m_createJointArguments.m_childFrame[4], clientCmd.m_createJointArguments.m_childFrame[5], clientCmd.m_createJointArguments.m_childFrame[6]));
+ btVector3 jointAxis(clientCmd.m_createJointArguments.m_jointAxis[0], clientCmd.m_createJointArguments.m_jointAxis[1], clientCmd.m_createJointArguments.m_jointAxis[2]);
+ if (clientCmd.m_createJointArguments.m_jointType == eFixedType)
+ {
+ if (childBody->m_multiBody)
+ {
+ btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
+ multibodyFixed->setMaxAppliedImpulse(500.0);
+ m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
+ }
+ else
+ {
+ btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild);
+ rigidbodyFixed->setMaxAppliedImpulse(500.0);
+ btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
+ world->addMultiBodyConstraint(rigidbodyFixed);
+ }
+ }
+ else if (clientCmd.m_createJointArguments.m_jointType == ePrismaticType)
+ {
+ if (childBody->m_multiBody)
+ {
+ btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
+ multibodySlider->setMaxAppliedImpulse(500.0);
+ m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
+ }
+ else
+ {
+ btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
+ rigidbodySlider->setMaxAppliedImpulse(500.0);
+ btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
+ world->addMultiBodyConstraint(rigidbodySlider);
+ }
+ } else if (clientCmd.m_createJointArguments.m_jointType == ePoint2PointType)
+ {
+ if (childBody->m_multiBody)
+ {
+ btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild);
+ p2p->setMaxAppliedImpulse(500);
+ m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
+ }
+ else
+ {
+ btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild);
+ p2p->setMaxAppliedImpulse(500);
+ btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
+ world->addMultiBodyConstraint(p2p);
+ }
+ }
+
+ }
+ }
+ SharedMemoryStatus& serverCmd =serverStatusOut;
+ serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
+ hasStatus = true;
+ break;
+ }
+ case CMD_CALCULATE_INVERSE_KINEMATICS:
+ {
+ SharedMemoryStatus& serverCmd = serverStatusOut;
+ serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED;
+
+ InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId);
+ if (bodyHandle && bodyHandle->m_multiBody)
+ {
+ IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
+ IKTrajectoryHelper* ikHelperPtr = 0;
+
+
+ if (ikHelperPtrPtr)
+ {
+ ikHelperPtr = *ikHelperPtrPtr;
+ }
+ else
+ {
+ IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
+ m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
+ ikHelperPtr = tmpHelper;
+ }
+
+ int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex;
+
+
+ if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
+ {
+ const int numDofs = bodyHandle->m_multiBody->getNumDofs();
+
+ b3AlignedObjectArray<double> jacobian_linear;
+ jacobian_linear.resize(3*numDofs);
+ b3AlignedObjectArray<double> jacobian_angular;
+ jacobian_angular.resize(3*numDofs);
+ int jacSize = 0;
+
+ btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
+
+
+
+ btAlignedObjectArray<double> q_current;
+ q_current.resize(numDofs);
+
+ if (tree)
+ {
+ jacSize = jacobian_linear.size();
+ // Set jacobian value
+ int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+
+
+ btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
+ for (int i = 0; i < numDofs; i++)
+ {
+ q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
+ q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i);
+ qdot[i + baseDofs] = 0;
+ nu[i+baseDofs] = 0;
+ }
+ // Set the gravity to correspond to the world gravity
+ btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
+
+ if (-1 != tree->setGravityInWorldFrame(id_grav) &&
+ -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
+ {
+ tree->calculateJacobians(q);
+ btInverseDynamics::mat3x jac_t(3, numDofs);
+ btInverseDynamics::mat3x jac_r(3,numDofs);
+ tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t);
+ tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r);
+ for (int i = 0; i < 3; ++i)
+ {
+ for (int j = 0; j < numDofs; ++j)
+ {
+ jacobian_linear[i*numDofs+j] = jac_t(i,j);
+ jacobian_angular[i*numDofs+j] = jac_r(i,j);
+ }
+ }
+ }
+ }
+
+
+ btAlignedObjectArray<double> q_new;
+ q_new.resize(numDofs);
+ int ikMethod = 0;
+ if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
+ {
+ ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
+ }
+ else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
+ {
+ ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
+ }
+ else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
+ {
+ ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
+ }
+ else
+ {
+ ikMethod = IK2_VEL_DLS;
+ }
+
+ if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
+ {
+ btAlignedObjectArray<double> lower_limit;
+ btAlignedObjectArray<double> upper_limit;
+ btAlignedObjectArray<double> joint_range;
+ btAlignedObjectArray<double> rest_pose;
+ lower_limit.resize(numDofs);
+ upper_limit.resize(numDofs);
+ joint_range.resize(numDofs);
+ rest_pose.resize(numDofs);
+ for (int i = 0; i < numDofs; ++i)
+ {
+ lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i];
+ upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i];
+ joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i];
+ rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i];
+ }
+ ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
+ }
+
+ btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
+
+ btVector3DoubleData endEffectorWorldPosition;
+ btVector3DoubleData endEffectorWorldOrientation;
+
+ btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
+ btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
+ btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
+
+ endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
+ endEffectorOri.serializeDouble(endEffectorWorldOrientation);
+ double dampIK[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
+ ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
+ endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
+ &q_current[0],
+ numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
+ &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIK);
+
+ serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
+ for (int i=0;i<numDofs;i++)
+ {
+ serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
+ }
+ serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
+ serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
+ }
+ }
+ hasStatus = true;
+ break;
+ }
+
default:
{
b3Error("Unknown command encountered");
-
+
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
hasStatus = true;
@@ -1631,22 +2929,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
};
-
+
}
}
return hasStatus;
}
+static int skip=1;
void PhysicsServerCommandProcessor::renderScene()
{
if (m_data->m_guiHelper)
{
m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
-
m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
}
-
+
}
void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
@@ -1662,6 +2960,7 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
}
+
bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
if (m_data->m_dynamicsWorld==0)
@@ -1674,6 +2973,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
{
btVector3 pickPos = rayCallback.m_hitPointWorld;
+ gLastPickPos = pickPos;
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
if (body)
{
@@ -1692,12 +2992,13 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
//very weak constraint for picking
p2p->m_setting.m_tau = 0.001f;
}
+
} else
{
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
if (multiCol && multiCol->m_multiBody)
{
-
+
m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep();
multiCol->m_multiBody->setCanSleep(false);
@@ -1710,10 +3011,10 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
btScalar scaling=1;
p2p->setMaxAppliedImpulse(2*scaling);
-
+
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(p2p);
- m_data->m_pickingMultiBodyPoint2Point =p2p;
+ m_data->m_pickingMultiBodyPoint2Point =p2p;
}
}
@@ -1738,7 +3039,7 @@ bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld
if (pickCon)
{
//keep it at the same picking distance
-
+
btVector3 dir = rayToWorld-rayFromWorld;
dir.normalize();
dir *= m_data->m_oldPickingDist;
@@ -1747,21 +3048,21 @@ bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld
pickCon->setPivotB(newPivotB);
}
}
-
+
if (m_data->m_pickingMultiBodyPoint2Point)
{
//keep it at the same picking distance
-
+
btVector3 dir = rayToWorld-rayFromWorld;
dir.normalize();
dir *= m_data->m_oldPickingDist;
btVector3 newPivotB = rayFromWorld + dir;
-
+
m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
}
-
+
return false;
}
@@ -1772,6 +3073,7 @@ void PhysicsServerCommandProcessor::removePickingConstraint()
m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint);
delete m_data->m_pickedConstraint;
m_data->m_pickedConstraint = 0;
+ m_data->m_pickedBody->forceActivationState(ACTIVE_TAG);
m_data->m_pickedBody = 0;
}
if (m_data->m_pickingMultiBodyPoint2Point)
@@ -1810,3 +3112,530 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
m_data->m_logPlayback = pb;
}
+
+btVector3 gVRGripperPos(0,0,0.2);
+btQuaternion gVRGripperOrn(0,0,0,1);
+btVector3 gVRController2Pos(0,0,0.2);
+btQuaternion gVRController2Orn(0,0,0,1);
+btScalar gVRGripper2Analog = 0;
+btScalar gVRGripperAnalog = 0;
+
+bool gVRGripperClosed = false;
+
+
+int gDroppedSimulationSteps = 0;
+int gNumSteps = 0;
+double gDtInSec = 0.f;
+double gSubStep = 0.f;
+void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
+{
+ if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
+ {
+ static btAlignedObjectArray<char> gBufferServerToClient;
+ gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+ int bodyId = 0;
+
+
+ if (gCreateObjectSimVR >= 0)
+ {
+ gCreateObjectSimVR = -1;
+ btMatrix3x3 mat(gVRGripperOrn);
+ btScalar spawnDistance = 0.1;
+ btVector3 spawnDir = mat.getColumn(0);
+ btVector3 shiftPos = spawnDir*spawnDistance;
+ btVector3 spawnPos = gVRGripperPos + shiftPos;
+ loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ //loadUrdf("lego/lego.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ m_data->m_sphereId = bodyId;
+ InteralBodyData* parentBody = m_data->getHandle(bodyId);
+ if (parentBody->m_multiBody)
+ {
+ parentBody->m_multiBody->setBaseVel(spawnDir * 5);
+ }
+ }
+
+ ///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later...
+ if (gCreateSamuraiRobotAssets)
+ {
+ if (!m_data->m_hasGround)
+ {
+ m_data->m_hasGround = true;
+
+ loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+
+ if (m_data->m_gripperRigidbodyFixed == 0)
+ {
+ int bodyId = 0;
+
+ if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
+ {
+ InteralBodyData* parentBody = m_data->getHandle(bodyId);
+ if (parentBody->m_multiBody)
+ {
+ parentBody->m_multiBody->setHasSelfCollision(0);
+ btVector3 pivotInParent(0.2, 0, 0);
+ btMatrix3x3 frameInParent;
+ //frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
+ frameInParent.setIdentity();
+ btVector3 pivotInChild(0, 0, 0);
+ btMatrix3x3 frameInChild;
+ frameInChild.setIdentity();
+
+ m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
+ m_data->m_gripperMultiBody = parentBody->m_multiBody;
+ if (m_data->m_gripperMultiBody->getNumLinks() > 2)
+ {
+ m_data->m_gripperMultiBody->setJointPos(0, 0);
+ m_data->m_gripperMultiBody->setJointPos(2, 0);
+ }
+ m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(500);
+ btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
+ world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
+ }
+ }
+ }
+
+ loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ m_data->m_KukaId = bodyId;
+ loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+
+ // Load one motor gripper for kuka
+ loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
+ m_data->m_gripperId = bodyId + 1;
+ InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
+ InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
+
+ // Reset the default gripper motor maximum torque for damping to 0
+ for (int i = 0; i < gripperBody->m_multiBody->getNumLinks(); i++)
+ {
+ if (supportsJointMotor(gripperBody->m_multiBody, i))
+ {
+ btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)gripperBody->m_multiBody->getLink(i).m_userPtr;
+ if (motor)
+ {
+ motor->setMaxAppliedImpulse(0);
+ }
+ }
+ }
+
+ for (int i = 0; i < 6; i++)
+ {
+ loadUrdf("jenga/jenga.urdf", btVector3(1.3-0.1*i,-0.7, .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ }
+
+ //loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+
+ // Add slider joint for fingers
+ btVector3 pivotInParent1(-0.055, 0, 0.02);
+ btVector3 pivotInChild1(0, 0, 0);
+ btMatrix3x3 frameInParent1(btQuaternion(0, 0, 0, 1.0));
+ btMatrix3x3 frameInChild1(btQuaternion(0, 0, 0, 1.0));
+ btVector3 jointAxis1(1.0, 0, 0);
+ btVector3 pivotInParent2(0.055, 0, 0.02);
+ btVector3 pivotInChild2(0, 0, 0);
+ btMatrix3x3 frameInParent2(btQuaternion(0, 0, 0, 1.0));
+ btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0));
+ btVector3 jointAxis2(1.0, 0, 0);
+ m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1);
+ m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0);
+ m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2);
+ m_data->m_kukaGripperRevolute2->setMaxAppliedImpulse(5.0);
+
+ m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
+ m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
+
+ if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
+ {
+ gripperBody->m_multiBody->setHasSelfCollision(0);
+ btVector3 pivotInParent(0, 0, 0.05);
+ btMatrix3x3 frameInParent;
+ frameInParent.setIdentity();
+ btVector3 pivotInChild(0, 0, 0);
+ btMatrix3x3 frameInChild;
+ frameInChild.setIdentity();
+
+ m_data->m_kukaGripperFixed = new btMultiBodyFixedConstraint(kukaBody->m_multiBody, 6, gripperBody->m_multiBody, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
+ m_data->m_kukaGripperMultiBody = gripperBody->m_multiBody;
+ m_data->m_kukaGripperFixed->setMaxAppliedImpulse(500);
+ m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperFixed);
+ }
+
+ for (int i = 0; i < 10; i++)
+ {
+ loadUrdf("cube.urdf", btVector3(-4, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ }
+ loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+
+ btTransform objectLocalTr[] = {
+ btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)),
+ btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.15, 0.64)),
+ btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.15, 0.85)),
+ btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.4, 0.05, 0.85)),
+ btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.3, -0.05, 0.7)),
+ btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.05, 0.7)),
+ btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.7)),
+ btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.9)),
+ btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.2, 0.05, 0.8))
+ };
+
+
+ btAlignedObjectArray<btTransform> objectWorldTr;
+ int numOb = sizeof(objectLocalTr) / sizeof(btTransform);
+ objectWorldTr.resize(numOb);
+
+ btTransform tr;
+ tr.setIdentity();
+ tr.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI));
+ tr.setOrigin(btVector3(1.0, -0.2, 0));
+
+ for (int i = 0; i < numOb; i++)
+ {
+ objectWorldTr[i] = tr*objectLocalTr[i];
+ }
+
+ // Table area
+ loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("tray.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ //loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ //loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("duck_vhacd.urdf", objectWorldTr[7].getOrigin(), objectWorldTr[7].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ //loadUrdf("Apple/apple.urdf", objectWorldTr[8].getOrigin(), objectWorldTr[8].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+
+ // Shelf area
+ loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
+ loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+
+ // Chess area
+ loadUrdf("table_square/table_square.urdf", btVector3(-1.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ //loadUrdf("pawn.urdf", btVector3(-0.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ //loadUrdf("queen.urdf", btVector3(-0.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ //loadUrdf("king.urdf", btVector3(-1.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ //loadUrdf("bishop.urdf", btVector3(-1.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ //loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ //loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+
+ //loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ m_data->m_huskyId = bodyId;
+
+ m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
+
+ }
+
+ if (m_data->m_kukaGripperFixed && m_data->m_kukaGripperMultiBody)
+ {
+ InteralBodyData* childBody = m_data->getHandle(m_data->m_gripperId);
+ // Add gripper controller
+ btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(1).m_userPtr;
+ if (motor)
+ {
+ btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75;
+ motor->setPositionTarget(posTarget, .2);
+ motor->setVelocityTarget(0.0, .5);
+ motor->setMaxAppliedImpulse(5.0);
+ }
+ }
+
+ if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)
+ {
+ m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
+ m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
+ for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++)
+ {
+ if (supportsJointMotor(m_data->m_gripperMultiBody, i))
+ {
+ btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr;
+ if (motor)
+ {
+ motor->setErp(0.2);
+ btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29;
+ btScalar maxPosTarget = 0.55;
+
+ if (m_data->m_gripperMultiBody->getJointPos(i) < 0)
+ {
+ m_data->m_gripperMultiBody->setJointPos(i,0);
+ }
+ if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget)
+ {
+ m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget);
+ }
+
+ motor->setPositionTarget(posTarget, 1);
+ motor->setVelocityTarget(0, 0.5);
+ btScalar maxImp = 1*m_data->m_physicsDeltaTime;
+ motor->setMaxAppliedImpulse(maxImp);
+ //motor->setRhsClamp(gRhsClamp);
+ }
+ }
+ }
+ }
+
+ // Inverse kinematics for KUKA
+ //if (0)
+ {
+ InternalBodyHandle* bodyHandle = m_data->getHandle(m_data->m_KukaId);
+ if (bodyHandle && bodyHandle->m_multiBody && bodyHandle->m_multiBody->getNumDofs()==7)
+ {
+ btMultiBody* mb = bodyHandle->m_multiBody;
+ btScalar sqLen = (mb->getBaseWorldTransform().getOrigin() - gVRController2Pos).length2();
+ btScalar distanceThreshold = 1.3;
+ gCloseToKuka=(sqLen<(distanceThreshold*distanceThreshold));
+
+ int numDofs = bodyHandle->m_multiBody->getNumDofs();
+ btAlignedObjectArray<double> q_new;
+ btAlignedObjectArray<double> q_current;
+ q_current.resize(numDofs);
+ for (int i = 0; i < numDofs; i++)
+ {
+ q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
+ }
+
+ q_new.resize(numDofs);
+ //sensible rest-pose
+ q_new[0] = 0;// -SIMD_HALF_PI;
+ q_new[1] = 0;
+ q_new[2] = 0;
+ q_new[3] = SIMD_HALF_PI;
+ q_new[4] = 0;
+ q_new[5] = -SIMD_HALF_PI*0.66;
+ q_new[6] = 0;
+
+ if (gCloseToKuka)
+ {
+ double dampIk[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 0.0};
+
+ IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
+ IKTrajectoryHelper* ikHelperPtr = 0;
+
+ if (ikHelperPtrPtr)
+ {
+ ikHelperPtr = *ikHelperPtrPtr;
+ }
+ else
+ {
+ IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
+ m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
+ ikHelperPtr = tmpHelper;
+ }
+
+ int endEffectorLinkIndex = 6;
+
+ if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
+ {
+ b3AlignedObjectArray<double> jacobian_linear;
+ jacobian_linear.resize(3*numDofs);
+ b3AlignedObjectArray<double> jacobian_angular;
+ jacobian_angular.resize(3*numDofs);
+ int jacSize = 0;
+
+ btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
+
+ if (tree)
+ {
+ jacSize = jacobian_linear.size();
+ // Set jacobian value
+ int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+
+ btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
+ for (int i = 0; i < numDofs; i++)
+ {
+ q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
+ q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i);
+ qdot[i + baseDofs] = 0;
+ nu[i+baseDofs] = 0;
+ }
+ // Set the gravity to correspond to the world gravity
+ btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
+
+ if (-1 != tree->setGravityInWorldFrame(id_grav) &&
+ -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
+ {
+ tree->calculateJacobians(q);
+ btInverseDynamics::mat3x jac_t(3,numDofs);
+ btInverseDynamics::mat3x jac_r(3,numDofs);
+ tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t);
+ tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r);
+ for (int i = 0; i < 3; ++i)
+ {
+ for (int j = 0; j < numDofs; ++j)
+ {
+ jacobian_linear[i*numDofs+j] = jac_t(i,j);
+ jacobian_angular[i*numDofs+j] = jac_r(i,j);
+ }
+ }
+ }
+ }
+
+ int ikMethod= IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;//IK2_VEL_DLS_WITH_ORIENTATION; //IK2_VEL_DLS;
+
+ btVector3DoubleData endEffectorWorldPosition;
+ btVector3DoubleData endEffectorWorldOrientation;
+ btVector3DoubleData targetWorldPosition;
+ btVector3DoubleData targetWorldOrientation;
+
+ btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin();
+ btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation();
+ btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
+
+ // Prescribed position and orientation
+ static btScalar time=0.f;
+ time+=0.01;
+ btVector3 targetPos(0.4-0.4*b3Cos( time), 0, 0.8+0.4*b3Cos( time));
+ targetPos +=mb->getBasePos();
+ btVector4 downOrn(0,1,0,0);
+
+ // Controller orientation
+ btVector4 controllerOrn(gVRController2Orn.x(), gVRController2Orn.y(), gVRController2Orn.z(), gVRController2Orn.w());
+
+ // Set position and orientation
+ endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
+ endEffectorOri.serializeDouble(endEffectorWorldOrientation);
+ downOrn.serializeDouble(targetWorldOrientation);
+ //targetPos.serializeDouble(targetWorldPosition);
+
+ gVRController2Pos.serializeDouble(targetWorldPosition);
+
+ //controllerOrn.serializeDouble(targetWorldOrientation);
+
+ if (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE)
+ {
+ btAlignedObjectArray<double> lower_limit;
+ btAlignedObjectArray<double> upper_limit;
+ btAlignedObjectArray<double> joint_range;
+ btAlignedObjectArray<double> rest_pose;
+ lower_limit.resize(numDofs);
+ upper_limit.resize(numDofs);
+ joint_range.resize(numDofs);
+ rest_pose.resize(numDofs);
+ lower_limit[0] = -.967;
+ lower_limit[1] = -2.0;
+ lower_limit[2] = -2.96;
+ lower_limit[3] = 0.19;
+ lower_limit[4] = -2.96;
+ lower_limit[5] = -2.09;
+ lower_limit[6] = -3.05;
+ upper_limit[0] = .96;
+ upper_limit[1] = 2.0;
+ upper_limit[2] = 2.96;
+ upper_limit[3] = 2.29;
+ upper_limit[4] = 2.96;
+ upper_limit[5] = 2.09;
+ upper_limit[6] = 3.05;
+ joint_range[0] = 5.8;
+ joint_range[1] = 4;
+ joint_range[2] = 5.8;
+ joint_range[3] = 4;
+ joint_range[4] = 5.8;
+ joint_range[5] = 4;
+ joint_range[6] = 6;
+ rest_pose[0] = 0;
+ rest_pose[1] = 0;
+ rest_pose[2] = 0;
+ rest_pose[3] = SIMD_HALF_PI;
+ rest_pose[4] = 0;
+ rest_pose[5] = -SIMD_HALF_PI*0.66;
+ rest_pose[6] = 0;
+ ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
+ }
+
+ ikHelperPtr->computeIK(targetWorldPosition.m_floats, targetWorldOrientation.m_floats,
+ endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
+ &q_current[0],
+ numDofs, endEffectorLinkIndex,
+ &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIk);
+ }
+ }
+
+ //directly set the position of the links, only for debugging IK, don't use this method!
+ if (0)
+ {
+ for (int i=0;i<mb->getNumLinks();i++)
+ {
+ btScalar desiredPosition = q_new[i];
+ mb->setJointPosMultiDof(i,&desiredPosition);
+ }
+ } else
+ {
+ int numMotors = 0;
+ //find the joint motors and apply the desired velocity and maximum force/torque
+ {
+ int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base
+ int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
+ for (int link=0;link<mb->getNumLinks();link++)
+ {
+ if (supportsJointMotor(mb,link))
+ {
+ btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
+
+ if (motor)
+ {
+ btScalar desiredVelocity = 0.f;
+ btScalar desiredPosition = q_new[link];
+ motor->setRhsClamp(gRhsClamp);
+ //printf("link %d: %f", link, q_new[link]);
+ motor->setVelocityTarget(desiredVelocity,1.0);
+ motor->setPositionTarget(desiredPosition,0.6);
+ btScalar maxImp = 1.0;
+
+ motor->setMaxAppliedImpulse(maxImp);
+ numMotors++;
+ }
+ }
+ velIndex += mb->getLink(link).m_dofCount;
+ posIndex += mb->getLink(link).m_posVarCount;
+ }
+ }
+ }
+ }
+
+ }
+ }
+
+ int maxSteps = m_data->m_numSimulationSubSteps+3;
+ if (m_data->m_numSimulationSubSteps)
+ {
+ gSubStep = m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps;
+ }
+ else
+ {
+ gSubStep = m_data->m_physicsDeltaTime;
+ }
+
+ int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec*simTimeScalingFactor,maxSteps, gSubStep);
+ gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
+
+ if (numSteps)
+ {
+ gNumSteps = numSteps;
+ gDtInSec = dtInSec;
+ }
+ }
+}
+
+void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId)
+{
+ InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+ if (body) {
+ btMultiBody* mb = body->m_multiBody;
+ if (mb) {
+ for (int l=0;l<mb->getNumLinks();l++) {
+ for (int d=0;d<mb->getLink(l).m_dofCount;d++) {
+ double damping_coefficient = mb->getLink(l).m_jointDamping;
+ double damping = -damping_coefficient*mb->getJointVelMultiDof(l)[d];
+ mb->addJointTorqueMultiDof(l, d, damping);
+ }
+ }
+ }
+ }
+}
+
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h
index 31448f2..6526fe0 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.h
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h
@@ -19,10 +19,17 @@ class PhysicsServerCommandProcessor
protected:
+
+
+ bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody);
+
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes);
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
+
+ int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
+ void deleteCachedInverseDynamicsBodies();
public:
PhysicsServerCommandProcessor();
@@ -32,6 +39,7 @@ public:
virtual void createEmptyDynamicsWorld();
virtual void deleteDynamicsWorld();
+
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes );
@@ -47,7 +55,9 @@ public:
void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName);
-
+ void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
+ void stepSimulationRealTime(double dtInSec);
+ void applyJointDamping(int bodyUniqueId);
};
#endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index 8f7df0c..ef7f140 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -1,35 +1,571 @@
+//todo(erwincoumans): re-use the upcoming b3RobotSimAPI here
#include "PhysicsServerExample.h"
#include "PhysicsServerSharedMemory.h"
-
+#include "Bullet3Common/b3CommandLineArgs.h"
#include "SharedMemoryCommon.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+#include "../Utils/b3Clock.h"
+#include "../MultiThreading/b3ThreadSupportInterface.h"
+#ifdef BT_ENABLE_VR
+#include "../RenderingExamples/TinyVRGui.h"
+#endif//BT_ENABLE_VR
+
+
+#include "../CommonInterfaces/CommonParameterInterface.h"
+
+#define MAX_VR_CONTROLLERS 8
+
+
+//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
+extern btVector3 gLastPickPos;
+btVector3 gVRTeleportPos(0,0,0);
+btQuaternion gVRTeleportOrn(0, 0, 0,1);
+extern btVector3 gVRGripperPos;
+extern btQuaternion gVRGripperOrn;
+extern btVector3 gVRController2Pos;
+extern btQuaternion gVRController2Orn;
+extern btScalar gVRGripperAnalog;
+extern btScalar gVRGripper2Analog;
+extern bool gCloseToKuka;
+extern bool gEnableRealTimeSimVR;
+extern bool gCreateSamuraiRobotAssets;
+extern int gCreateObjectSimVR;
+static int gGraspingController = -1;
+extern btScalar simTimeScalingFactor;
+
+extern bool gVRGripperClosed;
+
+bool gDebugRenderToggle = false;
+void MotionThreadFunc(void* userPtr,void* lsMemory);
+void* MotionlsMemoryFunc();
+#define MAX_MOTION_NUM_THREADS 1
+enum
+ {
+ numCubesX = 20,
+ numCubesY = 20
+ };
+
+
+enum TestExampleBrowserCommunicationEnums
+{
+ eRequestTerminateMotion= 13,
+ eMotionIsUnInitialized,
+ eMotionIsInitialized,
+ eMotionInitializationFailed,
+ eMotionHasTerminated
+};
+
+enum MultiThreadedGUIHelperCommunicationEnums
+{
+ eGUIHelperIdle= 13,
+ eGUIHelperRegisterTexture,
+ eGUIHelperRegisterGraphicsShape,
+ eGUIHelperRegisterGraphicsInstance,
+ eGUIHelperCreateCollisionShapeGraphicsObject,
+ eGUIHelperCreateCollisionObjectGraphicsObject,
+ eGUIHelperCreateRigidBodyGraphicsObject,
+ eGUIHelperRemoveAllGraphicsInstances,
+ eGUIHelperCopyCameraImageData,
+};
+
+#include <stdio.h>
+//#include "BulletMultiThreaded/PlatformDefinitions.h"
+
+#ifndef _WIN32
+#include "../MultiThreading/b3PosixThreadSupport.h"
+
+b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
+{
+ b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("MotionThreads",
+ MotionThreadFunc,
+ MotionlsMemoryFunc,
+ numThreads);
+ b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
+
+ return threadSupport;
+
+}
+
+
+#elif defined( _WIN32)
+#include "../MultiThreading/b3Win32ThreadSupport.h"
+
+b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
+{
+ b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("MotionThreads",MotionThreadFunc,MotionlsMemoryFunc,numThreads);
+ b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
+ return threadSupport;
+
+}
+#endif
+
+
+
+struct MotionArgs
+{
+ MotionArgs()
+ :m_physicsServerPtr(0)
+ {
+ for (int i=0;i<MAX_VR_CONTROLLERS;i++)
+ {
+ m_isVrControllerPicking[i] = false;
+ m_isVrControllerDragging[i] = false;
+ m_isVrControllerReleasing[i] = false;
+ m_isVrControllerTeleporting[i] = false;
+ }
+ }
+ b3CriticalSection* m_cs;
+
+ PhysicsServerSharedMemory* m_physicsServerPtr;
+ b3AlignedObjectArray<b3Vector3> m_positions;
+
+ btVector3 m_vrControllerPos[MAX_VR_CONTROLLERS];
+ btQuaternion m_vrControllerOrn[MAX_VR_CONTROLLERS];
+ bool m_isVrControllerPicking[MAX_VR_CONTROLLERS];
+ bool m_isVrControllerDragging[MAX_VR_CONTROLLERS];
+ bool m_isVrControllerReleasing[MAX_VR_CONTROLLERS];
+ bool m_isVrControllerTeleporting[MAX_VR_CONTROLLERS];
+
+};
+
+struct MotionThreadLocalStorage
+{
+ int threadId;
+};
+
+int skip = 0;
+int skip1 = 0;
+float clampedDeltaTime = 0.2;
+
+void MotionThreadFunc(void* userPtr,void* lsMemory)
+{
+ printf("MotionThreadFunc thread started\n");
+ MotionThreadLocalStorage* localStorage = (MotionThreadLocalStorage*) lsMemory;
+
+ MotionArgs* args = (MotionArgs*) userPtr;
+ int workLeft = true;
+ b3Clock clock;
+ clock.reset();
+ bool init = true;
+ if (init)
+ {
+
+ args->m_cs->lock();
+ args->m_cs->setSharedParam(0,eMotionIsInitialized);
+ args->m_cs->unlock();
+
+
+ double deltaTimeInSeconds = 0;
+
+ do
+ {
+ deltaTimeInSeconds+= double(clock.getTimeMicroseconds())/1000000.;
+
+ if (deltaTimeInSeconds<(1./5000.))
+ {
+
+ skip++;
+ skip1++;
+ if (skip1>5)
+ {
+ b3Clock::usleep(250);
+ }
+ } else
+ {
+ skip1=0;
+
+ //process special controller commands, such as
+ //VR controller button press/release and controller motion
+
+ for (int c=0;c<MAX_VR_CONTROLLERS;c++)
+ {
+
+ btVector3 from = args->m_vrControllerPos[c];
+ btMatrix3x3 mat(args->m_vrControllerOrn[c]);
+
+ btScalar pickDistance = 1000.;
+ btVector3 toX = from+mat.getColumn(0);
+ btVector3 toY = from+mat.getColumn(1);
+ btVector3 toZ = from+mat.getColumn(2)*pickDistance;
+
+ if (args->m_isVrControllerTeleporting[c])
+ {
+ args->m_isVrControllerTeleporting[c] = false;
+ args->m_physicsServerPtr->pickBody(from,-toZ);
+ args->m_physicsServerPtr->removePickingConstraint();
+ }
+
+ if (!gCloseToKuka)
+ {
+ if (args->m_isVrControllerPicking[c])
+ {
+ args->m_isVrControllerPicking[c] = false;
+ args->m_isVrControllerDragging[c] = true;
+ args->m_physicsServerPtr->pickBody(from,-toZ);
+ //printf("PICK!\n");
+ }
+ }
+
+ if (args->m_isVrControllerDragging[c])
+ {
+ args->m_physicsServerPtr->movePickedBody(from,-toZ);
+ // printf(".");
+ }
+
+ if (args->m_isVrControllerReleasing[c])
+ {
+ args->m_isVrControllerDragging[c] = false;
+ args->m_isVrControllerReleasing[c] = false;
+ args->m_physicsServerPtr->removePickingConstraint();
+ //printf("Release pick\n");
+ }
+ }
+
+ //don't simulate over a huge timestep if we had some interruption (debugger breakpoint etc)
+ if (deltaTimeInSeconds>clampedDeltaTime)
+ {
+ deltaTimeInSeconds = clampedDeltaTime;
+ b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
+ }
+
+ clock.reset();
+ args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
+ deltaTimeInSeconds = 0;
+
+ }
+
+ args->m_physicsServerPtr->processClientCommands();
+
+ } while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion);
+ } else
+ {
+ args->m_cs->lock();
+ args->m_cs->setSharedParam(0,eMotionInitializationFailed);
+ args->m_cs->unlock();
+ }
+
+
+ printf("finished, #skip = %d, skip1 = %d\n",skip,skip1);
+ skip=0;
+ skip1=0;
+ //do nothing
+
+}
+
+
+
+void* MotionlsMemoryFunc()
+{
+ //don't create local store memory, just return 0
+ return new MotionThreadLocalStorage;
+}
+
+
+
+class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
+{
+ CommonGraphicsApp* m_app;
+
+ b3CriticalSection* m_cs;
+
+
+public:
+
+ GUIHelperInterface* m_childGuiHelper;
+
+ const unsigned char* m_texels;
+ int m_textureWidth;
+ int m_textureHeight;
+
+
+ int m_shapeIndex;
+ const float* m_position;
+ const float* m_quaternion;
+ const float* m_color;
+ const float* m_scaling;
+
+ const float* m_vertices;
+ int m_numvertices;
+ const int* m_indices;
+ int m_numIndices;
+ int m_primitiveType;
+ int m_textureId;
+ int m_instanceId;
+
+
+ MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
+ :m_app(app)
+ ,m_cs(0),
+ m_texels(0),
+ m_textureId(-1)
+ {
+ m_childGuiHelper = guiHelper;;
+
+ }
+
+ virtual ~MultiThreadedOpenGLGuiHelper()
+ {
+ delete m_childGuiHelper;
+ }
+
+ void setCriticalSection(b3CriticalSection* cs)
+ {
+ m_cs = cs;
+ }
+
+ b3CriticalSection* getCriticalSection()
+ {
+ return m_cs;
+ }
+
+ btRigidBody* m_body;
+ btVector3 m_color3;
+ virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color)
+ {
+ m_body = body;
+ m_color3 = color;
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperCreateRigidBodyGraphicsObject);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ b3Clock::usleep(1000);
+ }
+ }
+
+ btCollisionObject* m_obj;
+ btVector3 m_color2;
+
+ virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
+ {
+ m_obj = obj;
+ m_color2 = color;
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperCreateCollisionObjectGraphicsObject);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ b3Clock::usleep(1000);
+ }
+
+ }
+
+ btCollisionShape* m_colShape;
+ virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
+ {
+ m_colShape = collisionShape;
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperCreateCollisionShapeGraphicsObject);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ b3Clock::usleep(1000);
+ }
+
+ }
+
+ virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
+ {
+ //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects.
+ //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms
+ if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0)
+ {
+ m_childGuiHelper->syncPhysicsToGraphics(rbWorld);
+ }
+ }
+
+ virtual void render(const btDiscreteDynamicsWorld* rbWorld)
+ {
+ m_childGuiHelper->render(0);
+ }
+
+ virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld)
+ {
+ m_childGuiHelper->createPhysicsDebugDrawer(rbWorld);
+ }
+
+ virtual int registerTexture(const unsigned char* texels, int width, int height)
+ {
+ m_texels = texels;
+ m_textureWidth = width;
+ m_textureHeight = height;
+
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperRegisterTexture);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ b3Clock::usleep(1000);
+ }
+ return m_textureId;
+ }
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
+ {
+ m_vertices = vertices;
+ m_numvertices = numvertices;
+ m_indices = indices;
+ m_numIndices = numIndices;
+ m_primitiveType = primitiveType;
+ m_textureId = textureId;
+
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsShape);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ b3Clock::usleep(1000);
+ }
+ return m_shapeIndex;
+ }
+ virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
+ {
+ m_shapeIndex = shapeIndex;
+ m_position = position;
+ m_quaternion = quaternion;
+ m_color = color;
+ m_scaling = scaling;
+
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsInstance);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ b3Clock::usleep(1000);
+ }
+ return m_instanceId;
+ }
+
+ virtual void removeAllGraphicsInstances()
+ {
+ m_cs->lock();
+ m_cs->setSharedParam(1,eGUIHelperRemoveAllGraphicsInstances);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ b3Clock::usleep(1000);
+ }
+ }
+
+ virtual Common2dCanvasInterface* get2dCanvasInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonParameterInterface* getParameterInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonRenderInterface* getRenderInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonGraphicsApp* getAppInterface()
+ {
+ return m_childGuiHelper->getAppInterface();
+ }
+
+
+ virtual void setUpAxis(int axis)
+ {
+ m_childGuiHelper->setUpAxis(axis);
+ }
+ virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
+ {
+ m_childGuiHelper->resetCamera(camDist,pitch,yaw,camPosX,camPosY,camPosZ);
+ }
+
+ float m_viewMatrix[16];
+ float m_projectionMatrix[16];
+ unsigned char* m_pixelsRGBA;
+ int m_rgbaBufferSizeInPixels;
+ float* m_depthBuffer;
+ int m_depthBufferSizeInPixels;
+ int* m_segmentationMaskBuffer;
+ int m_segmentationMaskBufferSizeInPixels;
+ int m_startPixelIndex;
+ int m_destinationWidth;
+ int m_destinationHeight;
+ int* m_numPixelsCopied;
+
+ virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
+ unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
+ float* depthBuffer, int depthBufferSizeInPixels,
+ int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
+ int startPixelIndex, int destinationWidth,
+ int destinationHeight, int* numPixelsCopied)
+ {
+ m_cs->lock();
+ for (int i=0;i<16;i++)
+ {
+ m_viewMatrix[i] = viewMatrix[i];
+ m_projectionMatrix[i] = projectionMatrix[i];
+ }
+ m_pixelsRGBA = pixelsRGBA;
+ m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels;
+ m_depthBuffer = depthBuffer;
+ m_depthBufferSizeInPixels = depthBufferSizeInPixels;
+ m_segmentationMaskBuffer = segmentationMaskBuffer;
+ m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels;
+ m_startPixelIndex = startPixelIndex;
+ m_destinationWidth = destinationWidth;
+ m_destinationHeight = destinationHeight;
+ m_numPixelsCopied = numPixelsCopied;
+
+ m_cs->setSharedParam(1,eGUIHelperCopyCameraImageData);
+ m_cs->unlock();
+ while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+ {
+ b3Clock::usleep(1000);
+ }
+ }
+
+
+ virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
+ {
+ }
+
+ virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
+ {
+ }
+};
+
class PhysicsServerExample : public SharedMemoryCommon
{
PhysicsServerSharedMemory m_physicsServer;
-
+ b3ThreadSupportInterface* m_threadSupport;
+ MotionArgs m_args[MAX_MOTION_NUM_THREADS];
+ MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper;
bool m_wantsShutdown;
bool m_isConnected;
btClock m_clock;
bool m_replay;
+ int m_options;
+#ifdef BT_ENABLE_VR
+ TinyVRGui* m_tinyVrGui;
+#endif
+
public:
-
- PhysicsServerExample(GUIHelperInterface* helper);
-
+
+ PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem=0, int options=0);
+
virtual ~PhysicsServerExample();
-
+
virtual void initPhysics();
-
+
virtual void stepSimulation(float deltaTime);
-
+
void enableCommandLogging()
{
m_physicsServer.enableCommandLogging(true,"BulletPhysicsCommandLog.bin");
@@ -40,9 +576,9 @@ public:
m_replay = true;
m_physicsServer.replayFromLogFile("BulletPhysicsCommandLog.bin");
}
-
-
+
+
virtual void resetCamera()
{
float dist = 5;
@@ -51,26 +587,28 @@ public:
float targetPos[3]={0,0,0};//-3,2.8,-2.5};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
-
+
virtual bool wantsTermination();
virtual bool isConnected();
virtual void renderScene();
- virtual void exitPhysics(){}
+ virtual void exitPhysics();
virtual void physicsDebugDraw(int debugFlags);
-
+
btVector3 getRayTo(int x,int y);
-
+
+ virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]);
+ virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis);
+
virtual bool mouseMoveCallback(float x,float y)
{
if (m_replay)
return false;
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
-
+
if (!renderer)
{
- btAssert(0);
return false;
}
@@ -87,16 +625,15 @@ public:
return false;
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
-
+
if (!renderer)
{
- btAssert(0);
return false;
}
-
+
CommonWindowInterface* window = m_guiHelper->getAppInterface()->m_window;
-
+
if (state==1)
{
if(button==0 && (!window->isModifierKeyPressed(B3G_ALT) && !window->isModifierKeyPressed(B3G_CONTROL) ))
@@ -130,15 +667,31 @@ public:
m_physicsServer.setSharedMemoryKey(key);
}
+ virtual void processCommandLineArgs(int argc, char* argv[])
+ {
+ b3CommandLineArgs args(argc,argv);
+ if (args.CheckCmdLineFlag("emptyworld"))
+ {
+ gCreateSamuraiRobotAssets = false;
+ }
+ }
+
+
};
-PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper)
+PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem, int options)
:SharedMemoryCommon(helper),
+m_physicsServer(sharedMem),
m_wantsShutdown(false),
m_isConnected(false),
-m_replay(false)
+m_replay(false),
+m_options(options)
+#ifdef BT_ENABLE_VR
+,m_tinyVrGui(0)
+#endif
{
+ m_multiThreadedHelper = helper;
b3Printf("Started PhysicsServer\n");
}
@@ -146,6 +699,9 @@ m_replay(false)
PhysicsServerExample::~PhysicsServerExample()
{
+#ifdef BT_ENABLE_VR
+ delete m_tinyVrGui;
+#endif
bool deInitializeSharedMemory = true;
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
m_isConnected = false;
@@ -162,22 +718,83 @@ void PhysicsServerExample::initPhysics()
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
-#if 0
-
- createEmptyDynamicsWorld();
- //todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
- btVector3 grav(0,0,0);
- grav[upAxis] = 0;//-9.8;
- this->m_dynamicsWorld->setGravity(grav);
-
-#endif
- m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
-
+
+
+ m_threadSupport = createMotionThreadSupport(MAX_MOTION_NUM_THREADS);
+
+
+
+ for (int i=0;i<m_threadSupport->getNumTasks();i++)
+ {
+ MotionThreadLocalStorage* storage = (MotionThreadLocalStorage*) m_threadSupport->getThreadLocalMemory(i);
+ b3Assert(storage);
+ storage->threadId = i;
+ //storage->m_sharedMem = data->m_sharedMem;
+ }
+
+
+
+
+ for (int w=0;w<MAX_MOTION_NUM_THREADS;w++)
+ {
+ m_args[w].m_cs = m_threadSupport->createCriticalSection();
+ m_args[w].m_cs->setSharedParam(0,eMotionIsUnInitialized);
+ int numMoving = 0;
+ m_args[w].m_positions.resize(numMoving);
+ m_args[w].m_physicsServerPtr = &m_physicsServer;
+ int index = 0;
+
+ m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &this->m_args[w], w);
+
+ while (m_args[w].m_cs->getSharedParam(0)==eMotionIsUnInitialized)
+ {
+ b3Clock::usleep(1000);
+ }
+ }
+
+ m_args[0].m_cs->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->setCriticalSection(m_args[0].m_cs);
+ m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
+}
+
+
+void PhysicsServerExample::exitPhysics()
+{
+ for (int i=0;i<MAX_MOTION_NUM_THREADS;i++)
+ {
+ m_args[i].m_cs->lock();
+ m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion);
+ m_args[i].m_cs->unlock();
+ }
+ int numActiveThreads = MAX_MOTION_NUM_THREADS;
+
+ while (numActiveThreads)
+ {
+ int arg0,arg1;
+ if (m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
+ {
+ numActiveThreads--;
+ printf("numActiveThreads = %d\n",numActiveThreads);
+
+ } else
+ {
+ b3Clock::usleep(1000);
+ }
+ };
+
+ printf("stopping threads\n");
+
+ delete m_threadSupport;
+ m_threadSupport = 0;
+
+ //m_physicsServer.resetDynamicsWorld();
+
}
+
bool PhysicsServerExample::wantsTermination()
{
return m_wantsShutdown;
@@ -187,11 +804,122 @@ bool PhysicsServerExample::wantsTermination()
void PhysicsServerExample::stepSimulation(float deltaTime)
{
- if (m_replay)
+ //this->m_physicsServer.processClientCommands();
+
+ //check if any graphics related tasks are requested
+
+ switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
{
- for (int i=0;i<100;i++)
- m_physicsServer.processClientCommands();
- } else
+ case eGUIHelperCreateCollisionShapeGraphicsObject:
+ {
+ m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_multiThreadedHelper->m_colShape);
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+
+ break;
+ }
+ case eGUIHelperCreateCollisionObjectGraphicsObject:
+ {
+ m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_multiThreadedHelper->m_obj,
+ m_multiThreadedHelper->m_color2);
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eGUIHelperCreateRigidBodyGraphicsObject:
+ {
+ m_multiThreadedHelper->m_childGuiHelper->createRigidBodyGraphicsObject(m_multiThreadedHelper->m_body,m_multiThreadedHelper->m_color3);
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eGUIHelperRegisterTexture:
+ {
+
+ m_multiThreadedHelper->m_textureId = m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_multiThreadedHelper->m_texels,
+ m_multiThreadedHelper->m_textureWidth,m_multiThreadedHelper->m_textureHeight);
+
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+
+ break;
+ }
+ case eGUIHelperRegisterGraphicsShape:
+ {
+ m_multiThreadedHelper->m_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape(
+ m_multiThreadedHelper->m_vertices,
+ m_multiThreadedHelper->m_numvertices,
+ m_multiThreadedHelper->m_indices,
+ m_multiThreadedHelper->m_numIndices,
+ m_multiThreadedHelper->m_primitiveType,
+ m_multiThreadedHelper->m_textureId);
+
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eGUIHelperRegisterGraphicsInstance:
+ {
+ m_multiThreadedHelper->m_instanceId = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance(
+ m_multiThreadedHelper->m_shapeIndex,
+ m_multiThreadedHelper->m_position,
+ m_multiThreadedHelper->m_quaternion,
+ m_multiThreadedHelper->m_color,
+ m_multiThreadedHelper->m_scaling);
+
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eGUIHelperRemoveAllGraphicsInstances:
+ {
+ m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
+ int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
+ b3Assert(numRenderInstances==0);
+
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+
+ case eGUIHelperCopyCameraImageData:
+ {
+ m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_multiThreadedHelper->m_viewMatrix,
+ m_multiThreadedHelper->m_projectionMatrix,
+ m_multiThreadedHelper->m_pixelsRGBA,
+ m_multiThreadedHelper->m_rgbaBufferSizeInPixels,
+ m_multiThreadedHelper->m_depthBuffer,
+ m_multiThreadedHelper->m_depthBufferSizeInPixels,
+ m_multiThreadedHelper->m_segmentationMaskBuffer,
+ m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels,
+ m_multiThreadedHelper->m_startPixelIndex,
+ m_multiThreadedHelper->m_destinationWidth,
+ m_multiThreadedHelper->m_destinationHeight,
+ m_multiThreadedHelper->m_numPixelsCopied);
+ m_multiThreadedHelper->getCriticalSection()->lock();
+ m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+ m_multiThreadedHelper->getCriticalSection()->unlock();
+ break;
+ }
+ case eGUIHelperIdle:
+ default:
+ {
+
+ }
+ }
+
+
+
+
+ #if 0
+ if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK)
{
btClock rtc;
btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
@@ -200,13 +928,195 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
{
m_physicsServer.processClientCommands();
}
+ } else
+ {
+ //for (int i=0;i<10;i++)
+ m_physicsServer.processClientCommands();
+ }
+ #endif
+
+ {
+ if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
+ {
+ m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
+ }
}
}
+static float vrOffset[16]={1,0,0,0,
+ 0,1,0,0,
+ 0,0,1,0,
+ 0,0,0,0};
+
+
+extern int gDroppedSimulationSteps;
+extern int gNumSteps;
+extern double gDtInSec;
+extern double gSubStep;
+
+
void PhysicsServerExample::renderScene()
{
+ B3_PROFILE("PhysicsServerExample::RenderScene");
+ static char line0[1024];
+ static char line1[1024];
+
+ if (gEnableRealTimeSimVR)
+ {
+
+ static int frameCount=0;
+ static btScalar prevTime = m_clock.getTimeSeconds();
+ frameCount++;
+
+ static btScalar worseFps = 1000000;
+ int numFrames = 200;
+ static int count = 0;
+ count++;
+
+ if (0 == (count & 1))
+ {
+ btScalar curTime = m_clock.getTimeSeconds();
+ btScalar fps = 1. / (curTime - prevTime);
+ prevTime = curTime;
+ if (fps < worseFps)
+ {
+ worseFps = fps;
+ }
+
+ if (count > numFrames)
+ {
+ count = 0;
+ sprintf(line0, "fps:%f frame:%d", worseFps, frameCount / 2);
+ sprintf(line1, "drop:%d tscale:%f dt:%f, substep %f)", gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep);
+ gDroppedSimulationSteps = 0;
+
+ worseFps = 1000000;
+ }
+ }
+
+#ifdef BT_ENABLE_VR
+ if (m_tinyVrGui==0)
+ {
+ ComboBoxParams comboParams;
+ comboParams.m_comboboxId = 0;
+ comboParams.m_numItems = 0;
+ comboParams.m_startItem = 0;
+ comboParams.m_callback = 0;//MyComboBoxCallback;
+ comboParams.m_userPointer = 0;//this;
+
+ m_tinyVrGui = new TinyVRGui(comboParams,this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface());
+ m_tinyVrGui->init();
+ }
+
+ if (m_tinyVrGui)
+ {
+
+ b3Transform tr;tr.setIdentity();
+ tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2]));
+ tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3]));
+ tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
+ b3Scalar dt = 0.01;
+ m_tinyVrGui->clearTextArea();
+
+ m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255);
+ m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255);
+
+ m_tinyVrGui->tick(dt,tr);
+ }
+#endif//BT_ENABLE_VR
+ }
///debug rendering
+ //m_args[0].m_cs->lock();
+
+ //gVRTeleportPos[0] += 0.01;
+ vrOffset[12]=-gVRTeleportPos[0];
+ vrOffset[13]=-gVRTeleportPos[1];
+ vrOffset[14]=-gVRTeleportPos[2];
+
+ this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->
+ getActiveCamera()->setVRCameraOffsetTransform(vrOffset);
+
m_physicsServer.renderScene();
+
+ for (int i=0;i<MAX_VR_CONTROLLERS;i++)
+ {
+ if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i])
+ {
+ btVector3 from = m_args[0].m_vrControllerPos[i];
+ btMatrix3x3 mat(m_args[0].m_vrControllerOrn[i]);
+
+ btVector3 toX = from+mat.getColumn(0);
+ btVector3 toY = from+mat.getColumn(1);
+ btVector3 toZ = from+mat.getColumn(2);
+
+ int width = 2;
+
+
+ btVector4 color;
+ color=btVector4(1,0,0,1);
+ m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width);
+ color=btVector4(0,1,0,1);
+ m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
+ color=btVector4(0,0,1,1);
+ m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
+
+ }
+ }
+
+ if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
+ {
+ gEnableRealTimeSimVR = true;
+ }
+
+ if (gDebugRenderToggle)
+ if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
+ {
+
+ B3_PROFILE("Draw Debug HUD");
+ //some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
+
+
+ float pos[4];
+ m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos);
+ pos[0]+=gVRTeleportPos[0];
+ pos[1]+=gVRTeleportPos[1];
+ pos[2]+=gVRTeleportPos[2];
+
+ btTransform viewTr;
+ btScalar m[16];
+ float mf[16];
+ m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraViewMatrix(mf);
+ for (int i=0;i<16;i++)
+ {
+ m[i] = mf[i];
+ }
+ m[12]=+gVRTeleportPos[0];
+ m[13]=+gVRTeleportPos[1];
+ m[14]=+gVRTeleportPos[2];
+ viewTr.setFromOpenGLMatrix(m);
+ btTransform viewTrInv = viewTr.inverse();
+
+ btVector3 side = viewTrInv.getBasis().getColumn(0);
+ btVector3 up = viewTrInv.getBasis().getColumn(1);
+ btVector3 fwd = viewTrInv.getBasis().getColumn(2);
+
+
+ float upMag = 0;
+ float sideMag = 2.2;
+ float fwdMag = -4;
+
+ m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1);
+ //btVector3 fwd = viewTrInv.getBasis().getColumn(2);
+
+ up = viewTrInv.getBasis().getColumn(1);
+ upMag = -0.3;
+
+
+
+ m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1);
+ }
+
+ //m_args[0].m_cs->unlock();
}
void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
@@ -221,7 +1131,7 @@ void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
btVector3 PhysicsServerExample::getRayTo(int x,int y)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
-
+
if (!renderer)
{
btAssert(0);
@@ -237,7 +1147,7 @@ btVector3 PhysicsServerExample::getRayTo(int x,int y)
btVector3 camPos,camTarget;
renderer->getActiveCamera()->getCameraPosition(camPos);
renderer->getActiveCamera()->getCameraTargetPosition(camTarget);
-
+
btVector3 rayFrom = camPos;
btVector3 rayForward = (camTarget-camPos);
rayForward.normalize();
@@ -287,7 +1197,13 @@ extern int gSharedMemoryKey;
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options)
{
- PhysicsServerExample* example = new PhysicsServerExample(options.m_guiHelper);
+
+ MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper);
+
+ PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper,
+ options.m_sharedMem,
+ options.m_option);
+
if (gSharedMemoryKey>=0)
{
example->setSharedMemoryKey(gSharedMemoryKey);
@@ -301,5 +1217,118 @@ class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOpt
example->replayFromLogFile();
}
return example;
+
+}
+
+
+
+void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
+{
+ //printf("controllerId %d, button=%d\n",controllerId, button);
+ if (controllerId<0 || controllerId>=MAX_VR_CONTROLLERS)
+ return;
+
+ if (gGraspingController < 0)
+ gGraspingController = controllerId;
+
+ if (controllerId != gGraspingController)
+ {
+ if (button == 1 && state == 0)
+ {
+ gVRTeleportPos = gLastPickPos;
+ }
+ } else
+ {
+ if (button == 1)
+ {
+ if (state == 1)
+ {
+ gDebugRenderToggle = 1;
+ } else
+ {
+ gDebugRenderToggle = 0;
+
+ if (simTimeScalingFactor==0)
+ {
+ simTimeScalingFactor = 1;
+ } else
+ {
+ if (simTimeScalingFactor==1)
+ {
+ simTimeScalingFactor = 0.25;
+ }
+ else
+ {
+ simTimeScalingFactor = 0;
+ }
+ }
+ }
+ } else
+ {
+
+ }
+ }
+ if (button==32 && state==0)
+ {
+ gCreateObjectSimVR = 1;
+ }
+
+
+ if (button==1)
+ {
+ m_args[0].m_isVrControllerTeleporting[controllerId] = true;
+ }
+
+ if (controllerId == gGraspingController && (button == 33))
+ {
+ gVRGripperClosed =state;
+ }
+ else
+ {
+
+ if (button == 33)
+ {
+ m_args[0].m_isVrControllerPicking[controllerId] = (state != 0);
+ m_args[0].m_isVrControllerReleasing[controllerId] = (state == 0);
+ }
+ if ((button == 33) || (button == 1))
+ {
+ m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
+ m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
+ }
+ }
+}
+
+
+
+void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
+{
+
+ gEnableRealTimeSimVR = true;
+
+ if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
+ {
+ printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
+ return;
+ }
+ if (controllerId == gGraspingController)
+ {
+ gVRGripperAnalog = analogAxis;
+ gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
+ btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]);
+ gVRGripperOrn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
+ }
+ else
+ {
+ gVRGripper2Analog = analogAxis;
+ gVRController2Pos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
+ btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]);
+ gVRController2Orn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
+
+ m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
+ m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
+ }
+
}
+B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)
diff --git a/examples/SharedMemory/PhysicsServerExample.h b/examples/SharedMemory/PhysicsServerExample.h
index e880742..8df83bc 100644
--- a/examples/SharedMemory/PhysicsServerExample.h
+++ b/examples/SharedMemory/PhysicsServerExample.h
@@ -5,6 +5,7 @@ enum PhysicsServerOptions
{
PHYSICS_SERVER_ENABLE_COMMAND_LOGGING=1,
PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG=2,
+ PHYSICS_SERVER_USE_RTC_CLOCK = 4,
};
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options);
diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp
index a00f3d1..111ac07 100644
--- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp
@@ -23,6 +23,8 @@ struct PhysicsServerSharedMemoryInternalData
SharedMemoryInterface* m_sharedMemory;
+ bool m_ownsSharedMemory;
+
SharedMemoryBlock* m_testBlock1;
int m_sharedMemoryKey;
bool m_isConnected;
@@ -31,6 +33,7 @@ struct PhysicsServerSharedMemoryInternalData
PhysicsServerSharedMemoryInternalData()
:m_sharedMemory(0),
+ m_ownsSharedMemory(false),
m_testBlock1(0),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_isConnected(false),
@@ -57,18 +60,24 @@ struct PhysicsServerSharedMemoryInternalData
};
-PhysicsServerSharedMemory::PhysicsServerSharedMemory()
+PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* sharedMem)
{
m_data = new PhysicsServerSharedMemoryInternalData();
-
+ if (sharedMem)
+ {
+ m_data->m_sharedMemory = sharedMem;
+ m_data->m_ownsSharedMemory = false;
+ } else
+ {
#ifdef _WIN32
m_data->m_sharedMemory = new Win32SharedMemoryServer();
#else
m_data->m_sharedMemory = new PosixSharedMemory();
#endif
-
+ m_data->m_ownsSharedMemory = true;
+ }
+
m_data->m_commandProcessor = new PhysicsServerCommandProcessor;
- m_data->m_commandProcessor ->createEmptyDynamicsWorld();
}
@@ -80,6 +89,11 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
delete m_data;
}
+void PhysicsServerSharedMemory::resetDynamicsWorld()
+{
+ m_data->m_commandProcessor->deleteDynamicsWorld();
+ m_data->m_commandProcessor ->createEmptyDynamicsWorld();
+}
void PhysicsServerSharedMemory::setSharedMemoryKey(int key)
{
m_data->m_sharedMemoryKey = key;
@@ -176,7 +190,10 @@ void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMe
{
b3Printf("m_sharedMemory\n");
}
- delete m_data->m_sharedMemory;
+ if (m_data->m_ownsSharedMemory)
+ {
+ delete m_data->m_sharedMemory;
+ }
m_data->m_sharedMemory = 0;
m_data->m_testBlock1 = 0;
}
@@ -209,38 +226,28 @@ void PhysicsServerSharedMemory::releaseSharedMemory()
{
b3Printf("m_sharedMemory\n");
}
- delete m_data->m_sharedMemory;
+ if (m_data->m_ownsSharedMemory)
+ {
+ delete m_data->m_sharedMemory;
+ }
m_data->m_sharedMemory = 0;
m_data->m_testBlock1 = 0;
}
}
-
-
+void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec)
+{
+ m_data->m_commandProcessor->stepSimulationRealTime(dtInSec);
+}
void PhysicsServerSharedMemory::processClientCommands()
{
if (m_data->m_isConnected && m_data->m_testBlock1)
{
-#if 0
- m_data->m_commandProcessor->processLogCommand();
-
- if (m_data->m_logPlayback)
- {
- if (m_data->m_testBlock1->m_numServerCommands>m_data->m_testBlock1->m_numProcessedServerCommands)
- {
- m_data->m_testBlock1->m_numProcessedServerCommands++;
- }
- //push a command from log file
- bool hasCommand = m_data->m_logPlayback->processNextCommand(&m_data->m_testBlock1->m_clientCommands[0]);
- if (hasCommand)
- {
- m_data->m_testBlock1->m_numClientCommands++;
- }
- }
-#endif
+ m_data->m_commandProcessor->replayLogCommand(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+
///we ignore overflow of integer for now
if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands)
{
diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h
index 7547bd9..f96e7ca 100644
--- a/examples/SharedMemory/PhysicsServerSharedMemory.h
+++ b/examples/SharedMemory/PhysicsServerSharedMemory.h
@@ -14,7 +14,7 @@ protected:
public:
- PhysicsServerSharedMemory();
+ PhysicsServerSharedMemory(class SharedMemoryInterface* sharedMem=0);
virtual ~PhysicsServerSharedMemory();
virtual void setSharedMemoryKey(int key);
@@ -26,6 +26,8 @@ public:
virtual void processClientCommands();
+ virtual void stepSimulationRealTime(double dtInSec);
+
//bool supportsJointMotor(class btMultiBody* body, int linkIndex);
//@todo(erwincoumans) Should we have shared memory commands for picking objects?
@@ -43,6 +45,7 @@ public:
void enableCommandLogging(bool enable, const char* fileName);
void replayFromLogFile(const char* fileName);
+ void resetDynamicsWorld();
};
diff --git a/examples/SharedMemory/PosixSharedMemory.cpp b/examples/SharedMemory/PosixSharedMemory.cpp
index b4ecad8..0297d43 100644
--- a/examples/SharedMemory/PosixSharedMemory.cpp
+++ b/examples/SharedMemory/PosixSharedMemory.cpp
@@ -2,7 +2,7 @@
#include "Bullet3Common/b3Logging.h"
#include "LinearMath/btScalar.h" //for btAssert
-//haven't implemented shared memory on Windows yet, just Linux and Mac
+//Windows implementation is in Win32SharedMemory.cpp
#ifndef _WIN32
#define TEST_SHARED_MEMORY
#endif//_WIN32
diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp
index 1a0dad3..ad5d5f8 100644
--- a/examples/SharedMemory/RobotControlExample.cpp
+++ b/examples/SharedMemory/RobotControlExample.cpp
@@ -15,7 +15,6 @@
#include <string>
//const char* blaatnaam = "basename";
-#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE 1024
struct MyMotorInfo
{
diff --git a/examples/SharedMemory/SharedMemoryBlock.h b/examples/SharedMemory/SharedMemoryBlock.h
index b737765..f266db4 100644
--- a/examples/SharedMemory/SharedMemoryBlock.h
+++ b/examples/SharedMemory/SharedMemoryBlock.h
@@ -3,7 +3,6 @@
#define SHARED_MEMORY_MAGIC_NUMBER 64738
#define SHARED_MEMORY_MAX_COMMANDS 4
-#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024)
#include "SharedMemoryCommands.h"
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index e0b2bf9..adfd533 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -27,10 +27,13 @@
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024)
#define SHARED_MEMORY_SERVER_TEST_C
-#define MAX_DEGREE_OF_FREEDOM 64
+#define MAX_DEGREE_OF_FREEDOM 128
#define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024
+#define MAX_SDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
+#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
+#define MAX_SDF_BODIES 500
struct TmpFloat3
{
@@ -53,6 +56,17 @@ TmpFloat3 CreateTmpFloat3(float x, float y, float z)
return tmp;
}
+enum EnumSdfArgsUpdateFlags
+{
+ SDF_ARGS_FILE_NAME=1,
+};
+
+struct SdfArgs
+{
+ char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
+ int m_useMultiBody;
+};
+
enum EnumUrdfArgsUpdateFlags
{
URDF_ARGS_FILE_NAME=1,
@@ -102,6 +116,7 @@ enum EnumInitPoseFlags
struct InitPoseArgs
{
int m_bodyUniqueId;
+ int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM];
double m_initialStateQ[MAX_DEGREE_OF_FREEDOM];
};
@@ -112,6 +127,31 @@ struct RequestDebugLinesArgs
int m_startingLineIndex;
};
+struct RequestPixelDataArgs
+{
+ float m_viewMatrix[16];
+ float m_projectionMatrix[16];
+ int m_startPixelIndex;
+ int m_pixelWidth;
+ int m_pixelHeight;
+};
+
+enum EnumRequestPixelDataUpdateFlags
+{
+ REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES=1,
+ REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=4,
+ //don't exceed (1<<15), because this enum is shared with EnumRenderer in SharedMemoryPublic.h
+
+};
+
+struct RequestContactDataArgs
+{
+ int m_startingContactPointIndex;
+ int m_objectAIndexFilter;
+ int m_objectBIndexFilter;
+};
+
+
struct SendDebugLinesArgs
{
int m_startingLineIndex;
@@ -119,6 +159,16 @@ struct SendDebugLinesArgs
int m_numRemainingDebugLines;
};
+struct SendPixelDataArgs
+{
+ int m_imageWidth;
+ int m_imageHeight;
+
+ int m_startingPixelIndex;
+ int m_numPixelsCopied;
+ int m_numRemainingPixels;
+};
+
struct PickBodyArgs
{
double m_rayFromWorld[3];
@@ -137,11 +187,14 @@ struct SendDesiredStateArgs
double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
+ int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM];
+
//desired state is only written by the client, read-only access by server is expected
//m_desiredStateQ is indexed by position variables,
//starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables
double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
+
//m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables
double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
@@ -150,7 +203,16 @@ struct SendDesiredStateArgs
//or the maximum applied force/torque for the PD/motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY and CONTROL_MODE_POSITION_VELOCITY_PD mode
//indexed by degree of freedom, 6 dof base, and then dofs for each link
double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM];
-
+
+};
+
+enum EnumSimDesiredStateUpdateFlags
+{
+ SIM_DESIRED_STATE_HAS_Q=1,
+ SIM_DESIRED_STATE_HAS_QDOT=2,
+ SIM_DESIRED_STATE_HAS_KD=4,
+ SIM_DESIRED_STATE_HAS_KP=8,
+ SIM_DESIRED_STATE_HAS_MAX_FORCE=16,
};
@@ -160,6 +222,8 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_GRAVITY=2,
SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4,
SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8,
+ SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16,
+ SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP=32
};
///Controlling a robot involves sending the desired state to its joint motor controllers.
@@ -170,6 +234,8 @@ struct SendPhysicsSimulationParameters
double m_gravityAcceleration[3];
int m_numSimulationSubSteps;
int m_numSolverIterations;
+ bool m_allowRealTimeSimulation;
+ double m_defaultContactERP;
};
struct RequestActualStateArgs
@@ -194,7 +260,11 @@ struct SendActualStateArgs
//measured 6DOF force/torque sensors: force[x,y,z] and torque[x,y,z]
double m_jointReactionForces[6*MAX_DEGREE_OF_FREEDOM];
-
+
+ double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM];
+
+ double m_linkState[7*MAX_NUM_LINKS];
+ double m_linkLocalInertialFrames[7*MAX_NUM_LINKS];
};
enum EnumSensorTypes
@@ -244,6 +314,123 @@ struct CreateBoxShapeArgs
double m_colorRGBA[4];
};
+struct SdfLoadedArgs
+{
+ int m_numBodies;
+ int m_bodyUniqueIds[MAX_SDF_BODIES];
+
+ ///@todo(erwincoumans) load cameras, lights etc
+ //int m_numCameras;
+ //int m_numLights;
+};
+
+
+struct SdfRequestInfoArgs
+{
+ int m_bodyUniqueId;
+};
+
+///flags for b3ApplyExternalTorque and b3ApplyExternalForce
+enum EnumExternalForcePrivateFlags
+{
+// EF_LINK_FRAME=1,
+// EF_WORLD_FRAME=2,
+ EF_TORQUE=4,
+ EF_FORCE=8,
+};
+
+
+
+struct ExternalForceArgs
+{
+ int m_numForcesAndTorques;
+ int m_bodyUniqueIds[MAX_SDF_BODIES];
+ int m_linkIds[MAX_SDF_BODIES];
+ double m_forcesAndTorques[3*MAX_SDF_BODIES];
+ double m_positions[3*MAX_SDF_BODIES];
+ int m_forceFlags[MAX_SDF_BODIES];
+};
+
+enum EnumSdfRequestInfoFlags
+{
+ SDF_REQUEST_INFO_BODY=1,
+ //SDF_REQUEST_INFO_CAMERA=2,
+};
+
+
+struct CalculateInverseDynamicsArgs
+{
+ int m_bodyUniqueId;
+
+ double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
+ double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
+ double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CalculateInverseDynamicsResultArgs
+{
+ int m_bodyUniqueId;
+ int m_dofCount;
+ double m_jointForces[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CalculateJacobianArgs
+{
+ int m_bodyUniqueId;
+ int m_linkIndex;
+ double m_localPosition[3];
+ double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
+ double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
+ double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CalculateJacobianResultArgs
+{
+ int m_dofCount;
+ double m_linearJacobian[3*MAX_DEGREE_OF_FREEDOM];
+ double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM];
+};
+
+enum EnumCalculateInverseKinematicsFlags
+{
+ IK_HAS_TARGET_POSITION=1,
+ IK_HAS_TARGET_ORIENTATION=2,
+ IK_HAS_NULL_SPACE_VELOCITY=4,
+ //IK_HAS_CURRENT_JOINT_POSITIONS=8,//not used yet
+};
+
+struct CalculateInverseKinematicsArgs
+{
+ int m_bodyUniqueId;
+// double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
+ double m_targetPosition[3];
+ double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w
+ int m_endEffectorLinkIndex;
+ double m_lowerLimit[MAX_DEGREE_OF_FREEDOM];
+ double m_upperLimit[MAX_DEGREE_OF_FREEDOM];
+ double m_jointRange[MAX_DEGREE_OF_FREEDOM];
+ double m_restPose[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CalculateInverseKinematicsResultArgs
+{
+ int m_bodyUniqueId;
+ int m_dofCount;
+ double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CreateJointArgs
+{
+ int m_parentBodyIndex;
+ int m_parentJointIndex;
+ int m_childBodyIndex;
+ int m_childJointIndex;
+ double m_parentFrame[7];
+ double m_childFrame[7];
+ double m_jointAxis[3];
+ int m_jointType;
+};
+
struct SharedMemoryCommand
{
int m_type;
@@ -257,6 +444,8 @@ struct SharedMemoryCommand
union
{
struct UrdfArgs m_urdfArguments;
+ struct SdfArgs m_sdfArguments;
+ struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
struct InitPoseArgs m_initPoseArgs;
struct SendPhysicsSimulationParameters m_physSimParamArgs;
struct BulletDataStreamArgs m_dataStreamArguments;
@@ -265,7 +454,14 @@ struct SharedMemoryCommand
struct CreateSensorArgs m_createSensorArguments;
struct CreateBoxShapeArgs m_createBoxShapeArguments;
struct RequestDebugLinesArgs m_requestDebugLinesArguments;
+ struct RequestPixelDataArgs m_requestPixelDataArguments;
struct PickBodyArgs m_pickBodyArguments;
+ struct ExternalForceArgs m_externalForceArguments;
+ struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
+ struct CalculateJacobianArgs m_calculateJacobianArguments;
+ struct CreateJointArgs m_createJointArguments;
+ struct RequestContactDataArgs m_requestContactPointArguments;
+ struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
};
};
@@ -274,6 +470,13 @@ struct RigidBodyCreateArgs
int m_bodyUniqueId;
};
+struct SendContactDataArgs
+{
+ int m_startingContactPointIndex;
+ int m_numContactPointsCopied;
+ int m_numRemainingContactPoints;
+};
+
struct SharedMemoryStatus
{
int m_type;
@@ -284,9 +487,15 @@ struct SharedMemoryStatus
union
{
struct BulletDataStreamArgs m_dataStreamArguments;
+ struct SdfLoadedArgs m_sdfLoadedArgs;
struct SendActualStateArgs m_sendActualStateArgs;
struct SendDebugLinesArgs m_sendDebugLinesArgs;
+ struct SendPixelDataArgs m_sendPixelDataArguments;
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
+ struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
+ struct CalculateJacobianResultArgs m_jacobianResultArgs;
+ struct SendContactDataArgs m_sendContactPointArgs;
+ struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
};
};
diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
new file mode 100644
index 0000000..deb8349
--- /dev/null
+++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
@@ -0,0 +1,105 @@
+
+#include "SharedMemoryInProcessPhysicsC_API.h"
+
+#include "PhysicsClientSharedMemory.h"
+#include"../ExampleBrowser/InProcessExampleBrowser.h"
+
+
+class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedMemory
+{
+ btInProcessExampleBrowserMainThreadInternalData* m_data;
+
+public:
+
+ InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[])
+ {
+ int newargc = argc+2;
+ char** newargv = (char**)malloc(sizeof(void*)*newargc);
+ for (int i=0;i<argc;i++)
+ newargv[i] = argv[i];
+
+ char* t0 = (char*)"--logtostderr";
+ char* t1 = (char*)"--start_demo_name=Physics Server";
+ newargv[argc] = t0;
+ newargv[argc+1] = t1;
+ m_data = btCreateInProcessExampleBrowserMainThread(newargc,newargv);
+ SharedMemoryInterface* shMem = btGetSharedMemoryInterfaceMainThread(m_data);
+
+ setSharedMemoryInterface(shMem);
+ }
+
+ virtual ~InProcessPhysicsClientSharedMemoryMainThread()
+ {
+ setSharedMemoryInterface(0);
+ btShutDownExampleBrowserMainThread(m_data);
+ }
+
+ // return non-null if there is a status, nullptr otherwise
+ virtual const struct SharedMemoryStatus* processServerStatus()
+ {
+ if (btIsExampleBrowserMainThreadTerminated(m_data))
+ {
+ PhysicsClientSharedMemory::disconnectSharedMemory();
+ }
+
+ btUpdateInProcessExampleBrowserMainThread(m_data);
+ return PhysicsClientSharedMemory::processServerStatus();
+
+
+ }
+
+ virtual bool submitClientCommand(const struct SharedMemoryCommand& command)
+ {
+// btUpdateInProcessExampleBrowserMainThread(m_data);
+ return PhysicsClientSharedMemory::submitClientCommand(command);
+ }
+
+};
+
+b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[])
+{
+ InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv);
+ cl->setSharedMemoryKey(SHARED_MEMORY_KEY);
+ cl->connect();
+ return (b3PhysicsClientHandle ) cl;
+}
+
+class InProcessPhysicsClientSharedMemory : public PhysicsClientSharedMemory
+{
+ btInProcessExampleBrowserInternalData* m_data;
+public:
+
+ InProcessPhysicsClientSharedMemory(int argc, char* argv[])
+ {
+ int newargc = argc+2;
+ char** newargv = (char**)malloc(sizeof(void*)*newargc);
+ for (int i=0;i<argc;i++)
+ newargv[i] = argv[i];
+
+ char* t0 = (char*)"--logtostderr";
+ char* t1 = (char*)"--start_demo_name=Physics Server";
+ newargv[argc] = t0;
+ newargv[argc+1] = t1;
+ m_data = btCreateInProcessExampleBrowser(newargc,newargv);
+ SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data);
+
+ setSharedMemoryInterface(shMem);
+ }
+
+ virtual ~InProcessPhysicsClientSharedMemory()
+ {
+ setSharedMemoryInterface(0);
+ btShutDownExampleBrowser(m_data);
+ }
+
+};
+
+b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[])
+{
+
+ InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv);
+ cl->setSharedMemoryKey(SHARED_MEMORY_KEY);
+ cl->connect();
+ return (b3PhysicsClientHandle ) cl;
+}
+
diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h
new file mode 100644
index 0000000..7e68a64
--- /dev/null
+++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h
@@ -0,0 +1,24 @@
+
+#ifndef IN_PROCESS_PHYSICS_C_API_H
+#define IN_PROCESS_PHYSICS_C_API_H
+
+#include "PhysicsClientC_API.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+///think more about naming. The b3ConnectPhysicsLoopback
+b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]);
+
+b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //IN_PROCESS_PHYSICS_C_API_H
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index 789bfbc..bd6e7b4 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -5,7 +5,8 @@
enum EnumSharedMemoryClientCommand
{
- CMD_LOAD_URDF,
+ CMD_LOAD_SDF,
+ CMD_LOAD_URDF,
CMD_SEND_BULLET_DATA_STREAM,
CMD_CREATE_BOX_COLLISION_SHAPE,
// CMD_DELETE_BOX_COLLISION_SHAPE,
@@ -18,24 +19,35 @@ enum EnumSharedMemoryClientCommand
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
CMD_REQUEST_ACTUAL_STATE,
CMD_REQUEST_DEBUG_LINES,
+ CMD_REQUEST_BODY_INFO,
CMD_STEP_FORWARD_SIMULATION,
CMD_RESET_SIMULATION,
CMD_PICK_BODY,
CMD_MOVE_PICKED_BODY,
CMD_REMOVE_PICKING_CONSTRAINT_BODY,
- CMD_MAX_CLIENT_COMMANDS
+ CMD_REQUEST_CAMERA_IMAGE_DATA,
+ CMD_APPLY_EXTERNAL_FORCE,
+ CMD_CALCULATE_INVERSE_DYNAMICS,
+ CMD_CALCULATE_INVERSE_KINEMATICS,
+ CMD_CALCULATE_JACOBIAN,
+ CMD_CREATE_JOINT,
+ CMD_REQUEST_CONTACT_POINT_INFORMATION,
+ CMD_SAVE_WORLD,
+ //don't go beyond this command!
+ CMD_MAX_CLIENT_COMMANDS,
+
};
enum EnumSharedMemoryServerStatus
{
CMD_SHARED_MEMORY_NOT_INITIALIZED=0,
CMD_WAITING_FOR_CLIENT_COMMAND,
-
//CMD_CLIENT_COMMAND_COMPLETED is a generic 'completed' status that doesn't need special handling on the client
CMD_CLIENT_COMMAND_COMPLETED,
//the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED'
CMD_UNKNOWN_COMMAND_FLUSHED,
-
+ CMD_SDF_LOADING_COMPLETED,
+ CMD_SDF_LOADING_FAILED,
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,
@@ -49,7 +61,24 @@ enum EnumSharedMemoryServerStatus
CMD_DEBUG_LINES_OVERFLOW_FAILED,
CMD_DESIRED_STATE_RECEIVED_COMPLETED,
CMD_STEP_FORWARD_SIMULATION_COMPLETED,
- CMD_RESET_SIMULATION_COMPLETED,
+ CMD_RESET_SIMULATION_COMPLETED,
+ CMD_CAMERA_IMAGE_COMPLETED,
+ CMD_CAMERA_IMAGE_FAILED,
+ CMD_BODY_INFO_COMPLETED,
+ CMD_BODY_INFO_FAILED,
+ CMD_INVALID_STATUS,
+ CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED,
+ CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
+ CMD_CALCULATED_JACOBIAN_COMPLETED,
+ CMD_CALCULATED_JACOBIAN_FAILED,
+ CMD_CONTACT_POINT_INFORMATION_COMPLETED,
+ CMD_CONTACT_POINT_INFORMATION_FAILED,
+ CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
+ CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
+ CMD_SAVE_WORLD_COMPLETED,
+ CMD_SAVE_WORLD_FAILED,
+
+ //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@@ -74,6 +103,8 @@ enum
enum JointType {
eRevoluteType = 0,
ePrismaticType = 1,
+ eFixedType = 2,
+ ePoint2PointType = 3,
};
struct b3JointInfo
@@ -85,13 +116,26 @@ struct b3JointInfo
int m_uIndex;
int m_jointIndex;
int m_flags;
+ double m_jointDamping;
+ double m_jointFriction;
+ double m_parentFrame[7]; // position and orientation (quaternion)
+ double m_childFrame[7]; // ^^^
+ double m_jointAxis[3]; // joint axis in parent local frame
+};
+
+struct b3BodyInfo
+{
+ const char* m_baseName;
};
+
+
struct b3JointSensorState
{
double m_jointPosition;
double m_jointVelocity;
double m_jointForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */
+ double m_jointMotorTorque;
};
struct b3DebugLines
@@ -102,6 +146,61 @@ struct b3DebugLines
const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'.
};
+struct b3CameraImageData
+{
+ int m_pixelWidth;
+ int m_pixelHeight;
+ const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes
+ const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats
+ const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints
+};
+
+
+struct b3ContactPointData
+{
+//todo: expose some contact flags, such as telling which fields below are valid
+ int m_contactFlags;
+ int m_bodyUniqueIdA;
+ int m_bodyUniqueIdB;
+ int m_linkIndexA;
+ int m_linkIndexB;
+ double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates
+ double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
+ double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
+ double m_contactDistance;//negative number is penetration, positive is distance.
+
+ double m_normalForce;
+
+//todo: expose the friction forces as well
+// double m_linearFrictionDirection0[3];
+// double m_linearFrictionForce0;
+// double m_linearFrictionDirection1[3];
+// double m_linearFrictionForce1;
+// double m_angularFrictionDirection[3];
+// double m_angularFrictionForce;
+};
+
+
+struct b3ContactInformation
+{
+ int m_numContactPoints;
+ struct b3ContactPointData* m_contactPointData;
+};
+
+///b3LinkState provides extra information such as the Cartesian world coordinates
+///center of mass (COM) of the link, relative to the world reference frame.
+///Orientation is a quaternion x,y,z,w
+///Note: to compute the URDF link frame (which equals the joint frame at joint position 0)
+///use URDF link frame = link COM frame * inertiaFrame.inverse()
+struct b3LinkState
+{
+ double m_worldPosition[3];
+ double m_worldOrientation[4];
+
+ double m_localInertialPosition[3];
+ double m_localInertialOrientation[4];
+};
+
//todo: discuss and decide about control mode and combinations
enum {
// POSITION_CONTROL=0,
@@ -110,4 +209,19 @@ enum {
CONTROL_MODE_POSITION_VELOCITY_PD,
};
+///flags for b3ApplyExternalTorque and b3ApplyExternalForce
+enum EnumExternalForceFlags
+{
+ EF_LINK_FRAME=1,
+ EF_WORLD_FRAME=2,
+};
+
+///flags to pick the renderer for synthetic camera
+enum EnumRenderer
+{
+ ER_TINY_RENDERER=(1<<16),
+ ER_BULLET_HARDWARE_OPENGL=(1<<17),
+ //ER_FIRE_RAYS=(1<<18),
+};
+
#endif//SHARED_MEMORY_PUBLIC_H
diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
new file mode 100644
index 0000000..736ce57
--- /dev/null
+++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
@@ -0,0 +1,731 @@
+/* Copyright (C) 2016 Google
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "TinyRendererVisualShapeConverter.h"
+
+
+
+#include "../Importers/ImportURDFDemo/URDFImporterInterface.h"
+#include "btBulletCollisionCommon.h"
+#include "../Importers/ImportObjDemo/LoadMeshFromObj.h"
+#include "../Importers/ImportSTLDemo/LoadMeshFromSTL.h"
+#include "../Importers/ImportColladaDemo/LoadMeshFromCollada.h"
+#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "Bullet3Common/b3FileUtils.h"
+#include <string>
+#include "../Utils/b3ResourcePath.h"
+#include "../TinyRenderer/TinyRenderer.h"
+#include "../OpenGLWindow/SimpleCamera.h"
+#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
+#include <iostream>
+#include <fstream>
+#include "../Importers/ImportURDFDemo/UrdfParser.h"
+
+
+enum MyFileType
+{
+ MY_FILE_STL=1,
+ MY_FILE_COLLADA=2,
+ MY_FILE_OBJ=3,
+};
+
+struct MyTexture2
+{
+ unsigned char* textureData;
+ int m_width;
+ int m_height;
+};
+
+struct TinyRendererObjectArray
+{
+ btAlignedObjectArray< TinyRenderObjectData*> m_renderObjects;
+};
+
+#define START_WIDTH 640
+#define START_HEIGHT 480
+
+struct TinyRendererVisualShapeConverterInternalData
+{
+
+ btHashMap<btHashPtr,TinyRendererObjectArray*> m_swRenderInstances;
+
+ int m_upAxis;
+ int m_swWidth;
+ int m_swHeight;
+ TGAImage m_rgbColorBuffer;
+ b3AlignedObjectArray<float> m_depthBuffer;
+ b3AlignedObjectArray<int> m_segmentationMaskBuffer;
+
+ SimpleCamera m_camera;
+
+ TinyRendererVisualShapeConverterInternalData()
+ :m_upAxis(2),
+ m_swWidth(START_WIDTH),
+ m_swHeight(START_HEIGHT),
+ m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB)
+ {
+ m_depthBuffer.resize(m_swWidth*m_swHeight);
+ m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1);
+ }
+
+};
+
+
+
+TinyRendererVisualShapeConverter::TinyRendererVisualShapeConverter()
+{
+ m_data = new TinyRendererVisualShapeConverterInternalData();
+
+ float dist = 1.5;
+ float pitch = -80;
+ float yaw = 10;
+ float targetPos[3]={0,0,0};
+ m_data->m_camera.setCameraUpAxis(m_data->m_upAxis);
+ resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+
+
+}
+TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter()
+{
+ delete m_data;
+}
+
+
+
+
+void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut)
+{
+
+
+ GLInstanceGraphicsShape* glmesh = 0;
+
+ btConvexShape* convexColShape = 0;
+
+ switch (visual->m_geometry.m_type)
+ {
+ case URDF_GEOM_CYLINDER:
+ {
+ btAlignedObjectArray<btVector3> vertices;
+
+ //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
+ int numSteps = 32;
+ for (int i = 0; i<numSteps; i++)
+ {
+
+ btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
+ btScalar cylLength = visual->m_geometry.m_cylinderLength;
+
+ btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
+ vertices.push_back(vert);
+ vert[2] = -cylLength / 2.;
+ vertices.push_back(vert);
+ }
+
+ btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+ cylZShape->setMargin(0.001);
+ convexColShape = cylZShape;
+ break;
+ }
+ case URDF_GEOM_BOX:
+ {
+
+ btVector3 extents = visual->m_geometry.m_boxSize;
+
+ btBoxShape* boxShape = new btBoxShape(extents*0.5f);
+ //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
+ convexColShape = boxShape;
+ convexColShape->setMargin(0.001);
+ break;
+ }
+ case URDF_GEOM_SPHERE:
+ {
+ btScalar radius = visual->m_geometry.m_sphereRadius;
+ btSphereShape* sphereShape = new btSphereShape(radius);
+ convexColShape = sphereShape;
+ convexColShape->setMargin(0.001);
+ break;
+
+ break;
+ }
+ case URDF_GEOM_MESH:
+ {
+ if (visual->m_name.length())
+ {
+ //b3Printf("visual->name=%s\n", visual->m_name.c_str());
+ }
+ if (1)//visual->m_geometry)
+ {
+ if (visual->m_geometry.m_meshFileName.length())
+ {
+ const char* filename = visual->m_geometry.m_meshFileName.c_str();
+ //b3Printf("mesh->filename=%s\n", filename);
+ char fullPath[1024];
+ int fileType = 0;
+
+ char tmpPathPrefix[1024];
+ std::string xml_string;
+ int maxPathLen = 1024;
+ b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
+
+ char visualPathPrefix[1024];
+ sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
+
+
+ sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
+ b3FileUtils::toLower(fullPath);
+ if (strstr(fullPath, ".dae"))
+ {
+ fileType = MY_FILE_COLLADA;
+ }
+ if (strstr(fullPath, ".stl"))
+ {
+ fileType = MY_FILE_STL;
+ }
+ if (strstr(fullPath,".obj"))
+ {
+ fileType = MY_FILE_OBJ;
+ }
+
+
+ sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
+ FILE* f = fopen(fullPath, "rb");
+ if (f)
+ {
+ fclose(f);
+
+
+
+ switch (fileType)
+ {
+ case MY_FILE_OBJ:
+ {
+ //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+ b3ImportMeshData meshData;
+ if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
+ {
+
+ if (meshData.m_textureImage)
+ {
+ MyTexture2 texData;
+ texData.m_width = meshData.m_textureWidth;
+ texData.m_height = meshData.m_textureHeight;
+ texData.textureData = meshData.m_textureImage;
+ texturesOut.push_back(texData);
+ }
+ glmesh = meshData.m_gfxShape;
+ }
+
+
+ break;
+ }
+
+ case MY_FILE_STL:
+ {
+ glmesh = LoadMeshFromSTL(fullPath);
+ break;
+ }
+ case MY_FILE_COLLADA:
+ {
+
+ btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
+ btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
+ btTransform upAxisTrans; upAxisTrans.setIdentity();
+ float unitMeterScaling = 1;
+ int upAxis = 2;
+
+ LoadMeshFromCollada(fullPath,
+ visualShapes,
+ visualShapeInstances,
+ upAxisTrans,
+ unitMeterScaling,
+ upAxis);
+
+ glmesh = new GLInstanceGraphicsShape;
+ // int index = 0;
+ glmesh->m_indices = new b3AlignedObjectArray<int>();
+ glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+ for (int i = 0; i<visualShapeInstances.size(); i++)
+ {
+ ColladaGraphicsInstance* instance = &visualShapeInstances[i];
+ GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
+
+ b3AlignedObjectArray<GLInstanceVertex> verts;
+ verts.resize(gfxShape->m_vertices->size());
+
+ int baseIndex = glmesh->m_vertices->size();
+
+ for (int i = 0; i<gfxShape->m_vertices->size(); i++)
+ {
+ verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
+ verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
+ verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
+ verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
+ verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
+ verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
+ verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
+ verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
+ verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
+
+ }
+
+ int curNumIndices = glmesh->m_indices->size();
+ int additionalIndices = gfxShape->m_indices->size();
+ glmesh->m_indices->resize(curNumIndices + additionalIndices);
+ for (int k = 0; k<additionalIndices; k++)
+ {
+ glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
+ }
+
+ //compensate upAxisTrans and unitMeterScaling here
+ btMatrix4x4 upAxisMat;
+ upAxisMat.setIdentity();
+// upAxisMat.setPureRotation(upAxisTrans.getRotation());
+ btMatrix4x4 unitMeterScalingMat;
+ unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
+ btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
+ //btMatrix4x4 worldMat = instance->m_worldTransform;
+ int curNumVertices = glmesh->m_vertices->size();
+ int additionalVertices = verts.size();
+ glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
+
+ for (int v = 0; v<verts.size(); v++)
+ {
+ btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
+ pos = worldMat*pos;
+ verts[v].xyzw[0] = float(pos[0]);
+ verts[v].xyzw[1] = float(pos[1]);
+ verts[v].xyzw[2] = float(pos[2]);
+ glmesh->m_vertices->push_back(verts[v]);
+ }
+ }
+ glmesh->m_numIndices = glmesh->m_indices->size();
+ glmesh->m_numvertices = glmesh->m_vertices->size();
+ //glmesh = LoadMeshFromCollada(fullPath);
+
+ break;
+ }
+ default:
+ {
+ b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath);
+ btAssert(0);
+ }
+ }
+
+
+ if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0))
+ {
+ //apply the geometry scaling
+ for (int i=0;i<glmesh->m_vertices->size();i++)
+ {
+ glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
+ glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
+ glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
+ }
+
+ }
+ else
+ {
+ b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
+ }
+
+ }
+ else
+ {
+ b3Warning("mesh geometry not found %s\n", fullPath);
+ }
+
+
+ }
+ }
+
+
+ break;
+ }
+ default:
+ {
+ b3Warning("Error: unknown visual geometry type\n");
+ }
+ }
+
+ //if we have a convex, tesselate into localVertices/localIndices
+ if ((glmesh==0) && convexColShape)
+ {
+ btShapeHull* hull = new btShapeHull(convexColShape);
+ hull->buildHull(0.0);
+ {
+ // int strideInBytes = 9*sizeof(float);
+ int numVertices = hull->numVertices();
+ int numIndices = hull->numIndices();
+
+
+ glmesh = new GLInstanceGraphicsShape;
+ // int index = 0;
+ glmesh->m_indices = new b3AlignedObjectArray<int>();
+ glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+
+ for (int i = 0; i < numVertices; i++)
+ {
+ GLInstanceVertex vtx;
+ btVector3 pos = hull->getVertexPointer()[i];
+ vtx.xyzw[0] = pos.x();
+ vtx.xyzw[1] = pos.y();
+ vtx.xyzw[2] = pos.z();
+ vtx.xyzw[3] = 1.f;
+ pos.normalize();
+ vtx.normal[0] = pos.x();
+ vtx.normal[1] = pos.y();
+ vtx.normal[2] = pos.z();
+ vtx.uv[0] = 0.5f;
+ vtx.uv[1] = 0.5f;
+ glmesh->m_vertices->push_back(vtx);
+ }
+
+ btAlignedObjectArray<int> indices;
+ for (int i = 0; i < numIndices; i++)
+ {
+ glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
+ }
+
+ glmesh->m_numvertices = glmesh->m_vertices->size();
+ glmesh->m_numIndices = glmesh->m_indices->size();
+ }
+ delete hull;
+ delete convexColShape;
+ convexColShape = 0;
+
+ }
+
+ if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
+ {
+
+ int baseIndex = verticesOut.size();
+
+
+
+ for (int i = 0; i < glmesh->m_indices->size(); i++)
+ {
+ indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
+ }
+
+ for (int i = 0; i < glmesh->m_vertices->size(); i++)
+ {
+ GLInstanceVertex& v = glmesh->m_vertices->at(i);
+ btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
+ btVector3 vt = visualTransform*vert;
+ v.xyzw[0] = vt[0];
+ v.xyzw[1] = vt[1];
+ v.xyzw[2] = vt[2];
+ btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
+ triNormal = visualTransform.getBasis()*triNormal;
+ v.normal[0] = triNormal[0];
+ v.normal[1] = triNormal[1];
+ v.normal[2] = triNormal[2];
+ verticesOut.push_back(v);
+ }
+ }
+ delete glmesh;
+
+}
+
+
+
+void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int objectIndex)
+{
+
+
+ UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
+ if (linkPtr)
+ {
+
+ const UrdfLink* link = *linkPtr;
+
+ for (int v = 0; v < link->m_visualArray.size();v++)
+ {
+ btAlignedObjectArray<MyTexture2> textures;
+ btAlignedObjectArray<GLInstanceVertex> vertices;
+ btAlignedObjectArray<int> indices;
+ btTransform startTrans; startTrans.setIdentity();
+ int graphicsIndex = -1;
+
+ const UrdfVisual& vis = link->m_visualArray[v];
+ btTransform childTrans = vis.m_linkLocalFrame;
+ btHashString matName(vis.m_materialName.c_str());
+ UrdfMaterial *const * matPtr = model.m_materials[matName];
+
+ float rgbaColor[4] = {1,1,1,1};
+
+ if (matPtr)
+ {
+ UrdfMaterial *const mat = *matPtr;
+ for (int i=0;i<4;i++)
+ rgbaColor[i] = mat->m_rgbaColor[i];
+ //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
+ //m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
+ }
+
+ TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj];
+ if (visualsPtr==0)
+ {
+ m_data->m_swRenderInstances.insert(colObj,new TinyRendererObjectArray);
+ }
+ visualsPtr = m_data->m_swRenderInstances[colObj];
+ btAssert(visualsPtr);
+ TinyRendererObjectArray* visuals = *visualsPtr;
+
+ convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
+
+ if (vertices.size() && indices.size())
+ {
+ TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_segmentationMaskBuffer, objectIndex);
+ unsigned char* textureImage=0;
+ int textureWidth=0;
+ int textureHeight=0;
+ if (textures.size())
+ {
+ textureImage = textures[0].textureData;
+ textureWidth = textures[0].m_width;
+ textureHeight = textures[0].m_height;
+ }
+
+ tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor,
+ textureImage,textureWidth,textureHeight);
+ visuals->m_renderObjects.push_back(tinyObj);
+ }
+ for (int i=0;i<textures.size();i++)
+ {
+ free(textures[i].textureData);
+ }
+ }
+ }
+}
+
+void TinyRendererVisualShapeConverter::setUpAxis(int axis)
+{
+ m_data->m_upAxis = axis;
+ m_data->m_camera.setCameraUpAxis(axis);
+ m_data->m_camera.update();
+}
+void TinyRendererVisualShapeConverter::resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
+{
+ m_data->m_camera.setCameraDistance(camDist);
+ m_data->m_camera.setCameraPitch(pitch);
+ m_data->m_camera.setCameraYaw(yaw);
+ m_data->m_camera.setCameraTargetPosition(camPosX,camPosY,camPosZ);
+ m_data->m_camera.setAspectRatio((float)m_data->m_swWidth/(float)m_data->m_swHeight);
+ m_data->m_camera.update();
+
+}
+
+void TinyRendererVisualShapeConverter::clearBuffers(TGAColor& clearColor)
+{
+ for(int y=0;y<m_data->m_swHeight;++y)
+ {
+ for(int x=0;x<m_data->m_swWidth;++x)
+ {
+ m_data->m_rgbColorBuffer.set(x,y,clearColor);
+ m_data->m_depthBuffer[x+y*m_data->m_swWidth] = -1e30f;
+ m_data->m_segmentationMaskBuffer[x+y*m_data->m_swWidth] = -1;
+ }
+ }
+
+}
+
+void TinyRendererVisualShapeConverter::render()
+{
+
+ ATTRIBUTE_ALIGNED16(float viewMat[16]);
+ ATTRIBUTE_ALIGNED16(float projMat[16]);
+
+ m_data->m_camera.getCameraProjectionMatrix(projMat);
+ m_data->m_camera.getCameraViewMatrix(viewMat);
+
+ render(viewMat,projMat);
+}
+
+void TinyRendererVisualShapeConverter::render(const float viewMat[16], const float projMat[16])
+{
+ //clear the color buffer
+ TGAColor clearColor;
+ clearColor.bgra[0] = 255;
+ clearColor.bgra[1] = 255;
+ clearColor.bgra[2] = 255;
+ clearColor.bgra[3] = 255;
+
+ clearBuffers(clearColor);
+
+
+ ATTRIBUTE_ALIGNED16(btScalar modelMat[16]);
+
+
+ btVector3 lightDirWorld(-5,200,-40);
+ switch (m_data->m_upAxis)
+ {
+ case 1:
+ lightDirWorld = btVector3(-50.f,100,30);
+ break;
+ case 2:
+ lightDirWorld = btVector3(-50.f,30,100);
+ break;
+ default:{}
+ };
+
+ lightDirWorld.normalize();
+
+ // printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
+ for (int i=0;i<m_data->m_swRenderInstances.size();i++)
+ {
+ TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i);
+ if (0==visualArrayPtr)
+ continue;//can this ever happen?
+ TinyRendererObjectArray* visualArray = *visualArrayPtr;
+
+ btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(i);
+
+
+ const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
+
+ for (int v=0;v<visualArray->m_renderObjects.size();v++)
+ {
+
+ TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v];
+
+
+ //sync the object transform
+ const btTransform& tr = colObj->getWorldTransform();
+ tr.getOpenGLMatrix(modelMat);
+
+ for (int i=0;i<4;i++)
+ {
+ for (int j=0;j<4;j++)
+ {
+
+ renderObj->m_projectionMatrix[i][j] = projMat[i+4*j];
+ renderObj->m_modelMatrix[i][j] = modelMat[i+4*j];
+ renderObj->m_viewMatrix[i][j] = viewMat[i+4*j];
+ renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling();
+ renderObj->m_lightDirWorld = lightDirWorld;
+ }
+ }
+ TinyRenderer::renderObject(*renderObj);
+ }
+ }
+ //printf("write tga \n");
+ //m_data->m_rgbColorBuffer.write_tga_file("camera.tga");
+// printf("flipped!\n");
+ m_data->m_rgbColorBuffer.flip_vertically();
+
+ //flip z-buffer and segmentation Buffer
+ {
+ int half = m_data->m_swHeight>>1;
+ for (int j=0; j<half; j++)
+ {
+ unsigned long l1 = j*m_data->m_swWidth;
+ unsigned long l2 = (m_data->m_swHeight-1-j)*m_data->m_swWidth;
+ for (int i=0;i<m_data->m_swWidth;i++)
+ {
+ btSwap(m_data->m_depthBuffer[l1+i],m_data->m_depthBuffer[l2+i]);
+ btSwap(m_data->m_segmentationMaskBuffer[l1+i],m_data->m_segmentationMaskBuffer[l2+i]);
+ }
+ }
+ }
+}
+
+void TinyRendererVisualShapeConverter::getWidthAndHeight(int& width, int& height)
+{
+ width = m_data->m_swWidth;
+ height = m_data->m_swHeight;
+}
+
+
+void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height)
+{
+ m_data->m_swWidth = width;
+ m_data->m_swHeight = height;
+
+ m_data->m_depthBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
+ m_data->m_segmentationMaskBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
+ m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB);
+
+
+}
+
+void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
+ float* depthBuffer, int depthBufferSizeInPixels,
+ int* segmentationMaskBuffer, int segmentationMaskSizeInPixels,
+ int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)
+{
+ int w = m_data->m_rgbColorBuffer.get_width();
+ int h = m_data->m_rgbColorBuffer.get_height();
+
+ if (numPixelsCopied)
+ *numPixelsCopied = 0;
+
+ if (widthPtr)
+ *widthPtr = w;
+
+ if (heightPtr)
+ *heightPtr = h;
+
+ int numTotalPixels = w*h;
+ int numRemainingPixels = numTotalPixels - startPixelIndex;
+ int numBytesPerPixel = 4;//RGBA
+ int numRequestedPixels = btMin(rgbaBufferSizeInPixels,numRemainingPixels);
+ if (numRequestedPixels)
+ {
+ for (int i=0;i<numRequestedPixels;i++)
+ {
+ if (depthBuffer)
+ {
+ depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex];
+ }
+ if (segmentationMaskBuffer)
+ {
+ segmentationMaskBuffer[i] = m_data->m_segmentationMaskBuffer[i+startPixelIndex];
+ }
+
+ if (pixelsRGBA)
+ {
+ pixelsRGBA[i*numBytesPerPixel] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0];
+ pixelsRGBA[i*numBytesPerPixel+1] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+1];
+ pixelsRGBA[i*numBytesPerPixel+2] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+2];
+ pixelsRGBA[i*numBytesPerPixel+3] = 255;
+
+ }
+ }
+
+ if (numPixelsCopied)
+ *numPixelsCopied = numRequestedPixels;
+
+ }
+}
+
+
+void TinyRendererVisualShapeConverter::resetAll()
+{
+ for (int i=0;i<m_data->m_swRenderInstances.size();i++)
+ {
+ TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i);
+ if (ptrptr && *ptrptr)
+ {
+ TinyRendererObjectArray* ptr = *ptrptr;
+ delete ptr;
+ }
+ }
+
+ m_data->m_swRenderInstances.clear();
+}
+
diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h
new file mode 100644
index 0000000..0602fe2
--- /dev/null
+++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h
@@ -0,0 +1,39 @@
+#ifndef TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H
+#define TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H
+
+
+#include "../Importers/ImportURDFDemo/LinkVisualShapesConverter.h"
+
+struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
+{
+
+ struct TinyRendererVisualShapeConverterInternalData* m_data;
+
+ TinyRendererVisualShapeConverter();
+
+ virtual ~TinyRendererVisualShapeConverter();
+
+ virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex);
+
+ void setUpAxis(int axis);
+
+ void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ);
+
+ void clearBuffers(struct TGAColor& clearColor);
+
+ void resetAll();
+
+ void getWidthAndHeight(int& width, int& height);
+ void setWidthAndHeight(int width, int height);
+
+ void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
+
+ void render();
+ void render(const float viewMat[16], const float projMat[16]);
+
+};
+
+
+
+
+#endif //TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H
diff --git a/examples/SharedMemory/main.cpp b/examples/SharedMemory/main.cpp
index 6aad5da..fa83a48 100644
--- a/examples/SharedMemory/main.cpp
+++ b/examples/SharedMemory/main.cpp
@@ -78,6 +78,9 @@ int main(int argc, char* argv[])
example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options);
}else
{
+// options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING;
+ // options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG;
+
example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options);
}
diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua
index 5882257..2215cfb 100644
--- a/examples/SharedMemory/premake4.lua
+++ b/examples/SharedMemory/premake4.lua
@@ -10,12 +10,15 @@ end
includedirs {".","../../src", "../ThirdPartyLibs",}
links {
- "Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath"
+ "Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "BussIK"
}
language "C++"
-files {
+myfiles =
+{
+ "IKTrajectoryHelper.cpp",
+ "IKTrajectoryHelper.h",
"PhysicsClient.cpp",
"PhysicsClientSharedMemory.cpp",
"PhysicsClientExample.cpp",
@@ -24,11 +27,13 @@ files {
"PhysicsServerSharedMemory.h",
"PhysicsServer.cpp",
"PhysicsServer.h",
- "main.cpp",
"PhysicsClientC_API.cpp",
+ "SharedMemoryCommands.h",
+ "SharedMemoryPublic.h",
"PhysicsServer.cpp",
"PosixSharedMemory.cpp",
"Win32SharedMemory.cpp",
+ "InProcessMemory.cpp",
"PhysicsDirect.cpp",
"PhysicsDirect.h",
"PhysicsDirectC_API.cpp",
@@ -39,6 +44,15 @@ files {
"PhysicsLoopBackC_API.h",
"PhysicsServerCommandProcessor.cpp",
"PhysicsServerCommandProcessor.h",
+ "TinyRendererVisualShapeConverter.cpp",
+ "TinyRendererVisualShapeConverter.h",
+ "../TinyRenderer/geometry.cpp",
+ "../TinyRenderer/model.cpp",
+ "../TinyRenderer/tgaimage.cpp",
+ "../TinyRenderer/our_gl.cpp",
+ "../TinyRenderer/TinyRenderer.cpp",
+ "../OpenGLWindow/SimpleCamera.cpp",
+ "../OpenGLWindow/SimpleCamera.h",
"../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h",
"../Importers/ImportURDFDemo/MultiBodyCreationInterface.h",
"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
@@ -46,8 +60,6 @@ files {
"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
"../Importers/ImportURDFDemo/BulletUrdfImporter.h",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
- "../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
- "../Importers/ImportURDFDemo/BulletUrdfImporter.h",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/UrdfParser.h",
@@ -66,26 +78,214 @@ files {
"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
"../Importers/ImportColladaDemo/ColladaGraphicsInstance.h",
"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
- "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp",
- "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/model.cpp",
- "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp",
- "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/joint.cpp",
- "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h",
- "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h",
- "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h",
- "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h",
- "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h",
- "../ThirdPartyLibs/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h",
- "../ThirdPartyLibs/tinyxml/tinystr.cpp",
- "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
- "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
- "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
- "../ThirdPartyLibs/urdf/boost_replacement/lexical_cast.h",
- "../ThirdPartyLibs/urdf/boost_replacement/shared_ptr.h",
- "../ThirdPartyLibs/urdf/boost_replacement/printf_console.cpp",
- "../ThirdPartyLibs/urdf/boost_replacement/printf_console.h",
- "../ThirdPartyLibs/urdf/boost_replacement/string_split.cpp",
- "../ThirdPartyLibs/urdf/boost_replacement/string_split.h",
+ "../ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../ThirdPartyLibs/stb_image/stb_image.cpp",
+}
+files {
+ myfiles,
+ "main.cpp",
}
+
+files {
+ "../MultiThreading/b3ThreadSupportInterface.cpp",
+ "../MultiThreading/b3ThreadSupportInterface.h"
+ }
+ if os.is("Windows") then
+
+ files {
+ "../MultiThreading/b3Win32ThreadSupport.cpp",
+ "../MultiThreading/b3Win32ThreadSupport.h"
+ }
+ --links {"winmm"}
+ --defines {"__WINDOWS_MM__", "WIN32"}
+ end
+
+ if os.is("Linux") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+ end
+
+ if os.is("MacOSX") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+ --links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+ --defines {"__MACOSX_CORE__"}
+ end
+
+
+project "App_SharedMemoryPhysics_GUI"
+
+if _OPTIONS["ios"] then
+ kind "WindowedApp"
+else
+ kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src", "../ThirdPartyLibs"}
+
+links {
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common","BussIK"
+}
+ initOpenGL()
+ initGlew()
+
+language "C++"
+
+files {
+ myfiles,
+ "../StandaloneMain/main_opengl_single_example.cpp",
+ "../ExampleBrowser/OpenGLGuiHelper.cpp",
+ "../ExampleBrowser/GL_ShapeDrawer.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+}
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+end
+
+
+files {
+ "../MultiThreading/b3ThreadSupportInterface.cpp",
+ "../MultiThreading/b3ThreadSupportInterface.h"
+ }
+if os.is("Windows") then
+
+ files {
+ "../MultiThreading/b3Win32ThreadSupport.cpp",
+ "../MultiThreading/b3Win32ThreadSupport.h"
+ }
+ --links {"winmm"}
+ --defines {"__WINDOWS_MM__", "WIN32"}
+end
+
+if os.is("Linux") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+end
+
+if os.is("MacOSX") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+ --links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+ --defines {"__MACOSX_CORE__"}
+end
+
+if os.is("Windows") then
+ project "App_SharedMemoryPhysics_VR"
+ --for now, only enable VR under Windows, until compilation issues are resolved on Mac/Linux
+ defines {"B3_USE_STANDALONE_EXAMPLE","BT_ENABLE_VR"}
+
+ if _OPTIONS["ios"] then
+ kind "WindowedApp"
+ else
+ kind "ConsoleApp"
+ end
+
+ includedirs {
+ ".","../../src", "../ThirdPartyLibs",
+ "../ThirdPartyLibs/openvr/headers",
+ "../ThirdPartyLibs/openvr/samples/shared"
+ }
+
+ links {
+ "BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common", "BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api","BussIK"
+ }
+
+
+ language "C++"
+
+
+ initOpenGL()
+ initGlew()
+
+
+ files
+ {
+ myfiles,
+ "../StandaloneMain/hellovr_opengl_main.cpp",
+ "../ExampleBrowser/OpenGLGuiHelper.cpp",
+ "../ExampleBrowser/GL_ShapeDrawer.cpp",
+ "../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+ "../RenderingExamples/TinyVRGui.cpp",
+ "../RenderingExamples/TimeSeriesCanvas.cpp",
+ "../RenderingExamples/TimeSeriesFontData.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/lodepng.h",
+ "../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/Matrices.h",
+ "../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp",
+ "../ThirdPartyLibs/openvr/samples/shared/pathtools.h",
+ "../ThirdPartyLibs/openvr/samples/shared/Vectors.h",
+ }
+ if os.is("Windows") then
+ libdirs {"../ThirdPartyLibs/openvr/lib/win32"}
+ end
+
+ if os.is("Linux") then initX11() end
+
+ if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+ end
+
+
+ files {
+ "../MultiThreading/b3ThreadSupportInterface.cpp",
+ "../MultiThreading/b3ThreadSupportInterface.h"
+ }
+ if os.is("Windows") then
+
+ files {
+ "../MultiThreading/b3Win32ThreadSupport.cpp",
+ "../MultiThreading/b3Win32ThreadSupport.h"
+ }
+ --links {"winmm"}
+ --defines {"__WINDOWS_MM__", "WIN32"}
+ end
+
+ if os.is("Linux") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+ end
+
+ if os.is("MacOSX") then
+ files {
+ "../MultiThreading/b3PosixThreadSupport.cpp",
+ "../MultiThreading/b3PosixThreadSupport.h"
+ }
+
+ links {"pthread"}
+ --links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+ --defines {"__MACOSX_CORE__"}
+ end
+
+
+end
\ No newline at end of file
diff --git a/examples/SimpleOpenGL3/main.cpp b/examples/SimpleOpenGL3/main.cpp
index 68dc003..fe63777 100644
--- a/examples/SimpleOpenGL3/main.cpp
+++ b/examples/SimpleOpenGL3/main.cpp
@@ -1,57 +1,163 @@
+
+
+
#include "OpenGLWindow/SimpleOpenGL3App.h"
-#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include "assert.h"
#include <stdio.h>
-#include "OpenGLWindow/OpenGLInclude.h"
char* gVideoFileName = 0;
char* gPngFileName = 0;
+static b3WheelCallback sOldWheelCB = 0;
+static b3ResizeCallback sOldResizeCB = 0;
+static b3MouseMoveCallback sOldMouseMoveCB = 0;
+static b3MouseButtonCallback sOldMouseButtonCB = 0;
+static b3KeyboardCallback sOldKeyboardCB = 0;
+//static b3RenderCallback sOldRenderCB = 0;
+
+float gWidth = 1024;
+float gHeight = 768;
+
+void MyWheelCallback(float deltax, float deltay)
+{
+ if (sOldWheelCB)
+ sOldWheelCB(deltax,deltay);
+}
+void MyResizeCallback( float width, float height)
+{
+ gWidth = width;
+ gHeight = height;
+
+ if (sOldResizeCB)
+ sOldResizeCB(width,height);
+}
+void MyMouseMoveCallback( float x, float y)
+{
+ printf("Mouse Move: %f, %f\n", x,y);
+
+ if (sOldMouseMoveCB)
+ sOldMouseMoveCB(x,y);
+}
+void MyMouseButtonCallback(int button, int state, float x, float y)
+{
+ if (sOldMouseButtonCB)
+ sOldMouseButtonCB(button,state,x,y);
+}
+
+
+void MyKeyboardCallback(int keycode, int state)
+{
+ //keycodes are in examples/CommonInterfaces/CommonWindowInterface.h
+ //for example B3G_ESCAPE for escape key
+ //state == 1 for pressed, state == 0 for released.
+ // use app->m_window->isModifiedPressed(...) to check for shift, escape and alt keys
+ printf("MyKeyboardCallback received key:%c in state %d\n",keycode,state);
+ if (sOldKeyboardCB)
+ sOldKeyboardCB(keycode,state);
+}
+
+
int main(int argc, char* argv[])
{
- b3CommandLineArgs myArgs(argc,argv);
+ {
+ b3CommandLineArgs myArgs(argc, argv);
- SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",1024,768,true);
- app->m_instancingRenderer->getActiveCamera()->setCameraDistance(13);
- app->m_instancingRenderer->getActiveCamera()->setCameraPitch(0);
- app->m_instancingRenderer->getActiveCamera()->setCameraTargetPosition(0,0,0);
+ SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App", gWidth, gHeight, true);
- assert(glGetError()==GL_NO_ERROR);
+ app->m_instancingRenderer->getActiveCamera()->setCameraDistance(13);
+ app->m_instancingRenderer->getActiveCamera()->setCameraPitch(0);
+ app->m_instancingRenderer->getActiveCamera()->setCameraTargetPosition(0, 0, 0);
+ sOldKeyboardCB = app->m_window->getKeyboardCallback();
+ app->m_window->setKeyboardCallback(MyKeyboardCallback);
+ sOldMouseMoveCB = app->m_window->getMouseMoveCallback();
+ app->m_window->setMouseMoveCallback(MyMouseMoveCallback);
+ sOldMouseButtonCB = app->m_window->getMouseButtonCallback();
+ app->m_window->setMouseButtonCallback(MyMouseButtonCallback);
+ sOldWheelCB = app->m_window->getWheelCallback();
+ app->m_window->setWheelCallback(MyWheelCallback);
+ sOldResizeCB = app->m_window->getResizeCallback();
+ app->m_window->setResizeCallback(MyResizeCallback);
- myArgs.GetCmdLineArgument("mp4_file",gVideoFileName);
- if (gVideoFileName)
- app->dumpFramesToVideo(gVideoFileName);
- myArgs.GetCmdLineArgument("png_file",gPngFileName);
- char fileName[1024];
+ myArgs.GetCmdLineArgument("mp4_file", gVideoFileName);
+ if (gVideoFileName)
+ app->dumpFramesToVideo(gVideoFileName);
- do
- {
- static int frameCount = 0;
- frameCount++;
- if (gPngFileName)
- {
- printf("gPngFileName=%s\n",gPngFileName);
+ myArgs.GetCmdLineArgument("png_file", gPngFileName);
+ char fileName[1024];
+
+ int textureWidth = 128;
+ int textureHeight = 128;
+
+ unsigned char* image = new unsigned char[textureWidth*textureHeight * 4];
+
+
+ int textureHandle = app->m_renderer->registerTexture(image, textureWidth, textureHeight);
+
+ int cubeIndex = app->registerCubeShape(1, 1, 1);
+
+ b3Vector3 pos = b3MakeVector3(0, 0, 0);
+ b3Quaternion orn(0, 0, 0, 1);
+ b3Vector3 color = b3MakeVector3(1, 0, 0);
+ b3Vector3 scaling = b3MakeVector3 (1, 1, 1);
+ app->m_renderer->registerGraphicsInstance(cubeIndex, pos, orn, color, scaling);
+ app->m_renderer->writeTransforms();
+
+ do
+ {
+ static int frameCount = 0;
+ frameCount++;
+ if (gPngFileName)
+ {
+ printf("gPngFileName=%s\n", gPngFileName);
+
+ sprintf(fileName, "%s%d.png", gPngFileName, frameCount++);
+ app->dumpNextFrameToPng(fileName);
+ }
+
+
+
+ //update the texels of the texture using a simple pattern, animated using frame index
+ for (int y = 0; y < textureHeight; ++y)
+ {
+ const int t = (y + frameCount) >> 4;
+ unsigned char* pi = image + y*textureWidth * 3;
+ for (int x = 0; x < textureWidth; ++x)
+ {
+ const int s = x >> 4;
+ const unsigned char b = 180;
+ unsigned char c = b + ((s + (t & 1)) & 1)*(255 - b);
+ pi[0] = pi[1] = pi[2] = pi[3] = c; pi += 3;
+ }
+ }
+
+ app->m_renderer->activateTexture(textureHandle);
+ app->m_renderer->updateTexture(textureHandle, image);
+
+ float color[4] = { 255, 1, 1, 1 };
+ app->m_primRenderer->drawTexturedRect(100, 200, gWidth / 2 - 50, gHeight / 2 - 50, color, 0, 0, 1, 1, true);
+
+
+ app->m_instancingRenderer->init();
+ app->m_instancingRenderer->updateCamera();
- sprintf(fileName,"%s%d.png",gPngFileName,frameCount++);
- app->dumpNextFrameToPng(fileName);
- }
+ app->m_renderer->renderScene();
+ app->drawGrid();
+ char bla[1024];
+ sprintf(bla, "Simple test frame %d", frameCount);
- assert(glGetError()==GL_NO_ERROR);
- app->m_instancingRenderer->init();
- app->m_instancingRenderer->updateCamera();
+ app->drawText(bla, 10, 10);
+ app->swapBuffer();
+ } while (!app->m_window->requestedExit());
- app->drawGrid();
- char bla[1024];
- sprintf(bla,"Simple test frame %d", frameCount);
- app->drawText(bla,10,10);
- app->swapBuffer();
- } while (!app->m_window->requestedExit());
+ delete app;
- delete app;
+ delete[] image;
+ }
return 0;
}
diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp
new file mode 100644
index 0000000..448ac09
--- /dev/null
+++ b/examples/StandaloneMain/hellovr_opengl_main.cpp
@@ -0,0 +1,2223 @@
+#ifdef BT_ENABLE_VR
+//#define BT_USE_CUSTOM_PROFILER
+//========= Copyright Valve Corporation ============//
+
+#include "../OpenGLWindow/SimpleOpenGL3App.h"
+#include "../OpenGLWindow/OpenGLInclude.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3Transform.h"
+
+
+#include "../ExampleBrowser/OpenGLGuiHelper.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+
+#include "LinearMath/btIDebugDraw.h"
+int gSharedMemoryKey = -1;
+int gDebugDrawFlags = 0;
+
+//how can you try typing on a keyboard, without seeing it?
+//it is pretty funny, to see the desktop in VR!
+
+
+#include <stdio.h>
+#include <string>
+#include <cstdlib>
+
+#include <openvr.h>
+
+#include "lodepng.h"
+#include "Matrices.h"
+#include "pathtools.h"
+
+CommonExampleInterface* sExample;
+
+int sPrevPacketNum=0;
+OpenGLGuiHelper* sGuiPtr = 0;
+
+
+static vr::VRControllerState_t sPrevStates[vr::k_unMaxTrackedDeviceCount] = { 0 };
+
+
+#if defined(POSIX)
+#include "unistd.h"
+#endif
+#ifdef _WIN32
+#include <Windows.h>
+#endif
+
+void ThreadSleep( unsigned long nMilliseconds )
+{
+#if defined(_WIN32)
+ ::Sleep( nMilliseconds );
+#elif defined(POSIX)
+ usleep( nMilliseconds * 1000 );
+#endif
+}
+
+
+class CGLRenderModel
+{
+public:
+ CGLRenderModel( const std::string & sRenderModelName );
+ ~CGLRenderModel();
+
+ bool BInit( const vr::RenderModel_t & vrModel, const vr::RenderModel_TextureMap_t & vrDiffuseTexture );
+ void Cleanup();
+ void Draw();
+ const std::string & GetName() const { return m_sModelName; }
+
+private:
+ GLuint m_glVertBuffer;
+ GLuint m_glIndexBuffer;
+ GLuint m_glVertArray;
+ GLuint m_glTexture;
+ GLsizei m_unVertexCount;
+ std::string m_sModelName;
+};
+
+static bool g_bPrintf = true;
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//------------------------------------------------------------------------------
+class CMainApplication
+{
+public:
+ CMainApplication( int argc, char *argv[] );
+ virtual ~CMainApplication();
+
+ bool BInit();
+ bool BInitGL();
+ bool BInitCompositor();
+ void getControllerTransform(int unDevice, b3Transform& tr);
+ void SetupRenderModels();
+
+ void Shutdown();
+
+ void RunMainLoop();
+ bool HandleInput();
+ void ProcessVREvent( const vr::VREvent_t & event );
+ void RenderFrame();
+
+ bool SetupTexturemaps();
+
+ void SetupScene();
+ void AddCubeToScene( Matrix4 mat, std::vector<float> &vertdata );
+ void AddCubeVertex( float fl0, float fl1, float fl2, float fl3, float fl4, std::vector<float> &vertdata );
+
+ void DrawControllers();
+
+ bool SetupStereoRenderTargets();
+ void SetupDistortion();
+ void SetupCameras();
+
+ void RenderStereoTargets();
+ void RenderDistortion();
+ void RenderScene( vr::Hmd_Eye nEye );
+
+ Matrix4 GetHMDMatrixProjectionEye( vr::Hmd_Eye nEye );
+ Matrix4 GetHMDMatrixPoseEye( vr::Hmd_Eye nEye );
+ Matrix4 GetCurrentViewProjectionMatrix( vr::Hmd_Eye nEye );
+ void UpdateHMDMatrixPose();
+
+ Matrix4 ConvertSteamVRMatrixToMatrix4( const vr::HmdMatrix34_t &matPose );
+
+ GLuint CompileGLShader( const char *pchShaderName, const char *pchVertexShader, const char *pchFragmentShader );
+ bool CreateAllShaders();
+
+ void SetupRenderModelForTrackedDevice( vr::TrackedDeviceIndex_t unTrackedDeviceIndex );
+ CGLRenderModel *FindOrLoadRenderModel( const char *pchRenderModelName );
+
+private:
+ bool m_bDebugOpenGL;
+ bool m_bVerbose;
+ bool m_bPerf;
+ bool m_bVblank;
+ bool m_bGlFinishHack;
+
+ vr::IVRSystem *m_pHMD;
+ vr::IVRRenderModels *m_pRenderModels;
+ std::string m_strDriver;
+ std::string m_strDisplay;
+ vr::TrackedDevicePose_t m_rTrackedDevicePose[ vr::k_unMaxTrackedDeviceCount ];
+ Matrix4 m_rmat4DevicePose[ vr::k_unMaxTrackedDeviceCount ];
+ bool m_rbShowTrackedDevice[ vr::k_unMaxTrackedDeviceCount ];
+
+private:
+ SimpleOpenGL3App* m_app;
+ uint32_t m_nWindowWidth;
+ uint32_t m_nWindowHeight;
+ bool m_hasContext;
+
+private: // OpenGL bookkeeping
+ int m_iTrackedControllerCount;
+ int m_iTrackedControllerCount_Last;
+ int m_iValidPoseCount;
+ int m_iValidPoseCount_Last;
+ bool m_bShowCubes;
+
+ std::string m_strPoseClasses; // what classes we saw poses for this frame
+ char m_rDevClassChar[ vr::k_unMaxTrackedDeviceCount ]; // for each device, a character representing its class
+
+ int m_iSceneVolumeWidth;
+ int m_iSceneVolumeHeight;
+ int m_iSceneVolumeDepth;
+ float m_fScaleSpacing;
+ float m_fScale;
+
+ int m_iSceneVolumeInit; // if you want something other than the default 20x20x20
+
+ float m_fNearClip;
+ float m_fFarClip;
+
+ GLuint m_iTexture;
+
+ unsigned int m_uiVertcount;
+
+ GLuint m_glSceneVertBuffer;
+ GLuint m_unSceneVAO;
+ GLuint m_unLensVAO;
+ GLuint m_glIDVertBuffer;
+ GLuint m_glIDIndexBuffer;
+ unsigned int m_uiIndexSize;
+
+ GLuint m_glControllerVertBuffer;
+ GLuint m_unControllerVAO;
+ unsigned int m_uiControllerVertcount;
+
+ Matrix4 m_mat4HMDPose;
+ Matrix4 m_mat4eyePosLeft;
+ Matrix4 m_mat4eyePosRight;
+
+ Matrix4 m_mat4ProjectionCenter;
+ Matrix4 m_mat4ProjectionLeft;
+ Matrix4 m_mat4ProjectionRight;
+
+ struct VertexDataScene
+ {
+ Vector3 position;
+ Vector2 texCoord;
+ };
+
+ struct VertexDataLens
+ {
+ Vector2 position;
+ Vector2 texCoordRed;
+ Vector2 texCoordGreen;
+ Vector2 texCoordBlue;
+ };
+
+ GLuint m_unSceneProgramID;
+ GLuint m_unLensProgramID;
+ GLuint m_unControllerTransformProgramID;
+ GLuint m_unRenderModelProgramID;
+
+ GLint m_nSceneMatrixLocation;
+ GLint m_nControllerMatrixLocation;
+ GLint m_nRenderModelMatrixLocation;
+
+ struct FramebufferDesc
+ {
+ GLuint m_nDepthBufferId;
+ GLuint m_nRenderTextureId;
+ GLuint m_nRenderFramebufferId;
+ GLuint m_nResolveTextureId;
+ GLuint m_nResolveFramebufferId;
+ };
+ FramebufferDesc leftEyeDesc;
+ FramebufferDesc rightEyeDesc;
+
+ bool CreateFrameBuffer( int nWidth, int nHeight, FramebufferDesc &framebufferDesc );
+
+ uint32_t m_nRenderWidth;
+ uint32_t m_nRenderHeight;
+
+ std::vector< CGLRenderModel * > m_vecRenderModels;
+ CGLRenderModel *m_rTrackedDeviceToRenderModel[ vr::k_unMaxTrackedDeviceCount ];
+};
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Constructor
+//-----------------------------------------------------------------------------
+CMainApplication::CMainApplication( int argc, char *argv[] )
+ : m_app(NULL)
+ , m_hasContext(false)
+ , m_nWindowWidth( 1280 )
+ , m_nWindowHeight( 720 )
+ , m_unSceneProgramID( 0 )
+ , m_unLensProgramID( 0 )
+ , m_unControllerTransformProgramID( 0 )
+ , m_unRenderModelProgramID( 0 )
+ , m_pHMD( NULL )
+ , m_pRenderModels( NULL )
+ , m_bDebugOpenGL( false )
+ , m_bVerbose( false )
+ , m_bPerf( false )
+ , m_bVblank( false )
+ , m_bGlFinishHack( true )
+ , m_glControllerVertBuffer( 0 )
+ , m_unControllerVAO( 0 )
+ , m_unLensVAO( 0 )
+ , m_unSceneVAO( 0 )
+ , m_nSceneMatrixLocation( -1 )
+ , m_nControllerMatrixLocation( -1 )
+ , m_nRenderModelMatrixLocation( -1 )
+ , m_iTrackedControllerCount( 0 )
+ , m_iTrackedControllerCount_Last( -1 )
+ , m_iValidPoseCount( 0 )
+ , m_iValidPoseCount_Last( -1 )
+ , m_iSceneVolumeInit( 20 )
+ , m_strPoseClasses("")
+ , m_bShowCubes( false )
+{
+
+ for( int i = 1; i < argc; i++ )
+ {
+ if( !stricmp( argv[i], "-gldebug" ) )
+ {
+ m_bDebugOpenGL = true;
+ }
+ else if( !stricmp( argv[i], "-verbose" ) )
+ {
+ m_bVerbose = true;
+ }
+ else if( !stricmp( argv[i], "-novblank" ) )
+ {
+ m_bVblank = false;
+ }
+ else if( !stricmp( argv[i], "-noglfinishhack" ) )
+ {
+ m_bGlFinishHack = false;
+ }
+ else if( !stricmp( argv[i], "-noprintf" ) )
+ {
+ g_bPrintf = false;
+ }
+ else if ( !stricmp( argv[i], "-cubevolume" ) && ( argc > i + 1 ) && ( *argv[ i + 1 ] != '-' ) )
+ {
+ m_iSceneVolumeInit = atoi( argv[ i + 1 ] );
+ i++;
+ }
+ }
+ // other initialization tasks are done in BInit
+ memset(m_rDevClassChar, 0, sizeof(m_rDevClassChar));
+};
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Destructor
+//-----------------------------------------------------------------------------
+CMainApplication::~CMainApplication()
+{
+ // work is done in Shutdown
+ b3Printf( "Shutdown" );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Helper to get a string from a tracked device property and turn it
+// into a std::string
+//-----------------------------------------------------------------------------
+std::string GetTrackedDeviceString( vr::IVRSystem *pHmd, vr::TrackedDeviceIndex_t unDevice, vr::TrackedDeviceProperty prop, vr::TrackedPropertyError *peError = NULL )
+{
+ uint32_t unRequiredBufferLen = pHmd->GetStringTrackedDeviceProperty( unDevice, prop, NULL, 0, peError );
+ if( unRequiredBufferLen == 0 )
+ return "";
+
+ char *pchBuffer = new char[ unRequiredBufferLen ];
+ unRequiredBufferLen = pHmd->GetStringTrackedDeviceProperty( unDevice, prop, pchBuffer, unRequiredBufferLen, peError );
+ std::string sResult = pchBuffer;
+ delete [] pchBuffer;
+ return sResult;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+bool CMainApplication::BInit()
+{
+
+ // Loading the SteamVR Runtime
+ vr::EVRInitError eError = vr::VRInitError_None;
+ m_pHMD = vr::VR_Init( &eError, vr::VRApplication_Scene );
+
+ if ( eError != vr::VRInitError_None )
+ {
+ m_pHMD = NULL;
+ char buf[1024];
+ sprintf_s( buf, sizeof( buf ), "Unable to init VR runtime: %s", vr::VR_GetVRInitErrorAsEnglishDescription( eError ) );
+ b3Warning( "VR_Init Failed %s", buf);
+ return false;
+ }
+
+
+ m_pRenderModels = (vr::IVRRenderModels *)vr::VR_GetGenericInterface( vr::IVRRenderModels_Version, &eError );
+ if( !m_pRenderModels )
+ {
+ m_pHMD = NULL;
+ vr::VR_Shutdown();
+
+ char buf[1024];
+ sprintf_s( buf, sizeof( buf ), "Unable to get render model interface: %s", vr::VR_GetVRInitErrorAsEnglishDescription( eError ) );
+ b3Warning( "VR_Init Failed %s", buf);
+ return false;
+ }
+
+// int nWindowPosX = 700;
+// int nWindowPosY = 100;
+ m_nWindowWidth = 1280;
+ m_nWindowHeight = 720;
+
+ /*
+
+ //SDL_GL_SetAttribute( SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_COMPATIBILITY );
+ SDL_GL_SetAttribute( SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_CORE );
+ SDL_GL_SetAttribute( SDL_GL_MULTISAMPLEBUFFERS, 0 );
+ SDL_GL_SetAttribute( SDL_GL_MULTISAMPLESAMPLES, 0 );
+ if( m_bDebugOpenGL )
+ SDL_GL_SetAttribute( SDL_GL_CONTEXT_FLAGS, SDL_GL_CONTEXT_DEBUG_FLAG );
+
+ */
+ m_app = new SimpleOpenGL3App("SimpleOpenGL3App",m_nWindowWidth,m_nWindowHeight,true);
+
+
+ sGuiPtr = new OpenGLGuiHelper(m_app,false);
+ sGuiPtr->setVRMode(true);
+
+ //sGuiPtr = new DummyGUIHelper;
+
+
+ CommonExampleOptions options(sGuiPtr);
+
+ sExample = StandaloneExampleCreateFunc(options);
+ sExample->initPhysics();
+ sExample->resetCamera();
+
+#if 0
+ int cubeIndex = m_app->registerCubeShape(1,1,1);
+
+ b3Quaternion orn(0,0,0,1);
+
+ {
+ b3Vector3 color=b3MakeVector3(0.3,0.3,0.6);
+ b3Vector3 pos = b3MakeVector3(0,0,0);
+ b3Vector3 scaling=b3MakeVector3 (1,.1,1);
+ m_app->m_renderer->registerGraphicsInstance(cubeIndex,pos,orn,color,scaling);
+ }
+ {
+ b3Vector3 color=b3MakeVector3(0.3,0.6,0.3);
+ b3Vector3 pos = b3MakeVector3(0,0.3,0);
+ b3Vector3 scaling=b3MakeVector3 (.1,.1,.1);
+ m_app->m_renderer->registerGraphicsInstance(cubeIndex,pos,orn,color,scaling);
+ }
+#endif
+
+
+
+ m_app->m_renderer->writeTransforms();
+
+
+
+/* if (m_pWindow == NULL)
+ {
+ printf( "%s - Window could not be created! SDL Error: %s\n", __FUNCTION__, SDL_GetError() );
+ return false;
+ }
+ */
+
+ /*m_pContext = SDL_GL_CreateContext(m_pWindow);
+ if (m_pContext == NULL)
+ {
+ printf( "%s - OpenGL context could not be created! SDL Error: %s\n", __FUNCTION__, SDL_GetError() );
+ return false;
+ }
+
+
+ glewExperimental = GL_TRUE;
+ GLenum nGlewError = glewInit();
+ if (nGlewError != GLEW_OK)
+ {
+ printf( "%s - Error initializing GLEW! %s\n", __FUNCTION__, glewGetErrorString( nGlewError ) );
+ return false;
+ }
+ glGetError(); // to clear the error caused deep in GLEW
+
+ if ( SDL_GL_SetSwapInterval( m_bVblank ? 1 : 0 ) < 0 )
+ {
+ printf( "%s - Warning: Unable to set VSync! SDL Error: %s\n", __FUNCTION__, SDL_GetError() );
+ return false;
+ }
+
+ */
+ m_strDriver = "No Driver";
+ m_strDisplay = "No Display";
+
+ m_strDriver = GetTrackedDeviceString( m_pHMD, vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_TrackingSystemName_String );
+ m_strDisplay = GetTrackedDeviceString( m_pHMD, vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_SerialNumber_String );
+
+ std::string strWindowTitle = "hellovr_bullet - " + m_strDriver + " " + m_strDisplay;
+ m_app->m_window->setWindowTitle(strWindowTitle.c_str() );
+
+ // cube array
+ m_iSceneVolumeWidth = m_iSceneVolumeInit;
+ m_iSceneVolumeHeight = m_iSceneVolumeInit;
+ m_iSceneVolumeDepth = m_iSceneVolumeInit;
+
+ m_fScale = 0.3f;
+ m_fScaleSpacing = 4.0f;
+
+ m_fNearClip = 0.1f;
+ m_fFarClip = 3000.0f;
+
+ m_iTexture = 0;
+ m_uiVertcount = 0;
+
+// m_MillisecondsTimer.start(1, this);
+// m_SecondsTimer.start(1000, this);
+
+ if (!BInitGL())
+ {
+ printf("%s - Unable to initialize OpenGL!\n", __FUNCTION__);
+ return false;
+ }
+
+ if (!BInitCompositor())
+ {
+ printf("%s - Failed to initialize VR Compositor!\n", __FUNCTION__);
+ return false;
+ }
+
+ return true;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+/*void APIENTRY DebugCallback(GLenum source, GLenum type, GLuint id, GLenum severity, GLsizei length, const char* message, const void* userParam)
+{
+ b3Printf( "GL Error: %s\n", message );
+}
+*/
+
+static void APIENTRY DebugCallback (GLenum source, GLenum type, GLuint id, GLenum severity, GLsizei length, const GLchar* message, GLvoid* userParam)
+{
+ b3Printf( "GL Error: %s\n", message );
+}
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+bool CMainApplication::BInitGL()
+{
+ if( m_bDebugOpenGL )
+ {
+ const GLvoid *userParam=0;
+ glDebugMessageCallback(DebugCallback, userParam);
+ glDebugMessageControl( GL_DONT_CARE, GL_DONT_CARE, GL_DONT_CARE, 0, nullptr, GL_TRUE );
+ glEnable(GL_DEBUG_OUTPUT_SYNCHRONOUS);
+ }
+
+ if( !CreateAllShaders() )
+ return false;
+
+ SetupTexturemaps();
+ SetupScene();
+ SetupCameras();
+ SetupStereoRenderTargets();
+ SetupDistortion();
+
+ SetupRenderModels();
+
+ return true;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+bool CMainApplication::BInitCompositor()
+{
+ vr::EVRInitError peError = vr::VRInitError_None;
+
+ if ( !vr::VRCompositor() )
+ {
+ printf( "Compositor initialization failed. See log file for details\n" );
+ return false;
+ }
+
+ return true;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+void CMainApplication::Shutdown()
+{
+ if( m_pHMD )
+ {
+ vr::VR_Shutdown();
+ m_pHMD = NULL;
+ }
+
+ for( std::vector< CGLRenderModel * >::iterator i = m_vecRenderModels.begin(); i != m_vecRenderModels.end(); i++ )
+ {
+ delete (*i);
+ }
+ m_vecRenderModels.clear();
+
+ if( m_hasContext)
+ {
+ if (m_glSceneVertBuffer)
+ {
+ glDebugMessageControl( GL_DONT_CARE, GL_DONT_CARE, GL_DONT_CARE, 0, nullptr, GL_FALSE );
+ glDebugMessageCallback(nullptr, nullptr);
+ glDeleteBuffers(1, &m_glSceneVertBuffer);
+ glDeleteBuffers(1, &m_glIDVertBuffer);
+ glDeleteBuffers(1, &m_glIDIndexBuffer);
+ }
+
+ if ( m_unSceneProgramID )
+ {
+ glDeleteProgram( m_unSceneProgramID );
+ }
+ if ( m_unControllerTransformProgramID )
+ {
+ glDeleteProgram( m_unControllerTransformProgramID );
+ }
+ if ( m_unRenderModelProgramID )
+ {
+ glDeleteProgram( m_unRenderModelProgramID );
+ }
+ if ( m_unLensProgramID )
+ {
+ glDeleteProgram( m_unLensProgramID );
+ }
+
+ glDeleteRenderbuffers( 1, &leftEyeDesc.m_nDepthBufferId );
+ glDeleteTextures( 1, &leftEyeDesc.m_nRenderTextureId );
+ glDeleteFramebuffers( 1, &leftEyeDesc.m_nRenderFramebufferId );
+ glDeleteTextures( 1, &leftEyeDesc.m_nResolveTextureId );
+ glDeleteFramebuffers( 1, &leftEyeDesc.m_nResolveFramebufferId );
+
+ glDeleteRenderbuffers( 1, &rightEyeDesc.m_nDepthBufferId );
+ glDeleteTextures( 1, &rightEyeDesc.m_nRenderTextureId );
+ glDeleteFramebuffers( 1, &rightEyeDesc.m_nRenderFramebufferId );
+ glDeleteTextures( 1, &rightEyeDesc.m_nResolveTextureId );
+ glDeleteFramebuffers( 1, &rightEyeDesc.m_nResolveFramebufferId );
+
+ if( m_unLensVAO != 0 )
+ {
+ glDeleteVertexArrays( 1, &m_unLensVAO );
+ }
+ if( m_unSceneVAO != 0 )
+ {
+ glDeleteVertexArrays( 1, &m_unSceneVAO );
+ }
+ if( m_unControllerVAO != 0 )
+ {
+ glDeleteVertexArrays( 1, &m_unControllerVAO );
+ }
+ }
+
+ sExample->exitPhysics();
+ delete sExample;
+
+ delete m_app;
+ m_app=0;
+
+}
+
+
+void CMainApplication::getControllerTransform(int unDevice, b3Transform& tr)
+{
+ const Matrix4 & matOrg = m_rmat4DevicePose[unDevice];
+ tr.setIdentity();
+ tr.setOrigin(b3MakeVector3(matOrg[12],matOrg[13],matOrg[14]));//pos[1]));
+ b3Matrix3x3 bmat;
+ for (int i=0;i<3;i++)
+ {
+ for (int j=0;j<3;j++)
+ {
+ bmat[i][j] = matOrg[i+4*j];
+ }
+ }
+ tr.setBasis(bmat);
+ b3Transform y2z;
+ y2z.setIdentity();
+ y2z.setRotation(b3Quaternion(0,B3_HALF_PI,0));
+ tr = y2z*tr;
+}
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+bool CMainApplication::HandleInput()
+{
+ bool bRet = false;
+
+ // Process SteamVR events
+ vr::VREvent_t event;
+ while( m_pHMD->PollNextEvent( &event, sizeof( event ) ) )
+ {
+ ProcessVREvent( event );
+ }
+
+ // Process SteamVR controller state
+ for( vr::TrackedDeviceIndex_t unDevice = 0; unDevice < vr::k_unMaxTrackedDeviceCount; unDevice++ )
+ {
+ vr::VRControllerState_t state;
+ if( m_pHMD->GetControllerState( unDevice, &state ) )
+ {
+ //we need to have the 'move' events, so no early out here
+ //if (sPrevStates[unDevice].unPacketNum != state.unPacketNum)
+ if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller )
+ {
+ sPrevStates[unDevice].unPacketNum = state.unPacketNum;
+
+ for (int button = 0; button < vr::k_EButton_Max; button++)
+ {
+ uint64_t trigger = vr::ButtonMaskFromId((vr::EVRButtonId)button);
+
+ bool isTrigger = (state.ulButtonPressed&trigger) != 0;
+ if (isTrigger)
+ {
+
+ b3Transform tr;
+ getControllerTransform(unDevice, tr);
+ float pos[3] = { tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2] };
+ b3Quaternion born = tr.getRotation();
+ float orn[4] = { born[0], born[1], born[2], born[3] };
+
+ //pressed now, not pressed before -> raise a button down event
+ if ((sPrevStates[unDevice].ulButtonPressed&trigger)==0)
+ {
+// printf("Device PRESSED: %d, button %d\n", unDevice, button);
+ if (button==2)
+ {
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+ ///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync
+ ///so it can (and likely will) cause crashes
+ ///add a special debug drawer that deals with this
+ //gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints+
+ //btIDebugDraw::DBG_DrawConstraintLimits+
+ //btIDebugDraw::DBG_DrawConstraints
+ //;
+ //gDebugDrawFlags = btIDebugDraw::DBG_DrawFrames;
+
+
+
+ }
+
+ sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn);
+
+ }
+ else
+ {
+
+// printf("Device MOVED: %d\n", unDevice);
+ sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x);
+ }
+ }
+ else
+ {
+ if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller )
+ {
+
+
+ b3Transform tr;
+ getControllerTransform(unDevice, tr);
+ float pos[3] = { tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2] };
+ b3Quaternion born = tr.getRotation();
+ float orn[4] = { born[0], born[1], born[2], born[3] };
+ // printf("Device RELEASED: %d, button %d\n", unDevice,button);
+
+ //not pressed now, but pressed before -> raise a button up event
+ if ((sPrevStates[unDevice].ulButtonPressed&trigger) != 0)
+ {
+ if (button==2)
+ {
+ gDebugDrawFlags = 0;
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL);
+ }
+
+ sExample->vrControllerButtonCallback(unDevice, button, 0, pos, orn);
+ } else
+ {
+
+ sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x);
+ }
+ }
+ }
+ }
+ }
+
+// m_rbShowTrackedDevice[ unDevice ] = state.ulButtonPressed == 0;
+ }
+ sPrevStates[unDevice] = state;
+ }
+
+ return bRet;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+
+void CMainApplication::RunMainLoop()
+{
+ bool bQuit = false;
+
+ while ( !bQuit && !m_app->m_window->requestedExit())
+ {
+ B3_PROFILE("main");
+
+ bQuit = HandleInput();
+
+ RenderFrame();
+ }
+
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Processes a single VR event
+//-----------------------------------------------------------------------------
+void CMainApplication::ProcessVREvent( const vr::VREvent_t & event )
+{
+ switch( event.eventType )
+ {
+ case vr::VREvent_TrackedDeviceActivated:
+ {
+ SetupRenderModelForTrackedDevice( event.trackedDeviceIndex );
+ b3Printf( "Device %u attached. Setting up render model.\n", event.trackedDeviceIndex );
+ }
+ break;
+ case vr::VREvent_TrackedDeviceDeactivated:
+ {
+ b3Printf( "Device %u detached.\n", event.trackedDeviceIndex );
+ }
+ break;
+ case vr::VREvent_TrackedDeviceUpdated:
+ {
+ b3Printf( "Device %u updated.\n", event.trackedDeviceIndex );
+ }
+ break;
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+void CMainApplication::RenderFrame()
+{
+ // for now as fast as possible
+ if ( m_pHMD )
+ {
+ {
+ B3_PROFILE("DrawControllers");
+ DrawControllers();
+ }
+ RenderStereoTargets();
+ {
+ B3_PROFILE("RenderDistortion");
+ RenderDistortion();
+ }
+
+ vr::Texture_t leftEyeTexture = {(void*)leftEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma };
+ vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture );
+ vr::Texture_t rightEyeTexture = {(void*)rightEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma };
+ vr::VRCompositor()->Submit(vr::Eye_Right, &rightEyeTexture );
+ }
+
+ if ( m_bVblank && m_bGlFinishHack )
+ {
+ B3_PROFILE("bGlFinishHack");
+ //$ HACKHACK. From gpuview profiling, it looks like there is a bug where two renders and a present
+ // happen right before and after the vsync causing all kinds of jittering issues. This glFinish()
+ // appears to clear that up. Temporary fix while I try to get nvidia to investigate this problem.
+ // 1/29/2014 mikesart
+ glFinish();
+ }
+
+ // SwapWindow
+ {
+ B3_PROFILE("m_app->swapBuffer");
+ m_app->swapBuffer();
+ //SDL_GL_SwapWindow( m_pWindow );
+
+ }
+
+ // Clear
+ {
+ B3_PROFILE("glClearColor");
+ // We want to make sure the glFinish waits for the entire present to complete, not just the submission
+ // of the command. So, we do a clear here right here so the glFinish will wait fully for the swap.
+ glClearColor( 0, 0, 0, 1 );
+ glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
+ }
+
+ // Flush and wait for swap.
+ if ( m_bVblank )
+ {
+ B3_PROFILE("glFlushglFinish");
+
+ glFlush();
+ glFinish();
+ }
+
+ // Spew out the controller and pose count whenever they change.
+ if ( m_iTrackedControllerCount != m_iTrackedControllerCount_Last || m_iValidPoseCount != m_iValidPoseCount_Last )
+ {
+ B3_PROFILE("debug pose");
+
+ m_iValidPoseCount_Last = m_iValidPoseCount;
+ m_iTrackedControllerCount_Last = m_iTrackedControllerCount;
+
+ b3Printf( "PoseCount:%d(%s) Controllers:%d\n", m_iValidPoseCount, m_strPoseClasses.c_str(), m_iTrackedControllerCount );
+ }
+
+ {
+ B3_PROFILE("UpdateHMDMatrixPose");
+ UpdateHMDMatrixPose();
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Compiles a GL shader program and returns the handle. Returns 0 if
+// the shader couldn't be compiled for some reason.
+//-----------------------------------------------------------------------------
+GLuint CMainApplication::CompileGLShader( const char *pchShaderName, const char *pchVertexShader, const char *pchFragmentShader )
+{
+ GLuint unProgramID = glCreateProgram();
+
+ GLuint nSceneVertexShader = glCreateShader(GL_VERTEX_SHADER);
+ glShaderSource( nSceneVertexShader, 1, &pchVertexShader, NULL);
+ glCompileShader( nSceneVertexShader );
+
+ GLint vShaderCompiled = GL_FALSE;
+ glGetShaderiv( nSceneVertexShader, GL_COMPILE_STATUS, &vShaderCompiled);
+ if ( vShaderCompiled != GL_TRUE)
+ {
+ b3Printf("%s - Unable to compile vertex shader %d!\n", pchShaderName, nSceneVertexShader);
+ glDeleteProgram( unProgramID );
+ glDeleteShader( nSceneVertexShader );
+ return 0;
+ }
+ glAttachShader( unProgramID, nSceneVertexShader);
+ glDeleteShader( nSceneVertexShader ); // the program hangs onto this once it's attached
+
+ GLuint nSceneFragmentShader = glCreateShader(GL_FRAGMENT_SHADER);
+ glShaderSource( nSceneFragmentShader, 1, &pchFragmentShader, NULL);
+ glCompileShader( nSceneFragmentShader );
+
+ GLint fShaderCompiled = GL_FALSE;
+ glGetShaderiv( nSceneFragmentShader, GL_COMPILE_STATUS, &fShaderCompiled);
+ if (fShaderCompiled != GL_TRUE)
+ {
+ b3Printf("%s - Unable to compile fragment shader %d!\n", pchShaderName, nSceneFragmentShader );
+ glDeleteProgram( unProgramID );
+ glDeleteShader( nSceneFragmentShader );
+ return 0;
+ }
+
+ glAttachShader( unProgramID, nSceneFragmentShader );
+ glDeleteShader( nSceneFragmentShader ); // the program hangs onto this once it's attached
+
+ glLinkProgram( unProgramID );
+
+ GLint programSuccess = GL_TRUE;
+ glGetProgramiv( unProgramID, GL_LINK_STATUS, &programSuccess);
+ if ( programSuccess != GL_TRUE )
+ {
+ b3Printf("%s - Error linking program %d!\n", pchShaderName, unProgramID);
+ glDeleteProgram( unProgramID );
+ return 0;
+ }
+
+ glUseProgram( unProgramID );
+ glUseProgram( 0 );
+
+ return unProgramID;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Creates all the shaders used by HelloVR SDL
+//-----------------------------------------------------------------------------
+bool CMainApplication::CreateAllShaders()
+{
+ m_unSceneProgramID = CompileGLShader(
+ "Scene",
+
+ // Vertex Shader
+ "#version 410\n"
+ "uniform mat4 matrix;\n"
+ "layout(location = 0) in vec4 position;\n"
+ "layout(location = 1) in vec2 v2UVcoordsIn;\n"
+ "layout(location = 2) in vec3 v3NormalIn;\n"
+ "out vec2 v2UVcoords;\n"
+ "void main()\n"
+ "{\n"
+ " v2UVcoords = v2UVcoordsIn;\n"
+ " gl_Position = matrix * position;\n"
+ "}\n",
+
+ // Fragment Shader
+ "#version 410 core\n"
+ "uniform sampler2D mytexture;\n"
+ "in vec2 v2UVcoords;\n"
+ "out vec4 outputColor;\n"
+ "void main()\n"
+ "{\n"
+ " outputColor = texture(mytexture, v2UVcoords);\n"
+ "}\n"
+ );
+ m_nSceneMatrixLocation = glGetUniformLocation( m_unSceneProgramID, "matrix" );
+ if( m_nSceneMatrixLocation == -1 )
+ {
+ b3Printf( "Unable to find matrix uniform in scene shader\n" );
+ return false;
+ }
+
+ m_unControllerTransformProgramID = CompileGLShader(
+ "Controller",
+
+ // vertex shader
+ "#version 410\n"
+ "uniform mat4 matrix;\n"
+ "layout(location = 0) in vec4 position;\n"
+ "layout(location = 1) in vec3 v3ColorIn;\n"
+ "out vec4 v4Color;\n"
+ "void main()\n"
+ "{\n"
+ " v4Color.xyz = v3ColorIn; v4Color.a = 1.0;\n"
+ " gl_Position = matrix * position;\n"
+ "}\n",
+
+ // fragment shader
+ "#version 410\n"
+ "in vec4 v4Color;\n"
+ "out vec4 outputColor;\n"
+ "void main()\n"
+ "{\n"
+ " outputColor = v4Color;\n"
+ "}\n"
+ );
+ m_nControllerMatrixLocation = glGetUniformLocation( m_unControllerTransformProgramID, "matrix" );
+ if( m_nControllerMatrixLocation == -1 )
+ {
+ b3Printf( "Unable to find matrix uniform in controller shader\n" );
+ return false;
+ }
+
+ m_unRenderModelProgramID = CompileGLShader(
+ "render model",
+
+ // vertex shader
+ "#version 410\n"
+ "uniform mat4 matrix;\n"
+ "layout(location = 0) in vec4 position;\n"
+ "layout(location = 1) in vec3 v3NormalIn;\n"
+ "layout(location = 2) in vec2 v2TexCoordsIn;\n"
+ "out vec2 v2TexCoord;\n"
+ "void main()\n"
+ "{\n"
+ " v2TexCoord = v2TexCoordsIn;\n"
+ " gl_Position = matrix * vec4(position.xyz, 1);\n"
+ "}\n",
+
+ //fragment shader
+ "#version 410 core\n"
+ "uniform sampler2D diffuse;\n"
+ "in vec2 v2TexCoord;\n"
+ "out vec4 outputColor;\n"
+ "void main()\n"
+ "{\n"
+ " outputColor = texture( diffuse, v2TexCoord);\n"
+ "}\n"
+
+ );
+ m_nRenderModelMatrixLocation = glGetUniformLocation( m_unRenderModelProgramID, "matrix" );
+ if( m_nRenderModelMatrixLocation == -1 )
+ {
+ b3Printf( "Unable to find matrix uniform in render model shader\n" );
+ return false;
+ }
+
+ m_unLensProgramID = CompileGLShader(
+ "Distortion",
+
+ // vertex shader
+ "#version 410 core\n"
+ "layout(location = 0) in vec4 position;\n"
+ "layout(location = 1) in vec2 v2UVredIn;\n"
+ "layout(location = 2) in vec2 v2UVGreenIn;\n"
+ "layout(location = 3) in vec2 v2UVblueIn;\n"
+ "noperspective out vec2 v2UVred;\n"
+ "noperspective out vec2 v2UVgreen;\n"
+ "noperspective out vec2 v2UVblue;\n"
+ "void main()\n"
+ "{\n"
+ " v2UVred = v2UVredIn;\n"
+ " v2UVgreen = v2UVGreenIn;\n"
+ " v2UVblue = v2UVblueIn;\n"
+ " gl_Position = position;\n"
+ "}\n",
+
+ // fragment shader
+ "#version 410 core\n"
+ "uniform sampler2D mytexture;\n"
+
+ "noperspective in vec2 v2UVred;\n"
+ "noperspective in vec2 v2UVgreen;\n"
+ "noperspective in vec2 v2UVblue;\n"
+
+ "out vec4 outputColor;\n"
+
+ "void main()\n"
+ "{\n"
+ " float fBoundsCheck = ( (dot( vec2( lessThan( v2UVgreen.xy, vec2(0.05, 0.05)) ), vec2(1.0, 1.0))+dot( vec2( greaterThan( v2UVgreen.xy, vec2( 0.95, 0.95)) ), vec2(1.0, 1.0))) );\n"
+ " if( fBoundsCheck > 1.0 )\n"
+ " { outputColor = vec4( 0, 0, 0, 1.0 ); }\n"
+ " else\n"
+ " {\n"
+ " float red = texture(mytexture, v2UVred).x;\n"
+ " float green = texture(mytexture, v2UVgreen).y;\n"
+ " float blue = texture(mytexture, v2UVblue).z;\n"
+ " outputColor = vec4( red, green, blue, 1.0 ); }\n"
+ "}\n"
+ );
+
+
+ return m_unSceneProgramID != 0
+ && m_unControllerTransformProgramID != 0
+ && m_unRenderModelProgramID != 0
+ && m_unLensProgramID != 0;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+bool CMainApplication::SetupTexturemaps()
+{
+ std::string sExecutableDirectory = Path_StripFilename( Path_GetExecutablePath() );
+ std::string strFullPath = Path_MakeAbsolute( "../cube_texture.png", sExecutableDirectory );
+
+ std::vector<unsigned char> imageRGBA;
+ unsigned nImageWidth, nImageHeight;
+ unsigned nError = lodepng::decode( imageRGBA, nImageWidth, nImageHeight, strFullPath.c_str() );
+
+ if ( nError != 0 )
+ return false;
+
+ glGenTextures(1, &m_iTexture );
+ glBindTexture( GL_TEXTURE_2D, m_iTexture );
+
+ glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, nImageWidth, nImageHeight,
+ 0, GL_RGBA, GL_UNSIGNED_BYTE, &imageRGBA[0] );
+
+ glGenerateMipmap(GL_TEXTURE_2D);
+
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR );
+
+ GLfloat fLargest;
+ glGetFloatv(GL_MAX_TEXTURE_MAX_ANISOTROPY_EXT, &fLargest);
+ glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAX_ANISOTROPY_EXT, fLargest);
+
+ glBindTexture( GL_TEXTURE_2D, 0 );
+
+ return ( m_iTexture != 0 );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: create a sea of cubes
+//-----------------------------------------------------------------------------
+void CMainApplication::SetupScene()
+{
+ if ( !m_pHMD )
+ return;
+
+ std::vector<float> vertdataarray;
+
+ Matrix4 matScale;
+ matScale.scale( m_fScale, m_fScale, m_fScale );
+ Matrix4 matTransform;
+ matTransform.translate(
+ -( (float)m_iSceneVolumeWidth * m_fScaleSpacing ) / 2.f,
+ -( (float)m_iSceneVolumeHeight * m_fScaleSpacing ) / 2.f,
+ -( (float)m_iSceneVolumeDepth * m_fScaleSpacing ) / 2.f);
+
+ Matrix4 mat = matScale * matTransform;
+
+ for( int z = 0; z< m_iSceneVolumeDepth; z++ )
+ {
+ for( int y = 0; y< m_iSceneVolumeHeight; y++ )
+ {
+ for( int x = 0; x< m_iSceneVolumeWidth; x++ )
+ {
+ AddCubeToScene( mat, vertdataarray );
+ mat = mat * Matrix4().translate( m_fScaleSpacing, 0, 0 );
+ }
+ mat = mat * Matrix4().translate( -((float)m_iSceneVolumeWidth) * m_fScaleSpacing, m_fScaleSpacing, 0 );
+ }
+ mat = mat * Matrix4().translate( 0, -((float)m_iSceneVolumeHeight) * m_fScaleSpacing, m_fScaleSpacing );
+ }
+ m_uiVertcount = vertdataarray.size()/5;
+
+ glGenVertexArrays( 1, &m_unSceneVAO );
+ glBindVertexArray( m_unSceneVAO );
+
+ glGenBuffers( 1, &m_glSceneVertBuffer );
+ glBindBuffer( GL_ARRAY_BUFFER, m_glSceneVertBuffer );
+ glBufferData( GL_ARRAY_BUFFER, sizeof(float) * vertdataarray.size(), &vertdataarray[0], GL_STATIC_DRAW);
+
+ glBindBuffer( GL_ARRAY_BUFFER, m_glSceneVertBuffer );
+
+ GLsizei stride = sizeof(VertexDataScene);
+ uintptr_t offset = 0;
+
+ glEnableVertexAttribArray( 0 );
+ glVertexAttribPointer( 0, 3, GL_FLOAT, GL_FALSE, stride , (const void *)offset);
+
+ offset += sizeof(Vector3);
+ glEnableVertexAttribArray( 1 );
+ glVertexAttribPointer( 1, 2, GL_FLOAT, GL_FALSE, stride, (const void *)offset);
+
+ glBindVertexArray( 0 );
+ glDisableVertexAttribArray(0);
+ glDisableVertexAttribArray(1);
+
+ m_hasContext = true;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+void CMainApplication::AddCubeVertex( float fl0, float fl1, float fl2, float fl3, float fl4, std::vector<float> &vertdata )
+{
+ vertdata.push_back( fl0 );
+ vertdata.push_back( fl1 );
+ vertdata.push_back( fl2 );
+ vertdata.push_back( fl3 );
+ vertdata.push_back( fl4 );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+void CMainApplication::AddCubeToScene( Matrix4 mat, std::vector<float> &vertdata )
+{
+ // Matrix4 mat( outermat.data() );
+
+ Vector4 A = mat * Vector4( 0, 0, 0, 1 );
+ Vector4 B = mat * Vector4( 1, 0, 0, 1 );
+ Vector4 C = mat * Vector4( 1, 1, 0, 1 );
+ Vector4 D = mat * Vector4( 0, 1, 0, 1 );
+ Vector4 E = mat * Vector4( 0, 0, 1, 1 );
+ Vector4 F = mat * Vector4( 1, 0, 1, 1 );
+ Vector4 G = mat * Vector4( 1, 1, 1, 1 );
+ Vector4 H = mat * Vector4( 0, 1, 1, 1 );
+
+ // triangles instead of quads
+ AddCubeVertex( E.x, E.y, E.z, 0, 1, vertdata ); //Front
+ AddCubeVertex( F.x, F.y, F.z, 1, 1, vertdata );
+ AddCubeVertex( G.x, G.y, G.z, 1, 0, vertdata );
+ AddCubeVertex( G.x, G.y, G.z, 1, 0, vertdata );
+ AddCubeVertex( H.x, H.y, H.z, 0, 0, vertdata );
+ AddCubeVertex( E.x, E.y, E.z, 0, 1, vertdata );
+
+ AddCubeVertex( B.x, B.y, B.z, 0, 1, vertdata ); //Back
+ AddCubeVertex( A.x, A.y, A.z, 1, 1, vertdata );
+ AddCubeVertex( D.x, D.y, D.z, 1, 0, vertdata );
+ AddCubeVertex( D.x, D.y, D.z, 1, 0, vertdata );
+ AddCubeVertex( C.x, C.y, C.z, 0, 0, vertdata );
+ AddCubeVertex( B.x, B.y, B.z, 0, 1, vertdata );
+
+ AddCubeVertex( H.x, H.y, H.z, 0, 1, vertdata ); //Top
+ AddCubeVertex( G.x, G.y, G.z, 1, 1, vertdata );
+ AddCubeVertex( C.x, C.y, C.z, 1, 0, vertdata );
+ AddCubeVertex( C.x, C.y, C.z, 1, 0, vertdata );
+ AddCubeVertex( D.x, D.y, D.z, 0, 0, vertdata );
+ AddCubeVertex( H.x, H.y, H.z, 0, 1, vertdata );
+
+ AddCubeVertex( A.x, A.y, A.z, 0, 1, vertdata ); //Bottom
+ AddCubeVertex( B.x, B.y, B.z, 1, 1, vertdata );
+ AddCubeVertex( F.x, F.y, F.z, 1, 0, vertdata );
+ AddCubeVertex( F.x, F.y, F.z, 1, 0, vertdata );
+ AddCubeVertex( E.x, E.y, E.z, 0, 0, vertdata );
+ AddCubeVertex( A.x, A.y, A.z, 0, 1, vertdata );
+
+ AddCubeVertex( A.x, A.y, A.z, 0, 1, vertdata ); //Left
+ AddCubeVertex( E.x, E.y, E.z, 1, 1, vertdata );
+ AddCubeVertex( H.x, H.y, H.z, 1, 0, vertdata );
+ AddCubeVertex( H.x, H.y, H.z, 1, 0, vertdata );
+ AddCubeVertex( D.x, D.y, D.z, 0, 0, vertdata );
+ AddCubeVertex( A.x, A.y, A.z, 0, 1, vertdata );
+
+ AddCubeVertex( F.x, F.y, F.z, 0, 1, vertdata ); //Right
+ AddCubeVertex( B.x, B.y, B.z, 1, 1, vertdata );
+ AddCubeVertex( C.x, C.y, C.z, 1, 0, vertdata );
+ AddCubeVertex( C.x, C.y, C.z, 1, 0, vertdata );
+ AddCubeVertex( G.x, G.y, G.z, 0, 0, vertdata );
+ AddCubeVertex( F.x, F.y, F.z, 0, 1, vertdata );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Draw all of the controllers as X/Y/Z lines
+//-----------------------------------------------------------------------------
+void CMainApplication::DrawControllers()
+{
+ // don't draw controllers if somebody else has input focus
+ if( m_pHMD->IsInputFocusCapturedByAnotherProcess() )
+ return;
+
+ std::vector<float> vertdataarray;
+
+ m_uiControllerVertcount = 0;
+ m_iTrackedControllerCount = 0;
+
+ for ( vr::TrackedDeviceIndex_t unTrackedDevice = vr::k_unTrackedDeviceIndex_Hmd + 1; unTrackedDevice < vr::k_unMaxTrackedDeviceCount; ++unTrackedDevice )
+ {
+ if ( !m_pHMD->IsTrackedDeviceConnected( unTrackedDevice ) )
+ continue;
+
+ if( m_pHMD->GetTrackedDeviceClass( unTrackedDevice ) != vr::TrackedDeviceClass_Controller )
+ continue;
+
+ m_iTrackedControllerCount += 1;
+
+ if( !m_rTrackedDevicePose[ unTrackedDevice ].bPoseIsValid )
+ continue;
+
+ const Matrix4 & mat = m_rmat4DevicePose[unTrackedDevice];
+
+ Vector4 center = mat * Vector4( 0, 0, 0, 1 );
+
+ for ( int i = 0; i < 3; ++i )
+ {
+ Vector3 color( 0, 0, 0 );
+ Vector4 point( 0, 0, 0, 1 );
+ point[i] += 0.05f; // offset in X, Y, Z
+ color[i] = 1.0; // R, G, B
+ point = mat * point;
+ vertdataarray.push_back( center.x );
+ vertdataarray.push_back( center.y );
+ vertdataarray.push_back( center.z );
+
+ vertdataarray.push_back( color.x );
+ vertdataarray.push_back( color.y );
+ vertdataarray.push_back( color.z );
+
+ vertdataarray.push_back( point.x );
+ vertdataarray.push_back( point.y );
+ vertdataarray.push_back( point.z );
+
+ vertdataarray.push_back( color.x );
+ vertdataarray.push_back( color.y );
+ vertdataarray.push_back( color.z );
+
+ m_uiControllerVertcount += 2;
+ }
+
+ Vector4 start = mat * Vector4( 0, 0, -0.02f, 1 );
+ Vector4 end = mat * Vector4( 0, 0, -39.f, 1 );
+ Vector3 color( .92f, .92f, .71f );
+
+ vertdataarray.push_back( start.x );vertdataarray.push_back( start.y );vertdataarray.push_back( start.z );
+ vertdataarray.push_back( color.x );vertdataarray.push_back( color.y );vertdataarray.push_back( color.z );
+
+ vertdataarray.push_back( end.x );vertdataarray.push_back( end.y );vertdataarray.push_back( end.z );
+ vertdataarray.push_back( color.x );vertdataarray.push_back( color.y );vertdataarray.push_back( color.z );
+ m_uiControllerVertcount += 2;
+ }
+
+ // Setup the VAO the first time through.
+ if ( m_unControllerVAO == 0 )
+ {
+ glGenVertexArrays( 1, &m_unControllerVAO );
+ glBindVertexArray( m_unControllerVAO );
+
+ glGenBuffers( 1, &m_glControllerVertBuffer );
+ glBindBuffer( GL_ARRAY_BUFFER, m_glControllerVertBuffer );
+
+ GLuint stride = 2 * 3 * sizeof( float );
+ GLuint offset = 0;
+
+ glEnableVertexAttribArray( 0 );
+ glVertexAttribPointer( 0, 3, GL_FLOAT, GL_FALSE, stride, (const void *)offset);
+
+ offset += sizeof( Vector3 );
+ glEnableVertexAttribArray( 1 );
+ glVertexAttribPointer( 1, 3, GL_FLOAT, GL_FALSE, stride, (const void *)offset);
+
+ glBindVertexArray( 0 );
+ }
+
+ glBindBuffer( GL_ARRAY_BUFFER, m_glControllerVertBuffer );
+
+ // set vertex data if we have some
+ if( vertdataarray.size() > 0 )
+ {
+ //$ TODO: Use glBufferSubData for this...
+ glBufferData( GL_ARRAY_BUFFER, sizeof(float) * vertdataarray.size(), &vertdataarray[0], GL_STREAM_DRAW );
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+void CMainApplication::SetupCameras()
+{
+ m_mat4ProjectionLeft = GetHMDMatrixProjectionEye( vr::Eye_Left );
+ m_mat4ProjectionRight = GetHMDMatrixProjectionEye( vr::Eye_Right );
+ m_mat4eyePosLeft = GetHMDMatrixPoseEye( vr::Eye_Left );
+ m_mat4eyePosRight = GetHMDMatrixPoseEye( vr::Eye_Right );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+bool CMainApplication::CreateFrameBuffer( int nWidth, int nHeight, FramebufferDesc &framebufferDesc )
+{
+ glGenFramebuffers(1, &framebufferDesc.m_nRenderFramebufferId );
+ glBindFramebuffer(GL_FRAMEBUFFER, framebufferDesc.m_nRenderFramebufferId);
+
+ glGenRenderbuffers(1, &framebufferDesc.m_nDepthBufferId);
+ glBindRenderbuffer(GL_RENDERBUFFER, framebufferDesc.m_nDepthBufferId);
+ glRenderbufferStorageMultisample(GL_RENDERBUFFER, 4, GL_DEPTH_COMPONENT, nWidth, nHeight );
+ glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, framebufferDesc.m_nDepthBufferId );
+
+ glGenTextures(1, &framebufferDesc.m_nRenderTextureId );
+ glBindTexture(GL_TEXTURE_2D_MULTISAMPLE, framebufferDesc.m_nRenderTextureId );
+ glTexImage2DMultisample(GL_TEXTURE_2D_MULTISAMPLE, 4, GL_RGBA8, nWidth, nHeight, true);
+ glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D_MULTISAMPLE, framebufferDesc.m_nRenderTextureId, 0);
+
+ glGenFramebuffers(1, &framebufferDesc.m_nResolveFramebufferId );
+ glBindFramebuffer(GL_FRAMEBUFFER, framebufferDesc.m_nResolveFramebufferId);
+
+ glGenTextures(1, &framebufferDesc.m_nResolveTextureId );
+ glBindTexture(GL_TEXTURE_2D, framebufferDesc.m_nResolveTextureId );
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAX_LEVEL, 0);
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, nWidth, nHeight, 0, GL_RGBA, GL_UNSIGNED_BYTE, nullptr);
+ glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, framebufferDesc.m_nResolveTextureId, 0);
+
+ // check FBO status
+ GLenum status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
+ if (status != GL_FRAMEBUFFER_COMPLETE)
+ {
+ return false;
+ }
+
+ glBindFramebuffer( GL_FRAMEBUFFER, 0 );
+
+ return true;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+bool CMainApplication::SetupStereoRenderTargets()
+{
+ if ( !m_pHMD )
+ return false;
+
+ m_pHMD->GetRecommendedRenderTargetSize( &m_nRenderWidth, &m_nRenderHeight );
+
+ CreateFrameBuffer( m_nRenderWidth, m_nRenderHeight, leftEyeDesc );
+ CreateFrameBuffer( m_nRenderWidth, m_nRenderHeight, rightEyeDesc );
+
+ return true;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+void CMainApplication::SetupDistortion()
+{
+ if ( !m_pHMD )
+ return;
+
+ GLushort m_iLensGridSegmentCountH = 43;
+ GLushort m_iLensGridSegmentCountV = 43;
+
+ float w = (float)( 1.0/float(m_iLensGridSegmentCountH-1));
+ float h = (float)( 1.0/float(m_iLensGridSegmentCountV-1));
+
+ float u, v = 0;
+
+ std::vector<VertexDataLens> vVerts(0);
+ VertexDataLens vert;
+
+ //left eye distortion verts
+ float Xoffset = -1;
+ for( int y=0; y<m_iLensGridSegmentCountV; y++ )
+ {
+ for( int x=0; x<m_iLensGridSegmentCountH; x++ )
+ {
+ u = x*w; v = 1-y*h;
+ vert.position = Vector2( Xoffset+u, -1+2*y*h );
+
+ vr::DistortionCoordinates_t dc0 = m_pHMD->ComputeDistortion(vr::Eye_Left, u, v);
+
+ vert.texCoordRed = Vector2(dc0.rfRed[0], 1 - dc0.rfRed[1]);
+ vert.texCoordGreen = Vector2(dc0.rfGreen[0], 1 - dc0.rfGreen[1]);
+ vert.texCoordBlue = Vector2(dc0.rfBlue[0], 1 - dc0.rfBlue[1]);
+
+ vVerts.push_back( vert );
+ }
+ }
+
+ //right eye distortion verts
+ Xoffset = 0;
+ for( int y=0; y<m_iLensGridSegmentCountV; y++ )
+ {
+ for( int x=0; x<m_iLensGridSegmentCountH; x++ )
+ {
+ u = x*w; v = 1-y*h;
+ vert.position = Vector2( Xoffset+u, -1+2*y*h );
+
+ vr::DistortionCoordinates_t dc0 = m_pHMD->ComputeDistortion( vr::Eye_Right, u, v );
+
+ vert.texCoordRed = Vector2(dc0.rfRed[0], 1 - dc0.rfRed[1]);
+ vert.texCoordGreen = Vector2(dc0.rfGreen[0], 1 - dc0.rfGreen[1]);
+ vert.texCoordBlue = Vector2(dc0.rfBlue[0], 1 - dc0.rfBlue[1]);
+
+ vVerts.push_back( vert );
+ }
+ }
+
+ std::vector<GLushort> vIndices;
+ GLushort a,b,c,d;
+
+ GLushort offset = 0;
+ for( GLushort y=0; y<m_iLensGridSegmentCountV-1; y++ )
+ {
+ for( GLushort x=0; x<m_iLensGridSegmentCountH-1; x++ )
+ {
+ a = m_iLensGridSegmentCountH*y+x +offset;
+ b = m_iLensGridSegmentCountH*y+x+1 +offset;
+ c = (y+1)*m_iLensGridSegmentCountH+x+1 +offset;
+ d = (y+1)*m_iLensGridSegmentCountH+x +offset;
+ vIndices.push_back( a );
+ vIndices.push_back( b );
+ vIndices.push_back( c );
+
+ vIndices.push_back( a );
+ vIndices.push_back( c );
+ vIndices.push_back( d );
+ }
+ }
+
+ offset = (m_iLensGridSegmentCountH)*(m_iLensGridSegmentCountV);
+ for( GLushort y=0; y<m_iLensGridSegmentCountV-1; y++ )
+ {
+ for( GLushort x=0; x<m_iLensGridSegmentCountH-1; x++ )
+ {
+ a = m_iLensGridSegmentCountH*y+x +offset;
+ b = m_iLensGridSegmentCountH*y+x+1 +offset;
+ c = (y+1)*m_iLensGridSegmentCountH+x+1 +offset;
+ d = (y+1)*m_iLensGridSegmentCountH+x +offset;
+ vIndices.push_back( a );
+ vIndices.push_back( b );
+ vIndices.push_back( c );
+
+ vIndices.push_back( a );
+ vIndices.push_back( c );
+ vIndices.push_back( d );
+ }
+ }
+ m_uiIndexSize = vIndices.size();
+
+ glGenVertexArrays( 1, &m_unLensVAO );
+ glBindVertexArray( m_unLensVAO );
+
+ glGenBuffers( 1, &m_glIDVertBuffer );
+ glBindBuffer( GL_ARRAY_BUFFER, m_glIDVertBuffer );
+ glBufferData( GL_ARRAY_BUFFER, vVerts.size()*sizeof(VertexDataLens), &vVerts[0], GL_STATIC_DRAW );
+
+ glGenBuffers( 1, &m_glIDIndexBuffer );
+ glBindBuffer( GL_ELEMENT_ARRAY_BUFFER, m_glIDIndexBuffer );
+ glBufferData( GL_ELEMENT_ARRAY_BUFFER, vIndices.size()*sizeof(GLushort), &vIndices[0], GL_STATIC_DRAW );
+
+ glEnableVertexAttribArray( 0 );
+ glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, sizeof(VertexDataLens), (void *)offsetof( VertexDataLens, position ) );
+
+ glEnableVertexAttribArray( 1 );
+ glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, sizeof(VertexDataLens), (void *)offsetof( VertexDataLens, texCoordRed ) );
+
+ glEnableVertexAttribArray(2);
+ glVertexAttribPointer(2, 2, GL_FLOAT, GL_FALSE, sizeof(VertexDataLens), (void *)offsetof( VertexDataLens, texCoordGreen ) );
+
+ glEnableVertexAttribArray(3);
+ glVertexAttribPointer(3, 2, GL_FLOAT, GL_FALSE, sizeof(VertexDataLens), (void *)offsetof( VertexDataLens, texCoordBlue ) );
+
+ glBindVertexArray( 0 );
+
+ glDisableVertexAttribArray(0);
+ glDisableVertexAttribArray(1);
+ glDisableVertexAttribArray(2);
+ glDisableVertexAttribArray(3);
+
+ glBindBuffer(GL_ARRAY_BUFFER, 0);
+ glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+void CMainApplication::RenderStereoTargets()
+{
+ B3_PROFILE("CMainApplication::RenderStereoTargets");
+
+ sExample->stepSimulation(1./60.);
+
+ glClearColor( 0.15f, 0.15f, 0.18f, 1.0f ); // nice background color, but not black
+ glEnable( GL_MULTISAMPLE );
+
+
+ m_app->m_instancingRenderer->init();
+
+
+ Matrix4 rotYtoZ = rotYtoZ.identity();
+
+ //some Bullet apps (especially robotics related) require Z as up-axis)
+ if (m_app->getUpAxis()==2)
+ {
+ rotYtoZ.rotateX(-90);
+ }
+
+
+
+ // Left Eye
+ {
+
+ Matrix4 viewMatLeft = m_mat4eyePosLeft * m_mat4HMDPose * rotYtoZ;
+ Matrix4 viewMatCenter = m_mat4HMDPose * rotYtoZ;
+ //0,1,2,3
+ //4,5,6,7,
+ //8,9,10,11
+ //12,13,14,15
+
+ //m_mat4eyePosLeft.get()[10]
+ //m_app->m_instancingRenderer->getActiveCamera()->setCameraTargetPosition(
+ // m_mat4eyePosLeft.get()[3],
+ // m_mat4eyePosLeft.get()[7],
+ // m_mat4eyePosLeft.get()[11]);
+ Matrix4 m;
+ m = viewMatCenter;
+ const float* mat = m.invertAffine().get();
+
+ /*printf("camera:\n,%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f",
+ mat[0],mat[1],mat[2],mat[3],
+ mat[4],mat[5],mat[6],mat[7],
+ mat[8],mat[9],mat[10],mat[11],
+ mat[12],mat[13],mat[14],mat[15]);
+ */
+ float dist=1;
+ m_app->m_instancingRenderer->getActiveCamera()->setCameraTargetPosition(
+ mat[12]-dist*mat[8],
+ mat[13]-dist*mat[9],
+ mat[14]-dist*mat[10]
+ );
+ m_app->m_instancingRenderer->getActiveCamera()->setCameraUpVector(mat[0],mat[1],mat[2]);
+ m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatLeft.get(),m_mat4ProjectionLeft.get());
+ m_app->m_instancingRenderer->updateCamera(m_app->getUpAxis());
+ }
+
+ glBindFramebuffer( GL_FRAMEBUFFER, leftEyeDesc.m_nRenderFramebufferId );
+ glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
+
+
+
+
+ m_app->m_window->startRendering();
+
+
+ RenderScene( vr::Eye_Left );
+
+
+
+
+
+ m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)leftEyeDesc.m_nRenderFramebufferId);
+
+ if (gDebugDrawFlags)
+ {
+ sExample->physicsDebugDraw(gDebugDrawFlags);
+ }
+ //else
+ {
+ sExample->renderScene();
+ }
+
+ //m_app->m_instancingRenderer->renderScene();
+ DrawGridData gridUp;
+ gridUp.upAxis = m_app->getUpAxis();
+// m_app->drawGrid(gridUp);
+
+
+ glBindFramebuffer( GL_FRAMEBUFFER, 0 );
+
+ glDisable( GL_MULTISAMPLE );
+
+ glBindFramebuffer(GL_READ_FRAMEBUFFER, leftEyeDesc.m_nRenderFramebufferId);
+ glBindFramebuffer(GL_DRAW_FRAMEBUFFER, leftEyeDesc.m_nResolveFramebufferId );
+
+ glBlitFramebuffer( 0, 0, m_nRenderWidth, m_nRenderHeight, 0, 0, m_nRenderWidth, m_nRenderHeight,
+ GL_COLOR_BUFFER_BIT,
+ GL_LINEAR );
+
+ glBindFramebuffer(GL_READ_FRAMEBUFFER, 0);
+ glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0 );
+
+ glEnable( GL_MULTISAMPLE );
+
+ // Right Eye
+
+
+
+ {
+ Matrix4 viewMatRight = m_mat4eyePosRight * m_mat4HMDPose * rotYtoZ;
+ m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatRight.get(),m_mat4ProjectionRight.get());
+ m_app->m_instancingRenderer->updateCamera(m_app->getUpAxis());
+ }
+
+ glBindFramebuffer( GL_FRAMEBUFFER, rightEyeDesc.m_nRenderFramebufferId );
+ glViewport(0, 0, m_nRenderWidth, m_nRenderHeight );
+
+ m_app->m_window->startRendering();
+
+ RenderScene( vr::Eye_Right );
+
+ m_app->m_instancingRenderer->setRenderFrameBuffer((unsigned int)rightEyeDesc.m_nRenderFramebufferId);
+ //m_app->m_renderer->renderScene();
+
+ if (gDebugDrawFlags)
+ {
+ sExample->physicsDebugDraw(gDebugDrawFlags);
+ }
+ //else
+ {
+ sExample->renderScene();
+ }
+
+ //m_app->drawGrid(gridUp);
+
+ glBindFramebuffer( GL_FRAMEBUFFER, 0 );
+
+ glDisable( GL_MULTISAMPLE );
+
+ glBindFramebuffer(GL_READ_FRAMEBUFFER, rightEyeDesc.m_nRenderFramebufferId );
+ glBindFramebuffer(GL_DRAW_FRAMEBUFFER, rightEyeDesc.m_nResolveFramebufferId );
+
+ glBlitFramebuffer( 0, 0, m_nRenderWidth, m_nRenderHeight, 0, 0, m_nRenderWidth, m_nRenderHeight,
+ GL_COLOR_BUFFER_BIT,
+ GL_LINEAR );
+
+ glBindFramebuffer(GL_READ_FRAMEBUFFER, 0);
+ glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0 );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+void CMainApplication::RenderScene( vr::Hmd_Eye nEye )
+{
+ B3_PROFILE("RenderScene");
+
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+ glEnable(GL_DEPTH_TEST);
+
+ if( m_bShowCubes )
+ {
+ glUseProgram( m_unSceneProgramID );
+ glUniformMatrix4fv( m_nSceneMatrixLocation, 1, GL_FALSE, GetCurrentViewProjectionMatrix( nEye ).get() );
+ glBindVertexArray( m_unSceneVAO );
+ glBindTexture( GL_TEXTURE_2D, m_iTexture );
+ glDrawArrays( GL_TRIANGLES, 0, m_uiVertcount );
+ glBindVertexArray( 0 );
+ }
+
+ bool bIsInputCapturedByAnotherProcess = m_pHMD->IsInputFocusCapturedByAnotherProcess();
+
+ if( !bIsInputCapturedByAnotherProcess )
+ {
+ // draw the controller axis lines
+ glUseProgram( m_unControllerTransformProgramID );
+ glUniformMatrix4fv( m_nControllerMatrixLocation, 1, GL_FALSE, GetCurrentViewProjectionMatrix( nEye ).get() );
+ glBindVertexArray( m_unControllerVAO );
+ glDrawArrays( GL_LINES, 0, m_uiControllerVertcount );
+ glBindVertexArray( 0 );
+ }
+
+ // ----- Render Model rendering -----
+ glUseProgram( m_unRenderModelProgramID );
+
+ for( uint32_t unTrackedDevice = 0; unTrackedDevice < vr::k_unMaxTrackedDeviceCount; unTrackedDevice++ )
+ {
+ if( !m_rTrackedDeviceToRenderModel[ unTrackedDevice ] || !m_rbShowTrackedDevice[ unTrackedDevice ] )
+ continue;
+
+ const vr::TrackedDevicePose_t & pose = m_rTrackedDevicePose[ unTrackedDevice ];
+ if( !pose.bPoseIsValid )
+ continue;
+
+ if( bIsInputCapturedByAnotherProcess && m_pHMD->GetTrackedDeviceClass( unTrackedDevice ) == vr::TrackedDeviceClass_Controller )
+ continue;
+
+ const Matrix4 & matDeviceToTracking = m_rmat4DevicePose[ unTrackedDevice ];
+ Matrix4 matMVP = GetCurrentViewProjectionMatrix( nEye ) * matDeviceToTracking;
+ glUniformMatrix4fv( m_nRenderModelMatrixLocation, 1, GL_FALSE, matMVP.get() );
+
+ m_rTrackedDeviceToRenderModel[ unTrackedDevice ]->Draw();
+ }
+
+ glUseProgram( 0 );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+void CMainApplication::RenderDistortion()
+{
+ glDisable(GL_DEPTH_TEST);
+ glViewport( 0, 0, m_nWindowWidth, m_nWindowHeight );
+
+ glBindVertexArray( m_unLensVAO );
+ glUseProgram( m_unLensProgramID );
+
+ //render left lens (first half of index array )
+ glBindTexture(GL_TEXTURE_2D, leftEyeDesc.m_nResolveTextureId );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR );
+ glDrawElements( GL_TRIANGLES, m_uiIndexSize/2, GL_UNSIGNED_SHORT, 0 );
+
+ //render right lens (second half of index array )
+ glBindTexture(GL_TEXTURE_2D, rightEyeDesc.m_nResolveTextureId );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR );
+ glDrawElements( GL_TRIANGLES, m_uiIndexSize/2, GL_UNSIGNED_SHORT, (const void *)(m_uiIndexSize) );
+
+ glBindVertexArray( 0 );
+ glUseProgram( 0 );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+Matrix4 CMainApplication::GetHMDMatrixProjectionEye( vr::Hmd_Eye nEye )
+{
+ if ( !m_pHMD )
+ return Matrix4();
+
+ vr::HmdMatrix44_t mat = m_pHMD->GetProjectionMatrix( nEye, m_fNearClip, m_fFarClip, vr::API_OpenGL);
+
+ return Matrix4(
+ mat.m[0][0], mat.m[1][0], mat.m[2][0], mat.m[3][0],
+ mat.m[0][1], mat.m[1][1], mat.m[2][1], mat.m[3][1],
+ mat.m[0][2], mat.m[1][2], mat.m[2][2], mat.m[3][2],
+ mat.m[0][3], mat.m[1][3], mat.m[2][3], mat.m[3][3]
+ );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+Matrix4 CMainApplication::GetHMDMatrixPoseEye( vr::Hmd_Eye nEye )
+{
+ if ( !m_pHMD )
+ return Matrix4();
+
+ vr::HmdMatrix34_t matEyeRight = m_pHMD->GetEyeToHeadTransform( nEye );
+ Matrix4 matrixObj(
+ matEyeRight.m[0][0], matEyeRight.m[1][0], matEyeRight.m[2][0], 0.0,
+ matEyeRight.m[0][1], matEyeRight.m[1][1], matEyeRight.m[2][1], 0.0,
+ matEyeRight.m[0][2], matEyeRight.m[1][2], matEyeRight.m[2][2], 0.0,
+ matEyeRight.m[0][3], matEyeRight.m[1][3], matEyeRight.m[2][3], 1.0f
+ );
+
+ return matrixObj.invert();
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+Matrix4 CMainApplication::GetCurrentViewProjectionMatrix( vr::Hmd_Eye nEye )
+{
+ Matrix4 matMVP;
+ if( nEye == vr::Eye_Left )
+ {
+ matMVP = m_mat4ProjectionLeft * m_mat4eyePosLeft * m_mat4HMDPose;
+ }
+ else if( nEye == vr::Eye_Right )
+ {
+ matMVP = m_mat4ProjectionRight * m_mat4eyePosRight * m_mat4HMDPose;
+ }
+
+ return matMVP;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+void CMainApplication::UpdateHMDMatrixPose()
+{
+ if (!m_pHMD)
+ return;
+ {
+ B3_PROFILE("WaitGetPoses");
+ vr::VRCompositor()->WaitGetPoses(m_rTrackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0);
+ }
+
+ m_iValidPoseCount = 0;
+ m_strPoseClasses = "";
+ {
+ B3_PROFILE("for loop");
+
+ for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
+ {
+ if (m_rTrackedDevicePose[nDevice].bPoseIsValid)
+ {
+ m_iValidPoseCount++;
+ m_rmat4DevicePose[nDevice] = ConvertSteamVRMatrixToMatrix4(m_rTrackedDevicePose[nDevice].mDeviceToAbsoluteTracking);
+ if (m_rDevClassChar[nDevice] == 0)
+ {
+ switch (m_pHMD->GetTrackedDeviceClass(nDevice))
+ {
+ case vr::TrackedDeviceClass_Controller: m_rDevClassChar[nDevice] = 'C'; break;
+ case vr::TrackedDeviceClass_HMD: m_rDevClassChar[nDevice] = 'H'; break;
+ case vr::TrackedDeviceClass_Invalid: m_rDevClassChar[nDevice] = 'I'; break;
+ case vr::TrackedDeviceClass_Other: m_rDevClassChar[nDevice] = 'O'; break;
+ case vr::TrackedDeviceClass_TrackingReference: m_rDevClassChar[nDevice] = 'T'; break;
+ default: m_rDevClassChar[nDevice] = '?'; break;
+ }
+ }
+ m_strPoseClasses += m_rDevClassChar[nDevice];
+ }
+ }
+ }
+ {
+ B3_PROFILE("m_mat4HMDPose invert");
+
+ if (m_rTrackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid)
+ {
+ m_mat4HMDPose = m_rmat4DevicePose[vr::k_unTrackedDeviceIndex_Hmd].invert();
+ }
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Finds a render model we've already loaded or loads a new one
+//-----------------------------------------------------------------------------
+CGLRenderModel *CMainApplication::FindOrLoadRenderModel( const char *pchRenderModelName )
+{
+ CGLRenderModel *pRenderModel = NULL;
+ for( std::vector< CGLRenderModel * >::iterator i = m_vecRenderModels.begin(); i != m_vecRenderModels.end(); i++ )
+ {
+ if( !stricmp( (*i)->GetName().c_str(), pchRenderModelName ) )
+ {
+ pRenderModel = *i;
+ break;
+ }
+ }
+
+ // load the model if we didn't find one
+ if( !pRenderModel )
+ {
+ vr::RenderModel_t *pModel;
+ vr::EVRRenderModelError error;
+ while ( 1 )
+ {
+ error = vr::VRRenderModels()->LoadRenderModel_Async( pchRenderModelName, &pModel );
+ if ( error != vr::VRRenderModelError_Loading )
+ break;
+
+ ThreadSleep( 1 );
+ }
+
+ if ( error != vr::VRRenderModelError_None )
+ {
+ b3Printf( "Unable to load render model %s - %s\n", pchRenderModelName, vr::VRRenderModels()->GetRenderModelErrorNameFromEnum( error ) );
+ return NULL; // move on to the next tracked device
+ }
+
+ vr::RenderModel_TextureMap_t *pTexture;
+ while ( 1 )
+ {
+ error = vr::VRRenderModels()->LoadTexture_Async( pModel->diffuseTextureId, &pTexture );
+ if ( error != vr::VRRenderModelError_Loading )
+ break;
+
+ ThreadSleep( 1 );
+ }
+
+ if ( error != vr::VRRenderModelError_None )
+ {
+ b3Printf( "Unable to load render texture id:%d for render model %s\n", pModel->diffuseTextureId, pchRenderModelName );
+ vr::VRRenderModels()->FreeRenderModel( pModel );
+ return NULL; // move on to the next tracked device
+ }
+
+ pRenderModel = new CGLRenderModel( pchRenderModelName );
+ if ( !pRenderModel->BInit( *pModel, *pTexture ) )
+ {
+ b3Printf( "Unable to create GL model from render model %s\n", pchRenderModelName );
+ delete pRenderModel;
+ pRenderModel = NULL;
+ }
+ else
+ {
+ m_vecRenderModels.push_back( pRenderModel );
+ }
+ vr::VRRenderModels()->FreeRenderModel( pModel );
+ vr::VRRenderModels()->FreeTexture( pTexture );
+ }
+ return pRenderModel;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Create/destroy GL a Render Model for a single tracked device
+//-----------------------------------------------------------------------------
+void CMainApplication::SetupRenderModelForTrackedDevice( vr::TrackedDeviceIndex_t unTrackedDeviceIndex )
+{
+ if( unTrackedDeviceIndex >= vr::k_unMaxTrackedDeviceCount )
+ return;
+
+ // try to find a model we've already set up
+ std::string sRenderModelName = GetTrackedDeviceString( m_pHMD, unTrackedDeviceIndex, vr::Prop_RenderModelName_String );
+ CGLRenderModel *pRenderModel = FindOrLoadRenderModel( sRenderModelName.c_str() );
+ if( !pRenderModel )
+ {
+ std::string sTrackingSystemName = GetTrackedDeviceString( m_pHMD, unTrackedDeviceIndex, vr::Prop_TrackingSystemName_String );
+ b3Printf( "Unable to load render model for tracked device %d (%s.%s)", unTrackedDeviceIndex, sTrackingSystemName.c_str(), sRenderModelName.c_str() );
+ }
+ else
+ {
+ m_rTrackedDeviceToRenderModel[ unTrackedDeviceIndex ] = pRenderModel;
+ m_rbShowTrackedDevice[ unTrackedDeviceIndex ] = true;
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Create/destroy GL Render Models
+//-----------------------------------------------------------------------------
+void CMainApplication::SetupRenderModels()
+{
+ memset( m_rTrackedDeviceToRenderModel, 0, sizeof( m_rTrackedDeviceToRenderModel ) );
+
+ if( !m_pHMD )
+ return;
+
+ for( uint32_t unTrackedDevice = vr::k_unTrackedDeviceIndex_Hmd + 1; unTrackedDevice < vr::k_unMaxTrackedDeviceCount; unTrackedDevice++ )
+ {
+ if( !m_pHMD->IsTrackedDeviceConnected( unTrackedDevice ) )
+ continue;
+
+ SetupRenderModelForTrackedDevice( unTrackedDevice );
+ }
+
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Converts a SteamVR matrix to our local matrix class
+//-----------------------------------------------------------------------------
+Matrix4 CMainApplication::ConvertSteamVRMatrixToMatrix4( const vr::HmdMatrix34_t &matPose )
+{
+ Matrix4 matrixObj(
+ matPose.m[0][0], matPose.m[1][0], matPose.m[2][0], 0.0,
+ matPose.m[0][1], matPose.m[1][1], matPose.m[2][1], 0.0,
+ matPose.m[0][2], matPose.m[1][2], matPose.m[2][2], 0.0,
+ matPose.m[0][3], matPose.m[1][3], matPose.m[2][3], 1.0f
+ );
+ return matrixObj;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Create/destroy GL Render Models
+//-----------------------------------------------------------------------------
+CGLRenderModel::CGLRenderModel( const std::string & sRenderModelName )
+ : m_sModelName( sRenderModelName )
+{
+ m_glIndexBuffer = 0;
+ m_glVertArray = 0;
+ m_glVertBuffer = 0;
+ m_glTexture = 0;
+}
+
+
+CGLRenderModel::~CGLRenderModel()
+{
+ Cleanup();
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Allocates and populates the GL resources for a render model
+//-----------------------------------------------------------------------------
+bool CGLRenderModel::BInit( const vr::RenderModel_t & vrModel, const vr::RenderModel_TextureMap_t & vrDiffuseTexture )
+{
+ // create and bind a VAO to hold state for this model
+ glGenVertexArrays( 1, &m_glVertArray );
+ glBindVertexArray( m_glVertArray );
+
+ // Populate a vertex buffer
+ glGenBuffers( 1, &m_glVertBuffer );
+ glBindBuffer( GL_ARRAY_BUFFER, m_glVertBuffer );
+ glBufferData( GL_ARRAY_BUFFER, sizeof( vr::RenderModel_Vertex_t ) * vrModel.unVertexCount, vrModel.rVertexData, GL_STATIC_DRAW );
+
+ // Identify the components in the vertex buffer
+ glEnableVertexAttribArray( 0 );
+ glVertexAttribPointer( 0, 3, GL_FLOAT, GL_FALSE, sizeof( vr::RenderModel_Vertex_t ), (void *)offsetof( vr::RenderModel_Vertex_t, vPosition ) );
+ glEnableVertexAttribArray( 1 );
+ glVertexAttribPointer( 1, 3, GL_FLOAT, GL_FALSE, sizeof( vr::RenderModel_Vertex_t ), (void *)offsetof( vr::RenderModel_Vertex_t, vNormal ) );
+ glEnableVertexAttribArray( 2 );
+ glVertexAttribPointer( 2, 2, GL_FLOAT, GL_FALSE, sizeof( vr::RenderModel_Vertex_t ), (void *)offsetof( vr::RenderModel_Vertex_t, rfTextureCoord ) );
+
+ // Create and populate the index buffer
+ glGenBuffers( 1, &m_glIndexBuffer );
+ glBindBuffer( GL_ELEMENT_ARRAY_BUFFER, m_glIndexBuffer );
+ glBufferData( GL_ELEMENT_ARRAY_BUFFER, sizeof( uint16_t ) * vrModel.unTriangleCount * 3, vrModel.rIndexData, GL_STATIC_DRAW );
+
+ glBindVertexArray( 0 );
+
+ // create and populate the texture
+ glGenTextures(1, &m_glTexture );
+ glBindTexture( GL_TEXTURE_2D, m_glTexture );
+
+ glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, vrDiffuseTexture.unWidth, vrDiffuseTexture.unHeight,
+ 0, GL_RGBA, GL_UNSIGNED_BYTE, vrDiffuseTexture.rubTextureMapData );
+
+ // If this renders black ask McJohn what's wrong.
+ glGenerateMipmap(GL_TEXTURE_2D);
+
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
+ glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR );
+
+ GLfloat fLargest;
+ glGetFloatv( GL_MAX_TEXTURE_MAX_ANISOTROPY_EXT, &fLargest );
+ glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAX_ANISOTROPY_EXT, fLargest );
+
+ glBindTexture( GL_TEXTURE_2D, 0 );
+
+ m_unVertexCount = vrModel.unTriangleCount * 3;
+
+ return true;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Frees the GL resources for a render model
+//-----------------------------------------------------------------------------
+void CGLRenderModel::Cleanup()
+{
+ if( m_glVertBuffer )
+ {
+ glDeleteBuffers(1, &m_glIndexBuffer);
+ glDeleteVertexArrays( 1, &m_glVertArray );
+ glDeleteBuffers(1, &m_glVertBuffer);
+ m_glIndexBuffer = 0;
+ m_glVertArray = 0;
+ m_glVertBuffer = 0;
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Draws the render model
+//-----------------------------------------------------------------------------
+void CGLRenderModel::Draw()
+{
+ glBindVertexArray( m_glVertArray );
+
+ glActiveTexture( GL_TEXTURE0 );
+ glBindTexture( GL_TEXTURE_2D, m_glTexture );
+
+ glDrawElements( GL_TRIANGLES, m_unVertexCount, GL_UNSIGNED_SHORT, 0 );
+
+ glBindVertexArray( 0 );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+int main(int argc, char *argv[])
+{
+#ifdef BT_USE_CUSTOM_PROFILER
+ //b3SetCustomEnterProfileZoneFunc(...);
+ //b3SetCustomLeaveProfileZoneFunc(...);
+#endif
+
+
+ CMainApplication *pMainApplication = new CMainApplication( argc, argv );
+
+ if (!pMainApplication->BInit())
+ {
+ pMainApplication->Shutdown();
+ return 1;
+ }
+
+ if (sExample)
+ {
+ sExample->processCommandLineArgs(argc,argv);
+ }
+
+ //request disable VSYNC
+ typedef bool (APIENTRY *PFNWGLSWAPINTERVALFARPROC)(int);
+ PFNWGLSWAPINTERVALFARPROC wglSwapIntervalEXT = 0;
+ wglSwapIntervalEXT =
+ (PFNWGLSWAPINTERVALFARPROC)wglGetProcAddress("wglSwapIntervalEXT");
+ if (wglSwapIntervalEXT)
+ wglSwapIntervalEXT(0);
+
+ pMainApplication->RunMainLoop();
+
+ pMainApplication->Shutdown();
+
+#ifdef BT_USE_CUSTOM_PROFILER
+//...
+#endif
+
+ return 0;
+}
+#endif //BT_ENABLE_VR
\ No newline at end of file
diff --git a/examples/BasicDemo/main.cpp b/examples/StandaloneMain/main_console_single_example.cpp
similarity index 64%
copy from examples/BasicDemo/main.cpp
copy to examples/StandaloneMain/main_console_single_example.cpp
index c6039fc..5764e28 100644
--- a/examples/BasicDemo/main.cpp
+++ b/examples/StandaloneMain/main_console_single_example.cpp
@@ -14,28 +14,37 @@ subject to the following restrictions:
*/
-#include "BasicExample.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btHashMap.h"
int main(int argc, char* argv[])
{
-
- DummyGUIHelper noGfx;
-
- CommonExampleOptions options(&noGfx);
- CommonExampleInterface* example = BasicExampleCreateFunc(options);
-
- example->initPhysics();
- example->stepSimulation(1.f/60.f);
- example->exitPhysics();
-
- delete example;
-
+ {
+ DummyGUIHelper noGfx;
+
+ CommonExampleOptions options(&noGfx);
+ CommonExampleInterface* example = StandaloneExampleCreateFunc(options);
+
+ example->initPhysics();
+ for (int i = 0; i < 1000; i++)
+ {
+ printf("Simulating step %d\n", i);
+ example->stepSimulation(1.f / 60.f);
+ }
+ example->exitPhysics();
+
+ delete example;
+ }
return 0;
}
+
diff --git a/examples/StandaloneMain/main_opengl_single_example.cpp b/examples/StandaloneMain/main_opengl_single_example.cpp
new file mode 100644
index 0000000..8bf9761
--- /dev/null
+++ b/examples/StandaloneMain/main_opengl_single_example.cpp
@@ -0,0 +1,116 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../Utils/b3Clock.h"
+
+
+
+
+#include "../OpenGLWindow/SimpleOpenGL3App.h"
+#include <stdio.h>
+#include "../ExampleBrowser/OpenGLGuiHelper.h"
+
+CommonExampleInterface* example;
+int gSharedMemoryKey=-1;
+
+b3MouseMoveCallback prevMouseMoveCallback = 0;
+static void OnMouseMove( float x, float y)
+{
+ bool handled = false;
+ handled = example->mouseMoveCallback(x,y);
+ if (!handled)
+ {
+ if (prevMouseMoveCallback)
+ prevMouseMoveCallback (x,y);
+ }
+}
+
+b3MouseButtonCallback prevMouseButtonCallback = 0;
+static void OnMouseDown(int button, int state, float x, float y) {
+ bool handled = false;
+
+ handled = example->mouseButtonCallback(button, state, x,y);
+ if (!handled)
+ {
+ if (prevMouseButtonCallback )
+ prevMouseButtonCallback (button,state,x,y);
+ }
+}
+
+class LessDummyGuiHelper : public DummyGUIHelper
+{
+ CommonGraphicsApp* m_app;
+public:
+ virtual CommonGraphicsApp* getAppInterface()
+ {
+ return m_app;
+ }
+
+ LessDummyGuiHelper(CommonGraphicsApp* app)
+ :m_app(app)
+ {
+ }
+};
+int main(int argc, char* argv[])
+{
+
+ SimpleOpenGL3App* app = new SimpleOpenGL3App("Bullet Standalone Example",1024,768,true);
+
+ prevMouseButtonCallback = app->m_window->getMouseButtonCallback();
+ prevMouseMoveCallback = app->m_window->getMouseMoveCallback();
+
+ app->m_window->setMouseButtonCallback((b3MouseButtonCallback)OnMouseDown);
+ app->m_window->setMouseMoveCallback((b3MouseMoveCallback)OnMouseMove);
+
+ OpenGLGuiHelper gui(app,false);
+ //LessDummyGuiHelper gui(app);
+ //DummyGUIHelper gui;
+
+ CommonExampleOptions options(&gui);
+
+ example = StandaloneExampleCreateFunc(options);
+ example->initPhysics();
+ example->resetCamera();
+
+ b3Clock clock;
+
+ do
+ {
+ app->m_instancingRenderer->init();
+ app->m_instancingRenderer->updateCamera(app->getUpAxis());
+
+ btScalar dtSec = btScalar(clock.getTimeInSeconds());
+ example->stepSimulation(dtSec);
+ clock.reset();
+
+ example->renderScene();
+
+ DrawGridData dg;
+ dg.upAxis = app->getUpAxis();
+ app->drawGrid(dg);
+
+ app->swapBuffer();
+ } while (!app->m_window->requestedExit());
+
+ example->exitPhysics();
+ delete example;
+ delete app;
+ return 0;
+}
+
diff --git a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp
new file mode 100644
index 0000000..2ab5d39
--- /dev/null
+++ b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp
@@ -0,0 +1,298 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btHashMap.h"
+
+
+#include "../TinyRenderer/TinyRenderer.h"
+#include "../OpenGLWindow/SimpleOpenGL3App.h"
+#include <stdio.h>
+#include "../ExampleBrowser/OpenGLGuiHelper.h"
+
+class SW_And_OpenGLGuiHelper : public OpenGLGuiHelper
+{
+
+ btHashMap<btHashInt,TinyRenderObjectData*> m_swRenderObjects;
+ btHashMap<btHashInt,int> m_swInstances;
+
+ int m_swWidth;
+ int m_swHeight;
+ TGAImage m_rgbColorBuffer;
+
+ b3AlignedObjectArray<float> m_depthBuffer;
+ int m_textureHandle;
+ unsigned char* m_image;
+ GLPrimitiveRenderer* m_primRenderer;
+
+public:
+ SW_And_OpenGLGuiHelper (CommonGraphicsApp* glApp, bool useOpenGL2, int swWidth, int swHeight, GLPrimitiveRenderer* primRenderer)
+ :OpenGLGuiHelper(glApp,useOpenGL2),
+ m_swWidth(swWidth),
+ m_swHeight(swHeight),
+ m_rgbColorBuffer(swWidth,swHeight,TGAImage::RGB),
+ m_primRenderer(primRenderer)
+ {
+
+ m_depthBuffer.resize(swWidth*swHeight);
+ CommonRenderInterface* render = getRenderInterface();
+ m_image=new unsigned char[m_swWidth*m_swHeight*4];
+
+ m_textureHandle = render->registerTexture(m_image,m_swWidth,m_swHeight);
+ }
+
+ void clearBuffers(TGAColor& clearColor)
+ {
+ for(int y=0;y<m_swHeight;++y)
+ {
+ for(int x=0;x<m_swWidth;++x)
+ {
+ m_rgbColorBuffer.set(x,y,clearColor);
+ m_depthBuffer[x+y*m_swWidth] = -1e30f;
+ }
+ }
+
+ }
+
+ const TGAImage& getFrameBuffer() const
+ {
+ return m_rgbColorBuffer;
+ }
+
+ virtual ~SW_And_OpenGLGuiHelper()
+ {
+ for (int i=0;i<m_swRenderObjects.size();i++)
+ {
+ TinyRenderObjectData** d = m_swRenderObjects[i];
+ if (d && *d)
+ {
+ delete *d;
+ }
+ }
+ }
+
+ virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
+ {
+ OpenGLGuiHelper::createCollisionObjectGraphicsObject(obj,color);
+ int colIndex = obj->getUserIndex();
+ int shapeIndex = obj->getCollisionShape()->getUserIndex();
+
+ if (colIndex>=0 && shapeIndex>=0)
+ {
+ m_swInstances.insert(colIndex,shapeIndex);
+ }
+
+ }
+
+
+ virtual int registerTexture(const unsigned char* texels, int width, int height)
+ {
+ return -1;
+ }
+
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
+ {
+ int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices,primitiveType, textureId);
+ if (shapeIndex>=0)
+ {
+ TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
+ float rgbaColor[4] = {1,1,1,1};
+ swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
+ //swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
+ m_swRenderObjects.insert(shapeIndex,swObj);
+ }
+ return shapeIndex;
+ }
+
+ virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
+ {
+ int instanceId = OpenGLGuiHelper::registerGraphicsInstance(shapeIndex,position,quaternion,color,scaling);
+ return instanceId ;
+ }
+
+ virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
+ {
+ OpenGLGuiHelper::createCollisionShapeGraphicsObject(collisionShape);
+ }
+
+ virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
+ {
+ OpenGLGuiHelper::syncPhysicsToGraphics(rbWorld);
+ }
+
+ virtual void render(const btDiscreteDynamicsWorld* rbWorld)
+ {
+ OpenGLGuiHelper::render(rbWorld);
+
+ //clear the color buffer
+ TGAColor clearColor;
+ clearColor.bgra[0] = 255;
+ clearColor.bgra[1] = 255;
+ clearColor.bgra[2] = 255;
+ clearColor.bgra[3] = 255;
+
+ clearBuffers(clearColor);
+
+
+ ATTRIBUTE_ALIGNED16(btScalar modelMat[16]);
+ ATTRIBUTE_ALIGNED16(float viewMat[16]);
+ ATTRIBUTE_ALIGNED16(float projMat[16]);
+
+ CommonRenderInterface* render = getRenderInterface();
+
+ render->getActiveCamera()->getCameraProjectionMatrix(projMat);
+ render->getActiveCamera()->getCameraViewMatrix(viewMat);
+
+ btVector3 lightDirWorld(-5,200,-40);
+ switch (1)//app->getUpAxis())
+ {
+ case 1:
+ lightDirWorld = btVector3(-50.f,100,30);
+ break;
+ case 2:
+ lightDirWorld = btVector3(-50.f,30,100);
+ break;
+ default:{}
+ };
+
+ lightDirWorld.normalize();
+
+
+ for (int i=0;i<rbWorld->getNumCollisionObjects();i++)
+ {
+ btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
+ int colObjIndex = colObj->getUserIndex();
+ int shapeIndex = colObj->getCollisionShape()->getUserIndex();
+ if (colObjIndex>=0 && shapeIndex>=0)
+ {
+
+ TinyRenderObjectData* renderObj = 0;
+
+ int* cptr = m_swInstances[colObjIndex];
+ if (cptr)
+ {
+ int c = *cptr;
+ TinyRenderObjectData** sptr = m_swRenderObjects[c];
+ if (sptr)
+ {
+ renderObj = *sptr;
+ //sync the object transform
+ const btTransform& tr = colObj->getWorldTransform();
+ tr.getOpenGLMatrix(modelMat);
+
+ for (int i=0;i<4;i++)
+ {
+ for (int j=0;j<4;j++)
+ {
+
+ renderObj->m_projectionMatrix[i][j] = projMat[i+4*j];
+ renderObj->m_modelMatrix[i][j] = modelMat[i+4*j];
+ renderObj->m_viewMatrix[i][j] = viewMat[i+4*j];
+ renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling();
+ renderObj->m_lightDirWorld = lightDirWorld;
+ }
+ }
+ TinyRenderer::renderObject(*renderObj);
+ }
+ }
+ }
+ }
+
+ for(int y=0;y<m_swHeight;++y)
+ {
+ unsigned char* pi=m_image+(y)*m_swWidth*3;
+ for(int x=0;x<m_swWidth;++x)
+ {
+
+ const TGAColor& color = getFrameBuffer().get(x,y);
+ pi[0] = color.bgra[2];
+ pi[1] = color.bgra[1];
+ pi[2] = color.bgra[0];
+ pi+=3;
+
+ }
+ }
+ render->activateTexture(m_textureHandle);
+ render->updateTexture(m_textureHandle,m_image);
+
+ static int counter=0;
+ counter++;
+ if ((counter&7)==0)
+ {
+
+ char filename[1024];
+ sprintf(filename,"framebuf%d.tga",counter);
+ getFrameBuffer().write_tga_file(filename,true);
+ }
+ float color[4] = {1,1,1,1};
+ m_primRenderer->drawTexturedRect(0,0,m_swWidth, m_swHeight,color,0,0,1,1,true);
+
+
+ }
+
+ virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
+ {
+ OpenGLGuiHelper::autogenerateGraphicsObjects(rbWorld);
+ }
+
+};
+
+int main(int argc, char* argv[])
+{
+
+ SimpleOpenGL3App* app = new SimpleOpenGL3App("Standalone Example (Software Renderer, TinyRenderer)",1024,768,true);
+ int textureWidth = 640;
+ int textureHeight = 480;
+
+
+ SW_And_OpenGLGuiHelper gui(app,false,textureWidth,textureHeight,app->m_primRenderer);
+
+ CommonExampleOptions options(&gui);
+ CommonExampleInterface* example = StandaloneExampleCreateFunc(options);
+
+ example->initPhysics();
+ example->resetCamera();
+ do
+ {
+ app->m_instancingRenderer->init();
+ app->m_instancingRenderer->updateCamera(app->getUpAxis());
+
+ example->stepSimulation(1./60.);
+
+
+ example->renderScene();
+
+ DrawGridData dg;
+ dg.upAxis = app->getUpAxis();
+ app->drawGrid(dg);
+ app->swapBuffer();
+ } while (!app->m_window->requestedExit());
+
+ example->exitPhysics();
+ delete example;
+ delete app;
+ return 0;
+}
+
+
diff --git a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp
new file mode 100644
index 0000000..6ca0efa
--- /dev/null
+++ b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp
@@ -0,0 +1,409 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "../ExampleBrowser/CollisionShape2TriangleMesh.h"
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btHashMap.h"
+
+#include "../TinyRenderer/TinyRenderer.h"
+#include "../OpenGLWindow/SimpleCamera.h"
+
+static btVector4 sMyColors[4] =
+{
+ btVector4(0.3,0.3,1,1),
+ btVector4(0.6,0.6,1,1),
+ btVector4(0,1,0,1),
+ btVector4(0,1,1,1),
+ //btVector4(1,1,0,1),
+};
+
+struct TinyRendererTexture
+{
+ const unsigned char* m_texels;
+ int m_width;
+ int m_height;
+};
+
+struct TinyRendererGUIHelper : public GUIHelperInterface
+{
+
+ int m_upAxis;
+
+ btHashMap<btHashInt,TinyRenderObjectData*> m_swRenderObjects;
+ btHashMap<btHashInt,int> m_swInstances;
+ btHashMap<btHashInt,TinyRendererTexture> m_textures;
+
+ int m_swWidth;
+ int m_swHeight;
+ TGAImage m_rgbColorBuffer;
+
+ b3AlignedObjectArray<float> m_depthBuffer;
+
+ SimpleCamera m_camera;
+ int m_colObjUniqueIndex;
+
+ TinyRendererGUIHelper( int swWidth, int swHeight)
+ : m_upAxis(1),
+ m_swWidth(swWidth),
+ m_swHeight(swHeight),
+ m_rgbColorBuffer(swWidth,swHeight,TGAImage::RGB),
+ m_colObjUniqueIndex(0)
+ {
+ m_depthBuffer.resize(swWidth*swHeight);
+ }
+
+ void clearBuffers(TGAColor& clearColor)
+ {
+ for(int y=0;y<m_swHeight;++y)
+ {
+ for(int x=0;x<m_swWidth;++x)
+ {
+ m_rgbColorBuffer.set(x,y,clearColor);
+ m_depthBuffer[x+y*m_swWidth] = -1e30f;
+ }
+ }
+
+ }
+
+ const TGAImage& getFrameBuffer() const
+ {
+ return m_rgbColorBuffer;
+ }
+
+ virtual ~TinyRendererGUIHelper()
+ {
+ for (int i=0;i<m_swRenderObjects.size();i++)
+ {
+ TinyRenderObjectData** d = m_swRenderObjects[i];
+ if (d && *d)
+ {
+ delete *d;
+ }
+ }
+ }
+
+
+
+ virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){}
+
+ virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
+ {
+ if (obj->getUserIndex()<0)
+ {
+ int colIndex = m_colObjUniqueIndex++;
+ obj->setUserIndex(colIndex);
+ int shapeIndex = obj->getCollisionShape()->getUserIndex();
+
+ if (colIndex>=0 && shapeIndex>=0)
+ {
+ m_swInstances.insert(colIndex,shapeIndex);
+ }
+ }
+ }
+
+ virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
+ {
+ //already has a graphics object?
+ if (collisionShape->getUserIndex()>=0)
+ return;
+
+
+ btAlignedObjectArray<GLInstanceVertex> gfxVertices;
+
+ btAlignedObjectArray<int> indices;
+ btTransform startTrans;startTrans.setIdentity();
+
+ {
+ btAlignedObjectArray<btVector3> vertexPositions;
+ btAlignedObjectArray<btVector3> vertexNormals;
+ CollisionShape2TriangleMesh(collisionShape,startTrans,vertexPositions,vertexNormals,indices);
+ gfxVertices.resize(vertexPositions.size());
+ for (int i=0;i<vertexPositions.size();i++)
+ {
+ for (int j=0;j<4;j++)
+ {
+ gfxVertices[i].xyzw[j] = vertexPositions[i][j];
+ }
+ for (int j=0;j<3;j++)
+ {
+ gfxVertices[i].normal[j] = vertexNormals[i][j];
+ }
+ for (int j=0;j<2;j++)
+ {
+ gfxVertices[i].uv[j] = 0.5;//we don't have UV info...
+ }
+ }
+ }
+
+
+ if (gfxVertices.size() && indices.size())
+ {
+ int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size(),
+ 1,-1);
+ collisionShape->setUserIndex(shapeId);
+ }
+ }
+
+ virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld){}
+
+ virtual void render(const btDiscreteDynamicsWorld* rbWorld)
+ {
+ //clear the color buffer
+ TGAColor clearColor;
+ clearColor.bgra[0] = 255;
+ clearColor.bgra[1] = 255;
+ clearColor.bgra[2] = 255;
+ clearColor.bgra[3] = 255;
+
+ clearBuffers(clearColor);
+
+
+ ATTRIBUTE_ALIGNED16(btScalar modelMat[16]);
+ ATTRIBUTE_ALIGNED16(float viewMat[16]);
+ ATTRIBUTE_ALIGNED16(float projMat[16]);
+
+ m_camera.getCameraProjectionMatrix(projMat);
+ m_camera.getCameraViewMatrix(viewMat);
+
+ btVector3 lightDirWorld(-5,200,-40);
+ switch (m_upAxis)
+ {
+ case 1:
+ lightDirWorld = btVector3(-50.f,100,30);
+ break;
+ case 2:
+ lightDirWorld = btVector3(-50.f,30,100);
+ break;
+ default:{}
+ };
+
+ lightDirWorld.normalize();
+
+
+ for (int i=0;i<rbWorld->getNumCollisionObjects();i++)
+ {
+ btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
+ int colObjIndex = colObj->getUserIndex();
+ int shapeIndex = colObj->getCollisionShape()->getUserIndex();
+ if (colObjIndex>=0 && shapeIndex>=0)
+ {
+
+ TinyRenderObjectData* renderObj = 0;
+
+ int* cptr = m_swInstances[colObjIndex];
+ if (cptr)
+ {
+ int c = *cptr;
+ TinyRenderObjectData** sptr = m_swRenderObjects[c];
+ if (sptr)
+ {
+ renderObj = *sptr;
+ //sync the object transform
+ const btTransform& tr = colObj->getWorldTransform();
+ tr.getOpenGLMatrix(modelMat);
+
+ for (int i=0;i<4;i++)
+ {
+ for (int j=0;j<4;j++)
+ {
+
+ renderObj->m_projectionMatrix[i][j] = projMat[i+4*j];
+ renderObj->m_modelMatrix[i][j] = modelMat[i+4*j];
+ renderObj->m_viewMatrix[i][j] = viewMat[i+4*j];
+ renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling();
+ renderObj->m_lightDirWorld = lightDirWorld;
+ }
+ }
+ TinyRenderer::renderObject(*renderObj);
+ }
+ }
+ }
+ }
+
+
+ static int counter=0;
+ counter++;
+ if ((counter&7)==0)
+ {
+
+ char filename[1024];
+ sprintf(filename,"framebuf%d.tga",counter);
+ m_rgbColorBuffer.flip_vertically();
+ getFrameBuffer().write_tga_file(filename,true);
+ }
+ float color[4] = {1,1,1,1};
+
+ }
+
+ virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
+
+ virtual int registerTexture(const unsigned char* texels, int width, int height)
+ {
+ //do we need to make a copy?
+ int textureId = m_textures.size();
+ TinyRendererTexture t;
+ t.m_texels = texels;
+ t.m_width = width;
+ t.m_height = height;
+ this->m_textures.insert(textureId,t);
+ return textureId;
+ }
+
+ virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
+ {
+ int shapeIndex = m_swRenderObjects.size();
+
+ TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
+ float rgbaColor[4] = {1,1,1,1};
+
+ //if (textureId>=0)
+ //{
+ // swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
+ //} else
+ {
+ swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
+ }
+ //swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
+ m_swRenderObjects.insert(shapeIndex,swObj);
+ return shapeIndex;
+ }
+
+ virtual void removeAllGraphicsInstances()
+ {
+ }
+
+ virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
+ {
+ int colIndex = m_colObjUniqueIndex++;
+
+ if (colIndex>=0 && shapeIndex>=0)
+ {
+ TinyRenderObjectData** dPtr = m_swRenderObjects[shapeIndex];
+ if (dPtr && *dPtr)
+ {
+ TinyRenderObjectData* d= *dPtr;
+ d->m_localScaling.setValue(scaling[0],scaling[1],scaling[2]);
+ m_swInstances.insert(colIndex,shapeIndex);
+ }
+ }
+
+ return colIndex;
+ }
+
+ virtual Common2dCanvasInterface* get2dCanvasInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonParameterInterface* getParameterInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonRenderInterface* getRenderInterface()
+ {
+ return 0;
+ }
+
+ virtual CommonGraphicsApp* getAppInterface()
+ {
+ return 0;
+ }
+
+
+ virtual void setUpAxis(int axis)
+ {
+ m_upAxis = axis;
+ m_camera.setCameraUpAxis(axis);
+ m_camera.update();
+ }
+ virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
+ {
+ m_camera.setCameraDistance(camDist);
+ m_camera.setCameraPitch(pitch);
+ m_camera.setCameraYaw(yaw);
+ m_camera.setCameraTargetPosition(camPosX,camPosY,camPosZ);
+ m_camera.setAspectRatio((float)m_swWidth/(float)m_swHeight);
+ m_camera.update();
+
+ }
+
+
+ virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
+ unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
+ float* depthBuffer, int depthBufferSizeInPixels,
+ int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
+ int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)
+ {
+ if (numPixelsCopied)
+ *numPixelsCopied = 0;
+ }
+
+
+ virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
+ {
+
+ for (int i=0;i<rbWorld->getNumCollisionObjects();i++)
+ {
+ btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
+ //btRigidBody* body = btRigidBody::upcast(colObj);
+ //does this also work for btMultiBody/btMultiBodyLinkCollider?
+ createCollisionShapeGraphicsObject(colObj->getCollisionShape());
+ int colorIndex = colObj->getBroadphaseHandle()->getUid() & 3;
+
+ btVector3 color= sMyColors[colorIndex];
+ createCollisionObjectGraphicsObject(colObj,color);
+
+ }
+ }
+
+ virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
+ {
+ }
+};
+
+int main(int argc, char* argv[])
+{
+
+ TinyRendererGUIHelper noGfx(640,480);
+
+ CommonExampleOptions options(&noGfx);
+ CommonExampleInterface* example = StandaloneExampleCreateFunc(options);
+
+ example->initPhysics();
+ example->resetCamera();
+
+ for (int i=0;i<1000;i++)
+ {
+ printf("Simulating step %d\n",i);
+ example->stepSimulation(1.f/60.f);
+ example->renderScene();
+ }
+ example->exitPhysics();
+
+ delete example;
+
+ return 0;
+}
+
+
diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/TinyRenderer/LICENSE.txt
similarity index 63%
copy from examples/InverseDynamics/InverseDynamicsExample.h
copy to examples/TinyRenderer/LICENSE.txt
index 13c37a4..95dbe5b 100644
--- a/examples/InverseDynamics/InverseDynamicsExample.h
+++ b/examples/TinyRenderer/LICENSE.txt
@@ -1,28 +1,13 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2015 Google Inc. http://bulletphysics.org
+Tiny Renderer, https://github.com/ssloy/tinyrenderer
+Copyright Dmitry V. Sokolov
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
-*/
-#ifndef INVERSE_DYNAMICS_EXAMPLE_H
-#define INVERSE_DYNAMICS_EXAMPLE_H
-
-enum btInverseDynamicsExampleOptions
-{
- BT_ID_LOAD_URDF=1,
- BT_ID_PROGRAMMATICALLY=2
-};
-
-class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options);
-
-
-#endif //INVERSE_DYNAMICS_EXAMPLE_H
diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp
new file mode 100644
index 0000000..0a5b7c8
--- /dev/null
+++ b/examples/TinyRenderer/TinyRenderer.cpp
@@ -0,0 +1,295 @@
+#include "TinyRenderer.h"
+
+#include <vector>
+#include <limits>
+#include <iostream>
+#include "tgaimage.h"
+#include "model.h"
+#include "geometry.h"
+#include "our_gl.h"
+#include "../Utils/b3ResourcePath.h"
+#include "Bullet3Common/b3MinMax.h"
+#include "../OpenGLWindow/ShapeData.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btVector3.h"
+
+
+
+struct Shader : public IShader {
+
+ Model* m_model;
+ Vec3f m_light_dir_local;
+ Matrix& m_modelMat;
+ Matrix m_invModelMat;
+
+ Matrix& m_modelView1;
+ Matrix& m_projectionMatrix;
+ Vec3f m_localScaling;
+ Vec4f m_colorRGBA;
+
+ mat<2,3,float> varying_uv; // triangle uv coordinates, written by the vertex shader, read by the fragment shader
+ mat<4,3,float> varying_tri; // triangle coordinates (clip coordinates), written by VS, read by FS
+ mat<3,3,float> varying_nrm; // normal per vertex to be interpolated by FS
+ //mat<3,3,float> ndc_tri; // triangle in normalized device coordinates
+
+ Shader(Model* model, Vec3f light_dir_local, Matrix& modelView, Matrix& projectionMatrix, Matrix& modelMat, Vec3f localScaling, const Vec4f& colorRGBA)
+ :m_model(model),
+ m_light_dir_local(light_dir_local),
+ m_modelView1(modelView),
+ m_projectionMatrix(projectionMatrix),
+ m_modelMat(modelMat),
+ m_localScaling(localScaling),
+ m_colorRGBA(colorRGBA)
+ {
+ m_invModelMat = m_modelMat.invert_transpose();
+ }
+
+ virtual Vec4f vertex(int iface, int nthvert) {
+ Vec2f uv = m_model->uv(iface, nthvert);
+ varying_uv.set_col(nthvert, uv);
+ //varying_nrm.set_col(nthvert, proj<3>((m_projectionMatrix*m_modelView).invert_transpose()*embed<4>(m_model->normal(iface, nthvert), 0.f)));
+ varying_nrm.set_col(nthvert, proj<3>(m_invModelMat*embed<4>(m_model->normal(iface, nthvert), 0.f)));
+ //m_localNormal = m_model->normal(iface, nthvert);
+ //varying_nrm.set_col(nthvert, m_model->normal(iface, nthvert));
+ Vec3f unScaledVert = m_model->vert(iface, nthvert);
+
+ Vec3f scaledVert=Vec3f(unScaledVert[0]*m_localScaling[0],
+ unScaledVert[1]*m_localScaling[1],
+ unScaledVert[2]*m_localScaling[2]);
+
+ Vec4f gl_Vertex = m_projectionMatrix*m_modelView1*embed<4>(scaledVert);
+ varying_tri.set_col(nthvert, gl_Vertex);
+ return gl_Vertex;
+ }
+
+ virtual bool fragment(Vec3f bar, TGAColor &color) {
+ Vec3f bn = (varying_nrm*bar).normalize();
+ Vec2f uv = varying_uv*bar;
+
+ Vec3f reflection_direction = (bn * (bn * m_light_dir_local * 2.f) - m_light_dir_local).normalize();
+ float specular = pow(b3Max(reflection_direction.z, 0.f), m_model->specular(uv));
+ float diffuse = b3Max(0.f, bn * m_light_dir_local);
+
+ float ambient_coefficient = 0.6;
+ float diffuse_coefficient = 0.35;
+ float specular_coefficient = 0.05;
+
+ float intensity = ambient_coefficient + b3Min(diffuse * diffuse_coefficient + specular * specular_coefficient, 1.0f - ambient_coefficient);
+
+ color = m_model->diffuse(uv) * intensity;
+
+ //warning: bgra color is swapped to rgba to upload texture
+ color.bgra[0] *= m_colorRGBA[0];
+ color.bgra[1] *= m_colorRGBA[1];
+ color.bgra[2] *= m_colorRGBA[2];
+ color.bgra[3] *= m_colorRGBA[3];
+
+ return false;
+ }
+};
+
+TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
+:m_rgbColorBuffer(rgbColorBuffer),
+m_depthBuffer(depthBuffer),
+m_segmentationMaskBufferPtr(0),
+m_model(0),
+m_userData(0),
+m_userIndex(-1),
+m_objectIndex(-1)
+{
+ Vec3f eye(1,1,3);
+ Vec3f center(0,0,0);
+ Vec3f up(0,0,1);
+ m_lightDirWorld.setValue(0,0,0);
+ m_localScaling.setValue(1,1,1);
+ m_modelMatrix = Matrix::identity();
+
+}
+
+
+
+TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex)
+:m_rgbColorBuffer(rgbColorBuffer),
+m_depthBuffer(depthBuffer),
+m_segmentationMaskBufferPtr(segmentationMaskBuffer),
+m_model(0),
+m_userData(0),
+m_userIndex(-1),
+m_objectIndex(objectIndex)
+{
+ Vec3f eye(1,1,3);
+ Vec3f center(0,0,0);
+ Vec3f up(0,0,1);
+ m_lightDirWorld.setValue(0,0,0);
+ m_localScaling.setValue(1,1,1);
+ m_modelMatrix = Matrix::identity();
+
+}
+
+void TinyRenderObjectData::loadModel(const char* fileName)
+{
+ //todo(erwincoumans) move the file loading out of here
+ char relativeFileName[1024];
+ if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
+ {
+ printf("Cannot find file %s\n", fileName);
+ } else
+ {
+ m_model = new Model(relativeFileName);
+ }
+}
+
+
+void TinyRenderObjectData::registerMeshShape(const float* vertices, int numVertices,const int* indices, int numIndices, const float rgbaColor[4],
+ unsigned char* textureImage, int textureWidth, int textureHeight)
+{
+ if (0==m_model)
+ {
+ m_model = new Model();
+ m_model->setColorRGBA(rgbaColor);
+ if (textureImage)
+ {
+ m_model->setDiffuseTextureFromData(textureImage,textureWidth,textureHeight);
+ } else
+ {
+ /*char relativeFileName[1024];
+ if (b3ResourcePath::findResourcePath("floor_diffuse.tga", relativeFileName, 1024))
+ {
+ m_model->loadDiffuseTexture(relativeFileName);
+ }
+ */
+ }
+
+ m_model->reserveMemory(numVertices,numIndices);
+ for (int i=0;i<numVertices;i++)
+ {
+ m_model->addVertex(vertices[i*9],
+ vertices[i*9+1],
+ vertices[i*9+2],
+ vertices[i*9+4],
+ vertices[i*9+5],
+ vertices[i*9+6],
+ vertices[i*9+7],
+ vertices[i*9+8]);
+ }
+ for (int i=0;i<numIndices;i+=3)
+ {
+ m_model->addTriangle(indices[i],indices[i],indices[i],
+ indices[i+1],indices[i+1],indices[i+1],
+ indices[i+2],indices[i+2],indices[i+2]);
+ }
+ }
+}
+
+void TinyRenderObjectData::registerMesh2(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals,btAlignedObjectArray<int>& indices)
+{
+ if (0==m_model)
+ {
+ int numVertices = vertices.size();
+ int numIndices = indices.size();
+
+ m_model = new Model();
+ char relativeFileName[1024];
+ if (b3ResourcePath::findResourcePath("floor_diffuse.tga", relativeFileName, 1024))
+ {
+ m_model->loadDiffuseTexture(relativeFileName);
+ }
+
+ for (int i=0;i<numVertices;i++)
+ {
+ m_model->addVertex(vertices[i].x(),
+ vertices[i].y(),
+ vertices[i].z(),
+ normals[i].x(),
+ normals[i].y(),
+ normals[i].z(),
+ 0.5,0.5);
+ }
+ for (int i=0;i<numIndices;i+=3)
+ {
+ m_model->addTriangle(indices[i],indices[i],indices[i],
+ indices[i+1],indices[i+1],indices[i+1],
+ indices[i+2],indices[i+2],indices[i+2]);
+ }
+ }
+}
+
+void TinyRenderObjectData::createCube(float halfExtentsX,float halfExtentsY,float halfExtentsZ)
+{
+ m_model = new Model();
+
+ char relativeFileName[1024];
+ if (b3ResourcePath::findResourcePath("floor_diffuse.tga", relativeFileName, 1024))
+ {
+ m_model->loadDiffuseTexture(relativeFileName);
+ }
+
+
+ int strideInBytes = 9*sizeof(float);
+ int numVertices = sizeof(cube_vertices_textured)/strideInBytes;
+ int numIndices = sizeof(cube_indices)/sizeof(int);
+
+ for (int i=0;i<numVertices;i++)
+ {
+ m_model->addVertex(halfExtentsX*cube_vertices_textured[i*9],
+ halfExtentsY*cube_vertices_textured[i*9+1],
+ halfExtentsY*cube_vertices_textured[i*9+2],
+ cube_vertices_textured[i*9+4],
+ cube_vertices_textured[i*9+5],
+ cube_vertices_textured[i*9+6],
+ cube_vertices_textured[i*9+7],
+ cube_vertices_textured[i*9+8]);
+ }
+ for (int i=0;i<numIndices;i+=3)
+ {
+ m_model->addTriangle(cube_indices[i],cube_indices[i],cube_indices[i],
+ cube_indices[i+1],cube_indices[i+1],cube_indices[i+1],
+ cube_indices[i+2],cube_indices[i+2],cube_indices[i+2]);
+ }
+
+}
+
+TinyRenderObjectData::~TinyRenderObjectData()
+{
+ delete m_model;
+}
+
+void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
+{
+ int width = renderData.m_rgbColorBuffer.get_width();
+ int height = renderData.m_rgbColorBuffer.get_height();
+
+ Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]);
+ Model* model = renderData.m_model;
+ if (0==model)
+ return;
+
+
+
+ renderData.m_viewportMatrix = viewport(0,0,width, height);
+
+ b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
+ int* segmentationMaskBufferPtr = (renderData.m_segmentationMaskBufferPtr && renderData.m_segmentationMaskBufferPtr->size())?&renderData.m_segmentationMaskBufferPtr->at(0):0;
+
+ TGAImage& frame = renderData.m_rgbColorBuffer;
+
+
+
+ {
+ Matrix modelViewMatrix = renderData.m_viewMatrix*renderData.m_modelMatrix;
+ Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]);
+ Shader shader(model, light_dir_local, modelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, localScaling, model->getColorRGBA());
+
+ //printf("Render %d triangles.\n",model->nfaces());
+ for (int i=0; i<model->nfaces(); i++)
+ {
+
+ for (int j=0; j<3; j++) {
+ shader.vertex(i, j);
+ }
+ triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
+ }
+
+ }
+
+}
diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h
new file mode 100644
index 0000000..1819790
--- /dev/null
+++ b/examples/TinyRenderer/TinyRenderer.h
@@ -0,0 +1,56 @@
+#ifndef TINY_RENDERER_H
+#define TINY_RENDERER_H
+
+#include "geometry.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btVector3.h"
+
+
+#include "tgaimage.h"
+
+struct TinyRenderObjectData
+{
+ //Camera
+ Matrix m_viewMatrix;
+ Matrix m_projectionMatrix;
+ Matrix m_viewportMatrix;
+ btVector3 m_localScaling;
+ btVector3 m_lightDirWorld;
+
+ //Model (vertices, indices, textures, shader)
+ Matrix m_modelMatrix;
+ class Model* m_model;
+ //class IShader* m_shader; todo(erwincoumans) expose the shader, for now we use a default shader
+
+ //Output
+
+ TGAImage& m_rgbColorBuffer;
+ b3AlignedObjectArray<float>& m_depthBuffer;//required, hence a reference
+ b3AlignedObjectArray<int>* m_segmentationMaskBufferPtr;//optional, hence a pointer
+
+ TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer);
+ TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
+ virtual ~TinyRenderObjectData();
+
+ void loadModel(const char* fileName);
+ void createCube(float HalfExtentsX,float HalfExtentsY,float HalfExtentsZ);
+ void registerMeshShape(const float* vertices, int numVertices,const int* indices, int numIndices, const float rgbaColor[4],
+ unsigned char* textureImage=0, int textureWidth=0, int textureHeight=0);
+
+ void registerMesh2(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& normals,btAlignedObjectArray<int>& indices);
+
+ void* m_userData;
+ int m_userIndex;
+ int m_objectIndex;
+};
+
+
+class TinyRenderer
+{
+ public:
+ static void renderObject(TinyRenderObjectData& renderData);
+};
+
+#endif // TINY_RENDERER_Hbla
diff --git a/examples/TinyRenderer/geometry.cpp b/examples/TinyRenderer/geometry.cpp
new file mode 100644
index 0000000..3b6b2f2
--- /dev/null
+++ b/examples/TinyRenderer/geometry.cpp
@@ -0,0 +1,7 @@
+#include "geometry.h"
+
+template <> template <> vec<3,int> ::vec(const vec<3,float> &v) : x(int(v.x+.5f)),y(int(v.y+.5f)),z(int(v.z+.5f)) {}
+template <> template <> vec<3,float>::vec(const vec<3,int> &v) : x(v.x),y(v.y),z(v.z) {}
+template <> template <> vec<2,int> ::vec(const vec<2,float> &v) : x(int(v.x+.5f)),y(int(v.y+.5f)) {}
+template <> template <> vec<2,float>::vec(const vec<2,int> &v) : x(v.x),y(v.y) {}
+
diff --git a/examples/TinyRenderer/geometry.h b/examples/TinyRenderer/geometry.h
new file mode 100644
index 0000000..38bb4d9
--- /dev/null
+++ b/examples/TinyRenderer/geometry.h
@@ -0,0 +1,222 @@
+#ifndef __GEOMETRY_H__
+#define __GEOMETRY_H__
+#include <cmath>
+#include <cassert>
+#include <stdlib.h>
+
+
+template<size_t DimCols,size_t DimRows,typename T> class mat;
+
+template <size_t DIM, typename T> struct vec {
+ vec() { for (size_t i=DIM; i--; data_[i] = T()); }
+ T& operator[](const size_t i) { assert(i<DIM); return data_[i]; }
+ const T& operator[](const size_t i) const { assert(i<DIM); return data_[i]; }
+private:
+ T data_[DIM];
+};
+
+/////////////////////////////////////////////////////////////////////////////////
+
+template <typename T> struct vec<2,T> {
+ vec() : x(T()), y(T()) {}
+ vec(T X, T Y) : x(X), y(Y) {}
+ template <class U> vec<2,T>(const vec<2,U> &v);
+ T& operator[](const size_t i) { assert(i<2); return i<=0 ? x : y; }
+ const T& operator[](const size_t i) const { assert(i<2); return i<=0 ? x : y; }
+
+ T x,y;
+};
+
+/////////////////////////////////////////////////////////////////////////////////
+
+template <typename T> struct vec<3,T> {
+ vec() : x(T()), y(T()), z(T()) {}
+ vec(T X, T Y, T Z) : x(X), y(Y), z(Z) {}
+ template <class U> vec<3,T>(const vec<3,U> &v);
+ T& operator[](const size_t i) { assert(i<3); return i<=0 ? x : (1==i ? y : z); }
+ const T& operator[](const size_t i) const { assert(i<3); return i<=0 ? x : (1==i ? y : z); }
+ float norm() { return std::sqrt(x*x+y*y+z*z); }
+ vec<3,T> & normalize(T l=1) { *this = (*this)*(l/norm()); return *this; }
+
+ T x,y,z;
+};
+
+/////////////////////////////////////////////////////////////////////////////////
+
+template<size_t DIM,typename T> T operator*(const vec<DIM,T>& lhs, const vec<DIM,T>& rhs) {
+ T ret = T();
+ for (size_t i=DIM; i--; ret+=lhs[i]*rhs[i]);
+ return ret;
+}
+
+
+template<size_t DIM,typename T>vec<DIM,T> operator+(vec<DIM,T> lhs, const vec<DIM,T>& rhs) {
+ for (size_t i=DIM; i--; lhs[i]+=rhs[i]);
+ return lhs;
+}
+
+template<size_t DIM,typename T>vec<DIM,T> operator-(vec<DIM,T> lhs, const vec<DIM,T>& rhs) {
+ for (size_t i=DIM; i--; lhs[i]-=rhs[i]);
+ return lhs;
+}
+
+template<size_t DIM,typename T,typename U> vec<DIM,T> operator*(vec<DIM,T> lhs, const U& rhs) {
+ for (size_t i=DIM; i--; lhs[i]*=rhs);
+ return lhs;
+}
+
+template<size_t DIM,typename T,typename U> vec<DIM,T> operator/(vec<DIM,T> lhs, const U& rhs) {
+ for (size_t i=DIM; i--; lhs[i]/=rhs);
+ return lhs;
+}
+
+template<size_t LEN,size_t DIM,typename T> vec<LEN,T> embed(const vec<DIM,T> &v, T fill=1) {
+ vec<LEN,T> ret;
+ for (size_t i=LEN; i--; ret[i]=(i<DIM?v[i]:fill));
+ return ret;
+}
+
+template<size_t LEN,size_t DIM, typename T> vec<LEN,T> proj(const vec<DIM,T> &v) {
+ vec<LEN,T> ret;
+ for (size_t i=LEN; i--; ret[i]=v[i]);
+ return ret;
+}
+
+template <typename T> vec<3,T> cross(vec<3,T> v1, vec<3,T> v2) {
+ return vec<3,T>(v1.y*v2.z - v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x);
+}
+#if 0
+template <size_t DIM, typename T> std::ostream& operator<<(std::ostream& out, vec<DIM,T>& v) {
+ for(unsigned int i=0; i<DIM; i++) {
+ out << v[i] << " " ;
+ }
+ return out ;
+}
+#endif
+/////////////////////////////////////////////////////////////////////////////////
+
+template<size_t DIM,typename T> struct dt {
+ static T det(const mat<DIM,DIM,T>& src) {
+ T ret=0;
+ for (size_t i=DIM; i--; ret += src[0][i]*src.cofactor(0,i));
+ return ret;
+ }
+};
+
+template<typename T> struct dt<1,T> {
+ static T det(const mat<1,1,T>& src) {
+ return src[0][0];
+ }
+};
+
+/////////////////////////////////////////////////////////////////////////////////
+
+template<size_t DimRows,size_t DimCols,typename T> class mat {
+ vec<DimCols,T> rows[DimRows];
+public:
+ mat() {}
+
+ vec<DimCols,T>& operator[] (const size_t idx) {
+ assert(idx<DimRows);
+ return rows[idx];
+ }
+
+ const vec<DimCols,T>& operator[] (const size_t idx) const {
+ assert(idx<DimRows);
+ return rows[idx];
+ }
+
+ vec<DimRows,T> col(const size_t idx) const {
+ assert(idx<DimCols);
+ vec<DimRows,T> ret;
+ for (size_t i=DimRows; i--; ret[i]=rows[i][idx]);
+ return ret;
+ }
+
+ void set_col(size_t idx, vec<DimRows,T> v) {
+ assert(idx<DimCols);
+ for (size_t i=DimRows; i--; rows[i][idx]=v[i]);
+ }
+
+ static mat<DimRows,DimCols,T> identity() {
+ mat<DimRows,DimCols,T> ret;
+ for (size_t i=DimRows; i--; )
+ for (size_t j=DimCols;j--; ret[i][j]=(i==j));
+ return ret;
+ }
+
+ T det() const {
+ return dt<DimCols,T>::det(*this);
+ }
+
+ mat<DimRows-1,DimCols-1,T> get_minor(size_t row, size_t col) const {
+ mat<DimRows-1,DimCols-1,T> ret;
+ for (size_t i=DimRows-1; i--; )
+ for (size_t j=DimCols-1;j--; ret[i][j]=rows[i<row?i:i+1][j<col?j:j+1]);
+ return ret;
+ }
+
+ T cofactor(size_t row, size_t col) const {
+ return get_minor(row,col).det()*((row+col)%2 ? -1 : 1);
+ }
+
+ mat<DimRows,DimCols,T> adjugate() const {
+ mat<DimRows,DimCols,T> ret;
+ for (size_t i=DimRows; i--; )
+ for (size_t j=DimCols; j--; ret[i][j]=cofactor(i,j));
+ return ret;
+ }
+
+ mat<DimRows,DimCols,T> invert_transpose() {
+ mat<DimRows,DimCols,T> ret = adjugate();
+ T tmp = ret[0]*rows[0];
+ return ret/tmp;
+ }
+
+ mat<DimRows,DimCols,T> invert() {
+ return invert_transpose().transpose();
+ }
+
+ mat<DimCols,DimRows,T> transpose() {
+ mat<DimCols,DimRows,T> ret;
+ for (size_t i=DimCols; i--; ret[i]=this->col(i));
+ return ret;
+ }
+};
+
+/////////////////////////////////////////////////////////////////////////////////
+
+template<size_t DimRows,size_t DimCols,typename T> vec<DimRows,T> operator*(const mat<DimRows,DimCols,T>& lhs, const vec<DimCols,T>& rhs) {
+ vec<DimRows,T> ret;
+ for (size_t i=DimRows; i--; ret[i]=lhs[i]*rhs);
+ return ret;
+}
+
+template<size_t R1,size_t C1,size_t C2,typename T>mat<R1,C2,T> operator*(const mat<R1,C1,T>& lhs, const mat<C1,C2,T>& rhs) {
+ mat<R1,C2,T> result;
+ for (size_t i=R1; i--; )
+ for (size_t j=C2; j--; result[i][j]=lhs[i]*rhs.col(j));
+ return result;
+}
+
+template<size_t DimRows,size_t DimCols,typename T>mat<DimCols,DimRows,T> operator/(mat<DimRows,DimCols,T> lhs, const T& rhs) {
+ for (size_t i=DimRows; i--; lhs[i]=lhs[i]/rhs);
+ return lhs;
+}
+
+#if 0
+template <size_t DimRows,size_t DimCols,class T> std::ostream& operator<<(std::ostream& out, mat<DimRows,DimCols,T>& m) {
+ for (size_t i=0; i<DimRows; i++) out << m[i] << std::endl;
+ return out;
+}
+#endif
+/////////////////////////////////////////////////////////////////////////////////
+
+typedef vec<2, float> Vec2f;
+typedef vec<2, int> Vec2i;
+typedef vec<3, float> Vec3f;
+typedef vec<3, int> Vec3i;
+typedef vec<4, float> Vec4f;
+typedef mat<4,4,float> Matrix;
+#endif //__GEOMETRY_H__
+
diff --git a/examples/TinyRenderer/main.cpp b/examples/TinyRenderer/main.cpp
new file mode 100644
index 0000000..32f5e7e
--- /dev/null
+++ b/examples/TinyRenderer/main.cpp
@@ -0,0 +1,237 @@
+#include "OpenGLWindow/SimpleOpenGL3App.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3CommandLineArgs.h"
+#include "Bullet3Common/b3Transform.h"
+
+#include "assert.h"
+#include <stdio.h>
+
+char* gVideoFileName = 0;
+char* gPngFileName = 0;
+
+static b3WheelCallback sOldWheelCB = 0;
+static b3ResizeCallback sOldResizeCB = 0;
+static b3MouseMoveCallback sOldMouseMoveCB = 0;
+static b3MouseButtonCallback sOldMouseButtonCB = 0;
+static b3KeyboardCallback sOldKeyboardCB = 0;
+//static b3RenderCallback sOldRenderCB = 0;
+
+float gWidth = 0 ;
+float gHeight = 0;
+
+void MyWheelCallback(float deltax, float deltay)
+{
+ if (sOldWheelCB)
+ sOldWheelCB(deltax,deltay);
+}
+void MyResizeCallback( float width, float height)
+{
+ gWidth = width;
+ gHeight = height;
+
+ if (sOldResizeCB)
+ sOldResizeCB(width,height);
+}
+void MyMouseMoveCallback( float x, float y)
+{
+ printf("Mouse Move: %f, %f\n", x,y);
+
+ if (sOldMouseMoveCB)
+ sOldMouseMoveCB(x,y);
+}
+void MyMouseButtonCallback(int button, int state, float x, float y)
+{
+ if (sOldMouseButtonCB)
+ sOldMouseButtonCB(button,state,x,y);
+}
+
+
+void MyKeyboardCallback(int keycode, int state)
+{
+ //keycodes are in examples/CommonInterfaces/CommonWindowInterface.h
+ //for example B3G_ESCAPE for escape key
+ //state == 1 for pressed, state == 0 for released.
+ // use app->m_window->isModifiedPressed(...) to check for shift, escape and alt keys
+ printf("MyKeyboardCallback received key:%c in state %d\n",keycode,state);
+ if (sOldKeyboardCB)
+ sOldKeyboardCB(keycode,state);
+}
+#include "TinyRenderer.h"
+
+int main(int argc, char* argv[])
+{
+ b3CommandLineArgs myArgs(argc,argv);
+
+
+
+ SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",640,480,true);
+
+ app->m_instancingRenderer->getActiveCamera()->setCameraDistance(13);
+ app->m_instancingRenderer->getActiveCamera()->setCameraPitch(0);
+ app->m_instancingRenderer->getActiveCamera()->setCameraTargetPosition(0,0,0);
+ sOldKeyboardCB = app->m_window->getKeyboardCallback();
+ app->m_window->setKeyboardCallback(MyKeyboardCallback);
+ sOldMouseMoveCB = app->m_window->getMouseMoveCallback();
+ app->m_window->setMouseMoveCallback(MyMouseMoveCallback);
+ sOldMouseButtonCB = app->m_window->getMouseButtonCallback();
+ app->m_window->setMouseButtonCallback(MyMouseButtonCallback);
+ sOldWheelCB = app->m_window->getWheelCallback();
+ app->m_window->setWheelCallback(MyWheelCallback);
+ sOldResizeCB = app->m_window->getResizeCallback();
+ app->m_window->setResizeCallback(MyResizeCallback);
+
+ int textureWidth = gWidth;
+ int textureHeight = gHeight;
+ TGAImage rgbColorBuffer(gWidth,gHeight,TGAImage::RGB);
+ b3AlignedObjectArray<float> depthBuffer;
+ depthBuffer.resize(gWidth*gHeight);
+
+ TinyRenderObjectData renderData(rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj");
+
+ //renderData.loadModel("african_head/african_head.obj");
+ renderData.loadModel("floor.obj");
+
+ //renderData.createCube(1,1,1);
+
+
+ myArgs.GetCmdLineArgument("mp4_file",gVideoFileName);
+ if (gVideoFileName)
+ app->dumpFramesToVideo(gVideoFileName);
+
+ myArgs.GetCmdLineArgument("png_file",gPngFileName);
+ char fileName[1024];
+
+
+
+ unsigned char* image=new unsigned char[textureWidth*textureHeight*4];
+
+
+ int textureHandle = app->m_renderer->registerTexture(image,textureWidth,textureHeight);
+
+ int cubeIndex = app->registerCubeShape(1,1,1);
+
+ b3Vector3 pos = b3MakeVector3(0,0,0);
+ b3Quaternion orn(0,0,0,1);
+ b3Vector3 color=b3MakeVector3(1,0,0);
+ b3Vector3 scaling=b3MakeVector3 (1,1,1);
+ app->m_renderer->registerGraphicsInstance(cubeIndex,pos,orn,color,scaling);
+ app->m_renderer->writeTransforms();
+
+ do
+ {
+ static int frameCount = 0;
+ frameCount++;
+ if (gPngFileName)
+ {
+ printf("gPngFileName=%s\n",gPngFileName);
+
+ sprintf(fileName,"%s%d.png",gPngFileName,frameCount++);
+ app->dumpNextFrameToPng(fileName);
+ }
+
+ app->m_instancingRenderer->init();
+ app->m_instancingRenderer->updateCamera();
+
+ ///clear the color and z (depth) buffer
+ for(int y=0;y<textureHeight;++y)
+ {
+ unsigned char* pi=image+(y)*textureWidth*3;
+ for(int x=0;x<textureWidth;++x)
+ {
+
+ TGAColor color;
+ color.bgra[0] = 255;
+ color.bgra[1] = 255;
+ color.bgra[2] = 255;
+ color.bgra[3] = 255;
+
+ renderData.m_rgbColorBuffer.set(x,y,color);
+ renderData.m_depthBuffer[x+y*textureWidth] = -1e30f;
+ }
+ }
+
+ float projMat[16];
+ app->m_instancingRenderer->getActiveCamera()->getCameraProjectionMatrix(projMat);
+ float viewMat[16];
+ app->m_instancingRenderer->getActiveCamera()->getCameraViewMatrix(viewMat);
+ B3_ATTRIBUTE_ALIGNED16(float modelMat[16]);
+
+ //sync the object transform
+ b3Transform tr;
+ tr.setIdentity();
+ static float posUp = 0.f;
+ // posUp += 0.001;
+ b3Vector3 org = b3MakeVector3(0,posUp,0);
+ tr.setOrigin(org);
+ tr.getOpenGLMatrix(modelMat);
+
+ for (int i=0;i<4;i++)
+ {
+ for (int j=0;j<4;j++)
+ {
+ renderData.m_viewMatrix[i][j] = viewMat[i+4*j];
+ renderData.m_modelMatrix[i][j] = modelMat[i+4*j];
+ }
+ }
+
+ //render the object
+ TinyRenderer::renderObject(renderData);
+
+ #if 1
+ //update the texels of the texture using a simple pattern, animated using frame index
+ for(int y=0;y<textureHeight;++y)
+ {
+ unsigned char* pi=image+(y)*textureWidth*3;
+ for(int x=0;x<textureWidth;++x)
+ {
+
+ TGAColor color = renderData.m_rgbColorBuffer.get(x,y);
+ pi[0] = color.bgra[2];
+ pi[1] = color.bgra[1];
+ pi[2] = color.bgra[0];
+ pi[3] = 255;
+ pi+=3;
+ }
+ }
+ #else
+
+ //update the texels of the texture using a simple pattern, animated using frame index
+ for(int y=0;y<textureHeight;++y)
+ {
+ const int t=(y+frameCount)>>4;
+ unsigned char* pi=image+y*textureWidth*3;
+ for(int x=0;x<textureWidth;++x)
+ {
+ TGAColor color = renderData.m_rgbColorBuffer.get(x,y);
+
+ const int s=x>>4;
+ const unsigned char b=180;
+ unsigned char c=b+((s+(t&1))&1)*(255-b);
+ pi[0]=pi[1]=pi[2]=pi[3]=c;
+ pi+=3;
+ }
+ }
+ #endif
+
+
+ app->m_renderer->activateTexture(textureHandle);
+ app->m_renderer->updateTexture(textureHandle,image);
+
+ float color[4] = {1,1,1,1};
+ app->m_primRenderer->drawTexturedRect(0,0,gWidth/3,gHeight/3,color,0,0,1,1,true);
+
+
+
+ app->m_renderer->renderScene();
+ app->drawGrid();
+ char bla[1024];
+ sprintf(bla,"Simple test frame %d", frameCount);
+
+ app->drawText(bla,10,10);
+ app->swapBuffer();
+ } while (!app->m_window->requestedExit());
+
+
+ delete app;
+ return 0;
+}
diff --git a/examples/TinyRenderer/model.cpp b/examples/TinyRenderer/model.cpp
new file mode 100644
index 0000000..8864d0a
--- /dev/null
+++ b/examples/TinyRenderer/model.cpp
@@ -0,0 +1,171 @@
+
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include "model.h"
+
+Model::Model(const char *filename) : verts_(), faces_(), norms_(), uv_(), diffusemap_(), normalmap_(), specularmap_() {
+ std::ifstream in;
+ in.open (filename, std::ifstream::in);
+ if (in.fail()) return;
+ std::string line;
+ while (!in.eof()) {
+ std::getline(in, line);
+ std::istringstream iss(line.c_str());
+ char trash;
+ if (!line.compare(0, 2, "v ")) {
+ iss >> trash;
+ Vec3f v;
+ for (int i=0;i<3;i++) iss >> v[i];
+ verts_.push_back(v);
+ } else if (!line.compare(0, 3, "vn ")) {
+ iss >> trash >> trash;
+ Vec3f n;
+ for (int i=0;i<3;i++) iss >> n[i];
+ norms_.push_back(n);
+ } else if (!line.compare(0, 3, "vt ")) {
+ iss >> trash >> trash;
+ Vec2f uv;
+ for (int i=0;i<2;i++) iss >> uv[i];
+ uv_.push_back(uv);
+ } else if (!line.compare(0, 2, "f ")) {
+ std::vector<Vec3i> f;
+ Vec3i tmp;
+ iss >> trash;
+ while (iss >> tmp[0] >> trash >> tmp[1] >> trash >> tmp[2]) {
+ for (int i=0; i<3; i++) tmp[i]--; // in wavefront obj all indices start at 1, not zero
+ f.push_back(tmp);
+ }
+ faces_.push_back(f);
+ }
+ }
+ std::cerr << "# v# " << verts_.size() << " f# " << faces_.size() << " vt# " << uv_.size() << " vn# " << norms_.size() << std::endl;
+ load_texture(filename, "_diffuse.tga", diffusemap_);
+ load_texture(filename, "_nm_tangent.tga", normalmap_);
+ load_texture(filename, "_spec.tga", specularmap_);
+}
+
+Model::Model():verts_(), faces_(), norms_(), uv_(), diffusemap_(), normalmap_(), specularmap_()
+{
+}
+
+void Model::setDiffuseTextureFromData(unsigned char* textureImage,int textureWidth,int textureHeight)
+{
+ diffusemap_ = TGAImage(textureWidth, textureHeight, TGAImage::RGB);
+ for (int i=0;i<textureWidth;i++)
+ {
+ for (int j=0;j<textureHeight;j++)
+ {
+ TGAColor color;
+ color.bgra[0] = textureImage[(i+j*textureWidth)*3+0];
+ color.bgra[1] = textureImage[(i+j*textureWidth)*3+1];
+ color.bgra[2] = textureImage[(i+j*textureWidth)*3+2];
+ color.bgra[3] = 255;
+
+ color.bytespp = 3;
+ diffusemap_.set(i,j,color);
+
+ }
+ }
+
+ diffusemap_.flip_vertically();
+}
+
+void Model::loadDiffuseTexture(const char* relativeFileName)
+{
+ diffusemap_.read_tga_file(relativeFileName);
+}
+
+void Model::reserveMemory(int numVertices, int numIndices)
+{
+ verts_.reserve(numVertices);
+ norms_.reserve(numVertices);
+ uv_.reserve(numVertices);
+ faces_.reserve(numIndices);
+}
+
+void Model::addVertex(float x,float y,float z, float normalX, float normalY, float normalZ, float u, float v)
+{
+ verts_.push_back(Vec3f(x,y,z));
+ norms_.push_back(Vec3f(normalX,normalY,normalZ));
+ uv_.push_back(Vec2f(u,v));
+
+}
+void Model::addTriangle(int vertexposIndex0, int normalIndex0, int uvIndex0,
+ int vertexposIndex1, int normalIndex1, int uvIndex1,
+ int vertexposIndex2, int normalIndex2, int uvIndex2)
+{
+ std::vector<Vec3i> f;
+ f.push_back(Vec3i(vertexposIndex0, normalIndex0, uvIndex0));
+ f.push_back(Vec3i(vertexposIndex1, normalIndex1, uvIndex1));
+ f.push_back(Vec3i(vertexposIndex2, normalIndex2, uvIndex2));
+ faces_.push_back(f);
+}
+
+
+Model::~Model() {}
+
+int Model::nverts() {
+ return (int)verts_.size();
+}
+
+int Model::nfaces() {
+ return (int)faces_.size();
+}
+
+std::vector<int> Model::face(int idx) {
+ std::vector<int> face;
+ for (int i=0; i<(int)faces_[idx].size(); i++) face.push_back(faces_[idx][i][0]);
+ return face;
+}
+
+Vec3f Model::vert(int i) {
+ return verts_[i];
+}
+
+Vec3f Model::vert(int iface, int nthvert) {
+ return verts_[faces_[iface][nthvert][0]];
+}
+
+void Model::load_texture(std::string filename, const char *suffix, TGAImage &img) {
+ std::string texfile(filename);
+ size_t dot = texfile.find_last_of(".");
+ if (dot!=std::string::npos) {
+ texfile = texfile.substr(0,dot) + std::string(suffix);
+ std::cerr << "texture file " << texfile << " loading " << (img.read_tga_file(texfile.c_str()) ? "ok" : "failed") << std::endl;
+ img.flip_vertically();
+ }
+}
+
+TGAColor Model::diffuse(Vec2f uvf) {
+ if (diffusemap_.get_width() && diffusemap_.get_height())
+ {
+ Vec2i uv(uvf[0]*diffusemap_.get_width(), uvf[1]*diffusemap_.get_height());
+ return diffusemap_.get(uv[0], uv[1]);
+ }
+ return TGAColor(255,255,255,255);
+}
+
+Vec3f Model::normal(Vec2f uvf) {
+ Vec2i uv(uvf[0]*normalmap_.get_width(), uvf[1]*normalmap_.get_height());
+ TGAColor c = normalmap_.get(uv[0], uv[1]);
+ Vec3f res;
+ for (int i=0; i<3; i++)
+ res[2-i] = (float)c[i]/255.f*2.f - 1.f;
+ return res;
+}
+
+Vec2f Model::uv(int iface, int nthvert) {
+ return uv_[faces_[iface][nthvert][1]];
+}
+
+float Model::specular(Vec2f uvf) {
+ Vec2i uv(uvf[0]*specularmap_.get_width(), uvf[1]*specularmap_.get_height());
+ return specularmap_.get(uv[0], uv[1])[0]/1.f;
+}
+
+Vec3f Model::normal(int iface, int nthvert) {
+ int idx = faces_[iface][nthvert][2];
+ return norms_[idx].normalize();
+}
+
diff --git a/examples/TinyRenderer/model.h b/examples/TinyRenderer/model.h
new file mode 100644
index 0000000..6cd9e38
--- /dev/null
+++ b/examples/TinyRenderer/model.h
@@ -0,0 +1,54 @@
+#ifndef __MODEL_H__
+#define __MODEL_H__
+#include <vector>
+#include <string>
+#include "geometry.h"
+#include "tgaimage.h"
+
+class Model {
+private:
+ std::vector<Vec3f> verts_;
+ std::vector<std::vector<Vec3i> > faces_; // attention, this Vec3i means vertex/uv/normal
+ std::vector<Vec3f> norms_;
+ std::vector<Vec2f> uv_;
+ TGAImage diffusemap_;
+ TGAImage normalmap_;
+ TGAImage specularmap_;
+ Vec4f m_colorRGBA;
+
+ void load_texture(std::string filename, const char *suffix, TGAImage &img);
+public:
+ Model(const char *filename);
+ Model();
+ void setColorRGBA(const float rgba[4])
+ {
+ for (int i=0;i<4;i++)
+ m_colorRGBA[i] = rgba[i];
+ }
+
+ const Vec4f& getColorRGBA() const
+ {
+ return m_colorRGBA;
+ }
+ void loadDiffuseTexture(const char* relativeFileName);
+ void setDiffuseTextureFromData(unsigned char* textureImage,int textureWidth,int textureHeight);
+ void reserveMemory(int numVertices, int numIndices);
+ void addVertex(float x,float y,float z, float normalX, float normalY, float normalZ, float u, float v);
+ void addTriangle(int vertexposIndex0, int normalIndex0, int uvIndex0,
+ int vertexposIndex1, int normalIndex1, int uvIndex1,
+ int vertexposIndex2, int normalIndex2, int uvIndex2);
+
+ ~Model();
+ int nverts();
+ int nfaces();
+ Vec3f normal(int iface, int nthvert);
+ Vec3f normal(Vec2f uv);
+ Vec3f vert(int i);
+ Vec3f vert(int iface, int nthvert);
+ Vec2f uv(int iface, int nthvert);
+ TGAColor diffuse(Vec2f uv);
+ float specular(Vec2f uv);
+ std::vector<int> face(int idx);
+};
+#endif //__MODEL_H__
+
diff --git a/examples/TinyRenderer/our_gl.cpp b/examples/TinyRenderer/our_gl.cpp
new file mode 100644
index 0000000..eb5a05a
--- /dev/null
+++ b/examples/TinyRenderer/our_gl.cpp
@@ -0,0 +1,117 @@
+#include <cmath>
+#include <limits>
+#include <cstdlib>
+#include "our_gl.h"
+#include "Bullet3Common/b3MinMax.h"
+
+
+
+
+IShader::~IShader() {}
+
+Matrix viewport(int x, int y, int w, int h)
+{
+ Matrix Viewport;
+ Viewport = Matrix::identity();
+ Viewport[0][3] = x+w/2.f;
+ Viewport[1][3] = y+h/2.f;
+ Viewport[2][3] = 1.f;
+ Viewport[0][0] = w/2.f;
+ Viewport[1][1] = h/2.f;
+ Viewport[2][2] = 0;
+ return Viewport;
+}
+
+Matrix projection(float coeff) {
+ Matrix Projection;
+ Projection = Matrix::identity();
+ Projection[3][2] = coeff;
+ return Projection;
+}
+
+Matrix lookat(Vec3f eye, Vec3f center, Vec3f up) {
+ Vec3f z = (eye-center).normalize();
+ Vec3f x = cross(up,z).normalize();
+ Vec3f y = cross(z,x).normalize();
+ Matrix Minv = Matrix::identity();
+ Matrix Tr = Matrix::identity();
+ for (int i=0; i<3; i++) {
+ Minv[0][i] = x[i];
+ Minv[1][i] = y[i];
+ Minv[2][i] = z[i];
+ Tr[i][3] = -center[i];
+ }
+ Matrix ModelView;
+ ModelView = Minv*Tr;
+ return ModelView;
+}
+
+Vec3f barycentric(Vec2f A, Vec2f B, Vec2f C, Vec2f P) {
+ Vec3f s[2];
+ for (int i=2; i--; ) {
+ s[i][0] = C[i]-A[i];
+ s[i][1] = B[i]-A[i];
+ s[i][2] = A[i]-P[i];
+ }
+ Vec3f u = cross(s[0], s[1]);
+ if (std::abs(u[2])>1e-2) // dont forget that u[2] is integer. If it is zero then triangle ABC is degenerate
+ return Vec3f(1.f-(u.x+u.y)/u.z, u.y/u.z, u.x/u.z);
+ return Vec3f(-1,1,1); // in this case generate negative coordinates, it will be thrown away by the rasterizator
+}
+
+void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix)
+{
+ triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0);
+}
+
+void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) {
+ mat<3,4,float> pts = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points
+
+
+ //we don't clip triangles that cross the near plane, just discard them instead of showing artifacts
+ if (pts[0][3]<0 || pts[1][3] <0 || pts[2][3] <0)
+ return;
+
+
+ mat<3,2,float> pts2;
+ for (int i=0; i<3; i++) pts2[i] = proj<2>(pts[i]/pts[i][3]);
+
+ Vec2f bboxmin( std::numeric_limits<float>::max(), std::numeric_limits<float>::max());
+ Vec2f bboxmax(-std::numeric_limits<float>::max(), -std::numeric_limits<float>::max());
+ Vec2f clamp(image.get_width()-1, image.get_height()-1);
+
+ for (int i=0; i<3; i++) {
+ for (int j=0; j<2; j++) {
+ bboxmin[j] = b3Max(0.f, b3Min(bboxmin[j], pts2[i][j]));
+ bboxmax[j] = b3Min(clamp[j], b3Max(bboxmax[j], pts2[i][j]));
+ }
+ }
+
+
+
+ Vec2i P;
+ TGAColor color;
+ for (P.x=bboxmin.x; P.x<=bboxmax.x; P.x++) {
+ for (P.y=bboxmin.y; P.y<=bboxmax.y; P.y++) {
+ Vec3f bc_screen = barycentric(pts2[0], pts2[1], pts2[2], P);
+
+
+ Vec3f bc_clip = Vec3f(bc_screen.x/pts[0][3], bc_screen.y/pts[1][3], bc_screen.z/pts[2][3]);
+ bc_clip = bc_clip/(bc_clip.x+bc_clip.y+bc_clip.z);
+ float frag_depth = -1*(clipc[2]*bc_clip);
+ if (bc_screen.x<0 || bc_screen.y<0 || bc_screen.z<0 ||
+ zbuffer[P.x+P.y*image.get_width()]>frag_depth)
+ continue;
+ bool discard = shader.fragment(bc_clip, color);
+ if (!discard) {
+ zbuffer[P.x+P.y*image.get_width()] = frag_depth;
+ if (segmentationMaskBuffer)
+ {
+ segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex;
+ }
+ image.set(P.x, P.y, color);
+ }
+ }
+ }
+}
+
diff --git a/examples/TinyRenderer/our_gl.h b/examples/TinyRenderer/our_gl.h
new file mode 100644
index 0000000..378324c
--- /dev/null
+++ b/examples/TinyRenderer/our_gl.h
@@ -0,0 +1,22 @@
+#ifndef __OUR_GL_H__
+#define __OUR_GL_H__
+#include "tgaimage.h"
+#include "geometry.h"
+
+
+
+
+Matrix viewport(int x, int y, int w, int h);
+Matrix projection(float coeff=0.f); // coeff = -1/c
+Matrix lookat(Vec3f eye, Vec3f center, Vec3f up);
+
+struct IShader {
+ virtual ~IShader();
+ virtual Vec4f vertex(int iface, int nthvert) = 0;
+ virtual bool fragment(Vec3f bar, TGAColor &color) = 0;
+};
+
+void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix);
+void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex);
+#endif //__OUR_GL_H__
+
diff --git a/examples/TinyRenderer/premake4.lua b/examples/TinyRenderer/premake4.lua
new file mode 100644
index 0000000..3eb417f
--- /dev/null
+++ b/examples/TinyRenderer/premake4.lua
@@ -0,0 +1,29 @@
+
+ project "App_TinyRenderer"
+
+ language "C++"
+
+ kind "ConsoleApp"
+
+ includedirs {
+ ".",
+ "../../src",
+ ".."
+ }
+
+
+ links{ "OpenGL_Window","Bullet3Common"}
+ initOpenGL()
+ initGlew()
+
+ files {
+ "*.cpp",
+ "*.h",
+ "../Utils/b3ResourcePath.cpp"
+ }
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+end
diff --git a/examples/TinyRenderer/tgaimage.cpp b/examples/TinyRenderer/tgaimage.cpp
new file mode 100644
index 0000000..b68fb32
--- /dev/null
+++ b/examples/TinyRenderer/tgaimage.cpp
@@ -0,0 +1,356 @@
+#include <iostream>
+#include <fstream>
+#include <string.h>
+#include <time.h>
+#include <math.h>
+#include "tgaimage.h"
+
+TGAImage::TGAImage() : data(NULL), width(0), height(0), bytespp(0) {}
+
+TGAImage::TGAImage(int w, int h, int bpp) : data(NULL), width(w), height(h), bytespp(bpp) {
+ unsigned long nbytes = width*height*bytespp;
+ data = new unsigned char[nbytes];
+ memset(data, 0, nbytes);
+}
+
+TGAImage::TGAImage(const TGAImage &img) : data(NULL), width(img.width), height(img.height), bytespp(img.bytespp) {
+ unsigned long nbytes = width*height*bytespp;
+ data = new unsigned char[nbytes];
+ memcpy(data, img.data, nbytes);
+}
+
+TGAImage::~TGAImage() {
+ if (data) delete [] data;
+}
+
+TGAImage & TGAImage::operator =(const TGAImage &img) {
+ if (this != &img) {
+ if (data) delete [] data;
+ width = img.width;
+ height = img.height;
+ bytespp = img.bytespp;
+ unsigned long nbytes = width*height*bytespp;
+ data = new unsigned char[nbytes];
+ memcpy(data, img.data, nbytes);
+ }
+ return *this;
+}
+
+bool TGAImage::read_tga_file(const char *filename) {
+ if (data) delete [] data;
+ data = NULL;
+ std::ifstream in;
+ in.open (filename, std::ios::binary);
+ if (!in.is_open()) {
+ std::cerr << "can't open file " << filename << "\n";
+ in.close();
+ return false;
+ }
+ TGA_Header header;
+ in.read((char *)&header, sizeof(header));
+ if (!in.good()) {
+ in.close();
+ std::cerr << "an error occured while reading the header\n";
+ return false;
+ }
+ width = header.width;
+ height = header.height;
+ bytespp = header.bitsperpixel>>3;
+ if (width<=0 || height<=0 || (bytespp!=GRAYSCALE && bytespp!=RGB && bytespp!=RGBA)) {
+ in.close();
+ std::cerr << "bad bpp (or width/height) value\n";
+ return false;
+ }
+ unsigned long nbytes = bytespp*width*height;
+ data = new unsigned char[nbytes];
+ if (3==header.datatypecode || 2==header.datatypecode) {
+ in.read((char *)data, nbytes);
+ if (!in.good()) {
+ in.close();
+ std::cerr << "an error occured while reading the data\n";
+ return false;
+ }
+ } else if (10==header.datatypecode||11==header.datatypecode) {
+ if (!load_rle_data(in)) {
+ in.close();
+ std::cerr << "an error occured while reading the data\n";
+ return false;
+ }
+ } else {
+ in.close();
+ std::cerr << "unknown file format " << (int)header.datatypecode << "\n";
+ return false;
+ }
+ if (!(header.imagedescriptor & 0x20)) {
+ flip_vertically();
+ }
+ if (header.imagedescriptor & 0x10) {
+ flip_horizontally();
+ }
+ std::cerr << width << "x" << height << "/" << bytespp*8 << "\n";
+ in.close();
+ return true;
+}
+
+bool TGAImage::load_rle_data(std::ifstream &in) {
+ unsigned long pixelcount = width*height;
+ unsigned long currentpixel = 0;
+ unsigned long currentbyte = 0;
+ TGAColor colorbuffer;
+ do {
+ unsigned char chunkheader = 0;
+ chunkheader = in.get();
+ if (!in.good()) {
+ std::cerr << "an error occured while reading the data\n";
+ return false;
+ }
+ if (chunkheader<128) {
+ chunkheader++;
+ for (int i=0; i<chunkheader; i++) {
+ in.read((char *)colorbuffer.bgra, bytespp);
+ if (!in.good()) {
+ std::cerr << "an error occured while reading the header\n";
+ return false;
+ }
+ for (int t=0; t<bytespp; t++)
+ data[currentbyte++] = colorbuffer.bgra[t];
+ currentpixel++;
+ if (currentpixel>pixelcount) {
+ std::cerr << "Too many pixels read\n";
+ return false;
+ }
+ }
+ } else {
+ chunkheader -= 127;
+ in.read((char *)colorbuffer.bgra, bytespp);
+ if (!in.good()) {
+ std::cerr << "an error occured while reading the header\n";
+ return false;
+ }
+ for (int i=0; i<chunkheader; i++) {
+ for (int t=0; t<bytespp; t++)
+ data[currentbyte++] = colorbuffer.bgra[t];
+ currentpixel++;
+ if (currentpixel>pixelcount) {
+ std::cerr << "Too many pixels read\n";
+ return false;
+ }
+ }
+ }
+ } while (currentpixel < pixelcount);
+ return true;
+}
+
+bool TGAImage::write_tga_file(const char *filename, bool rle) const {
+ unsigned char developer_area_ref[4] = {0, 0, 0, 0};
+ unsigned char extension_area_ref[4] = {0, 0, 0, 0};
+ unsigned char footer[18] = {'T','R','U','E','V','I','S','I','O','N','-','X','F','I','L','E','.','\0'};
+ std::ofstream out;
+ out.open (filename, std::ios::binary);
+ if (!out.is_open()) {
+ std::cerr << "can't open file " << filename << "\n";
+ out.close();
+ return false;
+ }
+ TGA_Header header;
+ memset((void *)&header, 0, sizeof(header));
+ header.bitsperpixel = bytespp<<3;
+ header.width = width;
+ header.height = height;
+ header.datatypecode = (bytespp==GRAYSCALE?(rle?11:3):(rle?10:2));
+ header.imagedescriptor = 0x20; // top-left origin
+ out.write((char *)&header, sizeof(header));
+ if (!out.good()) {
+ out.close();
+ std::cerr << "can't dump the tga file\n";
+ return false;
+ }
+ if (!rle) {
+ out.write((char *)data, width*height*bytespp);
+ if (!out.good()) {
+ std::cerr << "can't unload raw data\n";
+ out.close();
+ return false;
+ }
+ } else {
+ if (!unload_rle_data(out)) {
+ out.close();
+ std::cerr << "can't unload rle data\n";
+ return false;
+ }
+ }
+ out.write((char *)developer_area_ref, sizeof(developer_area_ref));
+ if (!out.good()) {
+ std::cerr << "can't dump the tga file\n";
+ out.close();
+ return false;
+ }
+ out.write((char *)extension_area_ref, sizeof(extension_area_ref));
+ if (!out.good()) {
+ std::cerr << "can't dump the tga file\n";
+ out.close();
+ return false;
+ }
+ out.write((char *)footer, sizeof(footer));
+ if (!out.good()) {
+ std::cerr << "can't dump the tga file\n";
+ out.close();
+ return false;
+ }
+ out.close();
+ return true;
+}
+
+// TODO: it is not necessary to break a raw chunk for two equal pixels (for the matter of the resulting size)
+bool TGAImage::unload_rle_data(std::ofstream &out) const {
+ const unsigned char max_chunk_length = 128;
+ unsigned long npixels = width*height;
+ unsigned long curpix = 0;
+ while (curpix<npixels) {
+ unsigned long chunkstart = curpix*bytespp;
+ unsigned long curbyte = curpix*bytespp;
+ unsigned char run_length = 1;
+ bool raw = true;
+ while (curpix+run_length<npixels && run_length<max_chunk_length) {
+ bool succ_eq = true;
+ for (int t=0; succ_eq && t<bytespp; t++) {
+ succ_eq = (data[curbyte+t]==data[curbyte+t+bytespp]);
+ }
+ curbyte += bytespp;
+ if (1==run_length) {
+ raw = !succ_eq;
+ }
+ if (raw && succ_eq) {
+ run_length--;
+ break;
+ }
+ if (!raw && !succ_eq) {
+ break;
+ }
+ run_length++;
+ }
+ curpix += run_length;
+ out.put(raw?run_length-1:run_length+127);
+ if (!out.good()) {
+ std::cerr << "can't dump the tga file\n";
+ return false;
+ }
+ out.write((char *)(data+chunkstart), (raw?run_length*bytespp:bytespp));
+ if (!out.good()) {
+ std::cerr << "can't dump the tga file\n";
+ return false;
+ }
+ }
+ return true;
+}
+
+TGAColor TGAImage::get(int x, int y) const {
+ if (!data || x<0 || y<0 || x>=width || y>=height) {
+ return TGAColor(128.f,128.f,128.f,255.f);
+ }
+ return TGAColor(data+(x+y*width)*bytespp, bytespp);
+}
+
+bool TGAImage::set(int x, int y, TGAColor &c) {
+ if (!data || x<0 || y<0 || x>=width || y>=height) {
+ return false;
+ }
+ memcpy(data+(x+y*width)*bytespp, c.bgra, bytespp);
+ return true;
+}
+
+bool TGAImage::set(int x, int y, const TGAColor &c) {
+ if (!data || x<0 || y<0 || x>=width || y>=height) {
+ return false;
+ }
+ memcpy(data+(x+y*width)*bytespp, c.bgra, bytespp);
+ return true;
+}
+
+int TGAImage::get_bytespp() {
+ return bytespp;
+}
+
+int TGAImage::get_width() {
+ return width;
+}
+
+int TGAImage::get_height() {
+ return height;
+}
+
+bool TGAImage::flip_horizontally() {
+ if (!data) return false;
+ int half = width>>1;
+ for (int i=0; i<half; i++) {
+ for (int j=0; j<height; j++) {
+ TGAColor c1 = get(i, j);
+ TGAColor c2 = get(width-1-i, j);
+ set(i, j, c2);
+ set(width-1-i, j, c1);
+ }
+ }
+ return true;
+}
+
+bool TGAImage::flip_vertically() {
+ if (!data) return false;
+ unsigned long bytes_per_line = width*bytespp;
+ unsigned char *line = new unsigned char[bytes_per_line];
+ int half = height>>1;
+ for (int j=0; j<half; j++) {
+ unsigned long l1 = j*bytes_per_line;
+ unsigned long l2 = (height-1-j)*bytes_per_line;
+ memmove((void *)line, (void *)(data+l1), bytes_per_line);
+ memmove((void *)(data+l1), (void *)(data+l2), bytes_per_line);
+ memmove((void *)(data+l2), (void *)line, bytes_per_line);
+ }
+ delete [] line;
+ return true;
+}
+
+unsigned char *TGAImage::buffer() {
+ return data;
+}
+
+void TGAImage::clear() {
+ memset((void *)data, 0, width*height*bytespp);
+}
+
+bool TGAImage::scale(int w, int h) {
+ if (w<=0 || h<=0 || !data) return false;
+ unsigned char *tdata = new unsigned char[w*h*bytespp];
+ int nscanline = 0;
+ int oscanline = 0;
+ int erry = 0;
+ unsigned long nlinebytes = w*bytespp;
+ unsigned long olinebytes = width*bytespp;
+ for (int j=0; j<height; j++) {
+ int errx = width-w;
+ int nx = -bytespp;
+ int ox = -bytespp;
+ for (int i=0; i<width; i++) {
+ ox += bytespp;
+ errx += w;
+ while (errx>=(int)width) {
+ errx -= width;
+ nx += bytespp;
+ memcpy(tdata+nscanline+nx, data+oscanline+ox, bytespp);
+ }
+ }
+ erry += h;
+ oscanline += olinebytes;
+ while (erry>=(int)height) {
+ if (erry>=(int)height<<1) // it means we jump over a scanline
+ memcpy(tdata+nscanline+nlinebytes, tdata+nscanline, nlinebytes);
+ erry -= height;
+ nscanline += nlinebytes;
+ }
+ }
+ delete [] data;
+ data = tdata;
+ width = w;
+ height = h;
+ return true;
+}
+
diff --git a/examples/TinyRenderer/tgaimage.h b/examples/TinyRenderer/tgaimage.h
new file mode 100644
index 0000000..80818dd
--- /dev/null
+++ b/examples/TinyRenderer/tgaimage.h
@@ -0,0 +1,101 @@
+#ifndef __IMAGE_H__
+#define __IMAGE_H__
+
+#include <fstream>
+
+#pragma pack(push,1)
+struct TGA_Header {
+ char idlength;
+ char colormaptype;
+ char datatypecode;
+ short colormaporigin;
+ short colormaplength;
+ char colormapdepth;
+ short x_origin;
+ short y_origin;
+ short width;
+ short height;
+ char bitsperpixel;
+ char imagedescriptor;
+};
+#pragma pack(pop)
+
+struct TGAColor {
+ unsigned char bgra[4];
+ unsigned char bytespp;
+
+ TGAColor() : bytespp(1)
+ {
+ for (int i=0; i<4; i++)
+ bgra[i] = 0;
+ }
+
+ TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bytespp(4) {
+ bgra[0] = B;
+ bgra[1] = G;
+ bgra[2] = R;
+ bgra[3] = A;
+ }
+
+ TGAColor(unsigned char v) : bytespp(1) {
+ for (int i=0; i<4; i++) bgra[i] = 0;
+ bgra[0] = v;
+ }
+
+
+ TGAColor(const unsigned char *p, unsigned char bpp) : bytespp(bpp) {
+ for (int i=0; i<(int)bpp; i++) {
+ bgra[i] = p[i];
+ }
+ for (int i=bpp; i<4; i++) {
+ bgra[i] = 0;
+ }
+ }
+
+ unsigned char& operator[](const int i) { return bgra[i]; }
+
+ TGAColor operator *(float intensity) const {
+ TGAColor res = *this;
+ intensity = (intensity>1.f?1.f:(intensity<0.f?0.f:intensity));
+ for (int i=0; i<4; i++) res.bgra[i] = bgra[i]*intensity;
+ return res;
+ }
+};
+
+class TGAImage {
+protected:
+ unsigned char* data;
+ int width;
+ int height;
+ int bytespp;
+
+ bool load_rle_data(std::ifstream &in);
+ bool unload_rle_data(std::ofstream &out) const;
+public:
+ enum Format {
+ GRAYSCALE=1, RGB=3, RGBA=4
+ };
+
+ TGAImage();
+ TGAImage(int w, int h, int bpp);
+ TGAImage(const TGAImage &img);
+ bool read_tga_file(const char *filename);
+ bool write_tga_file(const char *filename, bool rle=true) const;
+ bool flip_horizontally();
+ bool flip_vertically();
+ bool scale(int w, int h);
+ TGAColor get(int x, int y) const;
+
+ bool set(int x, int y, TGAColor &c);
+ bool set(int x, int y, const TGAColor &c);
+ ~TGAImage();
+ TGAImage & operator =(const TGAImage &img);
+ int get_width();
+ int get_height();
+ int get_bytespp();
+ unsigned char *buffer();
+ void clear();
+};
+
+#endif //__IMAGE_H__
+
diff --git a/examples/Tutorial/Tutorial.cpp b/examples/Tutorial/Tutorial.cpp
index 8a2e0ca..19bb62c 100644
--- a/examples/Tutorial/Tutorial.cpp
+++ b/examples/Tutorial/Tutorial.cpp
@@ -415,7 +415,7 @@ public:
{
char relativeFileName[1024];
sprintf(relativeFileName,"%s%s",prefix[i],filename);
- image = stbi_load(relativeFileName, &width, &height, &n, 0);
+ image = stbi_load(relativeFileName, &width, &height, &n, 3);
}
b3Assert(image);
diff --git a/examples/Utils/b3Clock.cpp b/examples/Utils/b3Clock.cpp
index 53a82de..8e18845 100644
--- a/examples/Utils/b3Clock.cpp
+++ b/examples/Utils/b3Clock.cpp
@@ -36,6 +36,7 @@ const T& b3ClockMin(const T& a, const T& b)
#else //_WIN32
#include <sys/time.h>
+#include <unistd.h>
#endif //_WIN32
@@ -68,6 +69,7 @@ b3Clock::b3Clock()
reset();
}
+
b3Clock::~b3Clock()
{
delete m_data;
@@ -165,7 +167,7 @@ unsigned long int b3Clock::getTimeMilliseconds()
/// Returns the time in us since the last call to reset or since
/// the Clock was created.
-unsigned long int b3Clock::getTimeMicroseconds()
+unsigned long long int b3Clock::getTimeMicroseconds()
{
#ifdef B3_USE_WINDOWS_TIMERS
LARGE_INTEGER currentTime;
@@ -174,14 +176,14 @@ unsigned long int b3Clock::getTimeMicroseconds()
m_data->mStartTime.QuadPart;
// Compute the number of millisecond ticks elapsed.
- unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
+ unsigned long long msecTicks = (unsigned long long)(1000 * elapsedTime /
m_data->mClockFrequency.QuadPart);
// Check for unexpected leaps in the Win32 performance counter.
// (This is caused by unexpected data across the PCI to ISA
// bridge, aka south bridge. See Microsoft KB274323.)
- unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick;
- signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+ unsigned long long elapsedTicks = GetTickCount() - m_data->mStartTick;
+ signed long long msecOff = (signed long)(msecTicks - elapsedTicks);
if (msecOff < -100 || msecOff > 100)
{
// Adjust the starting time forwards.
@@ -196,7 +198,7 @@ unsigned long int b3Clock::getTimeMicroseconds()
m_data->mPrevElapsedTime = elapsedTime;
// Convert to microseconds.
- unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime /
+ unsigned long long usecTicks = (unsigned long)(1000000 * elapsedTime /
m_data->mClockFrequency.QuadPart);
return usecTicks;
@@ -221,3 +223,26 @@ unsigned long int b3Clock::getTimeMicroseconds()
#endif
}
+double b3Clock::getTimeInSeconds()
+{
+ return double(getTimeMicroseconds()/1.e6);
+}
+
+void b3Clock::usleep(int microSeconds)
+{
+#ifdef _WIN32
+ int millis = microSeconds/1000;
+ if (millis < 1)
+ {
+ millis = 1;
+ }
+ Sleep(millis);
+#else
+
+ ::usleep(microSeconds);
+ //struct timeval tv;
+ //tv.tv_sec = microSeconds/1000000L;
+ //tv.tv_usec = microSeconds%1000000L;
+ //return select(0, 0, 0, 0, &tv);
+#endif
+}
diff --git a/examples/Utils/b3Clock.h b/examples/Utils/b3Clock.h
index d30c615..10a3668 100644
--- a/examples/Utils/b3Clock.h
+++ b/examples/Utils/b3Clock.h
@@ -22,7 +22,16 @@ public:
/// Returns the time in us since the last call to reset or since
/// the Clock was created.
- unsigned long int getTimeMicroseconds();
+ unsigned long long int getTimeMicroseconds();
+
+ /// Returns the time in seconds since the last call to reset or since
+ /// the Clock was created.
+ double getTimeInSeconds();
+
+ ///Sleep for 'microSeconds', to yield to other threads and not waste 100% CPU cycles.
+ ///Note that some operating systems may sleep a longer time.
+ static void usleep(int microSeconds);
+
private:
struct b3ClockData* m_data;
};
diff --git a/examples/Utils/b3ResourcePath.cpp b/examples/Utils/b3ResourcePath.cpp
index d27b61e..bc50ef5 100644
--- a/examples/Utils/b3ResourcePath.cpp
+++ b/examples/Utils/b3ResourcePath.cpp
@@ -4,7 +4,7 @@
#include <mach-o/dyld.h> /* _NSGetExecutablePath */
#else
#ifdef _WIN32
-#include <Windows.h>
+#include <windows.h>
#else
//not Mac, not Windows, let's cross the fingers it is Linux :-)
#include <unistd.h>
@@ -33,8 +33,10 @@ int b3ResourcePath::getExePath(char* path, int maxPathLenInBytes)
#else
#ifdef _WIN32
//https://msdn.microsoft.com/en-us/library/windows/desktop/ms683197(v=vs.85).aspx
+
HMODULE hModule = GetModuleHandle(NULL);
- numBytes = GetModuleFileName(hModule, path, maxPathLenInBytes);
+ numBytes = GetModuleFileNameA(hModule, path, maxPathLenInBytes);
+
#else
///http://stackoverflow.com/questions/933850/how-to-find-the-location-of-the-executable-in-c
numBytes = (int)readlink("/proc/self/exe", path, maxPathLenInBytes-1);
@@ -77,6 +79,12 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat
{
return strlen(resourcePath);
}
+ sprintf(resourcePath,"%s.runfiles/google3/third_party/bullet/data/%s",exePath,resourceName);
+ //printf("try resource at %s\n", resourcePath);
+ if (b3FileUtils::findFile(resourcePath, resourcePath, resourcePathMaxNumBytes))
+ {
+ return strlen(resourcePath);
+ }
}
}
diff --git a/examples/Vehicles/Hinge2Vehicle.cpp b/examples/Vehicles/Hinge2Vehicle.cpp
index 9d72ad5..794ebcd 100644
--- a/examples/Vehicles/Hinge2Vehicle.cpp
+++ b/examples/Vehicles/Hinge2Vehicle.cpp
@@ -568,11 +568,6 @@ void Hinge2Vehicle::renderScene()
m_guiHelper->render(m_dynamicsWorld);
-
-
- ATTRIBUTE_ALIGNED16(btScalar) m[16];
- int i;
-
btVector3 wheelColor(1,0,0);
btVector3 worldBoundsMin,worldBoundsMax;
diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt
new file mode 100644
index 0000000..dbac567
--- /dev/null
+++ b/examples/pybullet/CMakeLists.txt
@@ -0,0 +1,88 @@
+
+INCLUDE_DIRECTORIES(
+ ${BULLET_PHYSICS_SOURCE_DIR}/src
+ ${BULLET_PHYSICS_SOURCE_DIR}/examples
+ ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
+ ${PYTHON_INCLUDE_DIRS}
+ )
+IF(BUILD_PYBULLET_NUMPY)
+ INCLUDE_DIRECTORIES(
+ ${PYTHON_NUMPY_INCLUDE_DIR}
+ )
+ENDIF()
+
+SET(pybullet_SRCS
+ pybullet.c
+ ../../examples/SharedMemory/IKTrajectoryHelper.cpp
+ ../../examples/SharedMemory/IKTrajectoryHelper.h
+ ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
+ ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+ ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
+ ../../examples/OpenGLWindow/SimpleCamera.cpp
+ ../../examples/OpenGLWindow/SimpleCamera.h
+ ../../examples/TinyRenderer/geometry.cpp
+ ../../examples/TinyRenderer/model.cpp
+ ../../examples/TinyRenderer/tgaimage.cpp
+ ../../examples/TinyRenderer/our_gl.cpp
+ ../../examples/TinyRenderer/TinyRenderer.cpp
+ ../../examples/SharedMemory/InProcessMemory.cpp
+ ../../examples/SharedMemory/PhysicsClient.cpp
+ ../../examples/SharedMemory/PhysicsClient.h
+ ../../examples/SharedMemory/PhysicsServer.cpp
+ ../../examples/SharedMemory/PhysicsServer.h
+ ../../examples/SharedMemory/PhysicsServerExample.cpp
+ ../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
+ ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
+ ../../examples/SharedMemory/PhysicsServerSharedMemory.h
+ ../../examples/SharedMemory/PhysicsDirect.cpp
+ ../../examples/SharedMemory/PhysicsDirect.h
+ ../../examples/SharedMemory/PhysicsDirectC_API.cpp
+ ../../examples/SharedMemory/PhysicsDirectC_API.h
+ ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+ ../../examples/SharedMemory/PhysicsServerCommandProcessor.h
+ ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
+ ../../examples/SharedMemory/PhysicsClientSharedMemory.h
+ ../../examples/SharedMemory/PhysicsClientC_API.cpp
+ ../../examples/SharedMemory/PhysicsClientC_API.h
+ ../../examples/SharedMemory/Win32SharedMemory.cpp
+ ../../examples/SharedMemory/Win32SharedMemory.h
+ ../../examples/SharedMemory/PosixSharedMemory.cpp
+ ../../examples/SharedMemory/PosixSharedMemory.h
+ ../../examples/Utils/b3ResourcePath.cpp
+ ../../examples/Utils/b3ResourcePath.h
+ ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
+ ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
+ ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
+ ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
+ ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
+ ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
+ ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
+ ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+ ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
+ ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+ ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
+ ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
+ ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
+ ../../examples/MultiThreading/b3PosixThreadSupport.cpp
+ ../../examples/MultiThreading/b3Win32ThreadSupport.cpp
+ ../../examples/MultiThreading/b3ThreadSupportInterface.cpp
+)
+
+IF(WIN32)
+LINK_LIBRARIES(
+ ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+ )
+
+ENDIF(WIN32)
+
+ADD_LIBRARY(pybullet SHARED ${pybullet_SRCS})
+
+SET_TARGET_PROPERTIES(pybullet PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(pybullet PROPERTIES SOVERSION ${BULLET_VERSION})
+
+TARGET_LINK_LIBRARIES(pybullet BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common ${PYTHON_LIBRARIES})
+
+
diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua
new file mode 100644
index 0000000..6f684a3
--- /dev/null
+++ b/examples/pybullet/premake4.lua
@@ -0,0 +1,107 @@
+
+
+project ("pybullet")
+ language "C++"
+ kind "SharedLib"
+
+ includedirs {"../../src", "../../examples",
+ "../../examples/ThirdPartyLibs"}
+ defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
+ hasCL = findOpenCL("clew")
+
+ links{"BulletExampleBrowserLib","gwen", "BulletFileLoader","BulletWorldImporter","OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"}
+ initOpenGL()
+ initGlew()
+
+ includedirs {
+ ".",
+ "../../src",
+ "../ThirdPartyLibs",
+ }
+
+ if os.is("MacOSX") then
+ links{"Cocoa.framework","Python"}
+ end
+
+ if (hasCL) then
+ links {
+ "Bullet3OpenCL_clew",
+ "Bullet3Dynamics",
+ "Bullet3Collision",
+ "Bullet3Geometry",
+ "Bullet3Common",
+ }
+ end
+
+ files {
+ "pybullet.c",
+ "../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+ "../../examples/SharedMemory/IKTrajectoryHelper.h",
+ "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
+ "../../examples/OpenGLWindow/SimpleCamera.cpp",
+ "../../examples/OpenGLWindow/SimpleCamera.h",
+ "../../examples/TinyRenderer/geometry.cpp",
+ "../../examples/TinyRenderer/model.cpp",
+ "../../examples/TinyRenderer/tgaimage.cpp",
+ "../../examples/TinyRenderer/our_gl.cpp",
+ "../../examples/TinyRenderer/TinyRenderer.cpp",
+ "../../examples/SharedMemory/InProcessMemory.cpp",
+ "../../examples/SharedMemory/PhysicsClient.cpp",
+ "../../examples/SharedMemory/PhysicsClient.h",
+ "../../examples/SharedMemory/PhysicsServer.cpp",
+ "../../examples/SharedMemory/PhysicsServer.h",
+ "../../examples/SharedMemory/PhysicsServerExample.cpp",
+ "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
+ "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
+ "../../examples/SharedMemory/PhysicsServerSharedMemory.h",
+ "../../examples/SharedMemory/PhysicsDirect.cpp",
+ "../../examples/SharedMemory/PhysicsDirect.h",
+ "../../examples/SharedMemory/PhysicsDirectC_API.cpp",
+ "../../examples/SharedMemory/PhysicsDirectC_API.h",
+ "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
+ "../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
+ "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
+ "../../examples/SharedMemory/PhysicsClientSharedMemory.h",
+ "../../examples/SharedMemory/PhysicsClientC_API.cpp",
+ "../../examples/SharedMemory/PhysicsClientC_API.h",
+ "../../examples/SharedMemory/Win32SharedMemory.cpp",
+ "../../examples/SharedMemory/Win32SharedMemory.h",
+ "../../examples/SharedMemory/PosixSharedMemory.cpp",
+ "../../examples/SharedMemory/PosixSharedMemory.h",
+ "../../examples/Utils/b3ResourcePath.cpp",
+ "../../examples/Utils/b3ResourcePath.h",
+ "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
+ "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../../examples/MultiThreading/b3PosixThreadSupport.cpp",
+ "../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
+ "../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
+ }
+
+ includedirs {
+ _OPTIONS["python_include_dir"],
+ }
+ libdirs {
+ _OPTIONS["python_lib_dir"]
+ }
+
+ if os.is("Linux") then
+ initX11()
+ end
+
+
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
new file mode 100644
index 0000000..f3ef08e
--- /dev/null
+++ b/examples/pybullet/pybullet.c
@@ -0,0 +1,2288 @@
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include "../SharedMemory/PhysicsDirectC_API.h"
+#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
+
+
+#ifdef __APPLE__
+#include <Python/Python.h>
+#else
+#include <Python.h>
+#endif
+
+#ifdef PYBULLET_USE_NUMPY
+#include <numpy/arrayobject.h>
+#endif
+
+#if PY_MAJOR_VERSION >= 3
+#define PyInt_FromLong PyLong_FromLong
+#define PyString_FromString PyBytes_FromString
+#endif
+
+enum eCONNECT_METHOD {
+ eCONNECT_GUI = 1,
+ eCONNECT_DIRECT = 2,
+ eCONNECT_SHARED_MEMORY = 3,
+};
+
+static PyObject* SpamError;
+static b3PhysicsClientHandle sm = 0;
+
+// Step through one timestep of the simulation
+static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+
+ if (b3CanSubmitCommand(sm)) {
+ statusHandle = b3SubmitClientCommandAndWaitStatus(
+ sm, b3InitStepSimulationCommand(sm));
+ statusType = b3GetStatusType(statusHandle);
+ }
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args) {
+ if (0 != sm) {
+ PyErr_SetString(SpamError,
+ "Already connected to physics server, disconnect first.");
+ return NULL;
+ }
+
+ {
+ int method = eCONNECT_GUI;
+ if (!PyArg_ParseTuple(args, "i", &method)) {
+ PyErr_SetString(SpamError,
+ "connectPhysicsServer expected argument eCONNECT_GUI, "
+ "eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY");
+ return NULL;
+ }
+
+ switch (method) {
+ case eCONNECT_GUI: {
+ int argc = 0;
+ char* argv[1] = {0};
+
+#ifdef __APPLE__
+ sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
+#else
+ sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
+#endif
+ break;
+ }
+ case eCONNECT_DIRECT: {
+ sm = b3ConnectPhysicsDirect();
+ break;
+ }
+ case eCONNECT_SHARED_MEMORY: {
+ sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
+ break;
+ }
+
+ default: {
+ PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument");
+ return NULL;
+ }
+ };
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
+ PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+ {
+ b3DisconnectSharedMemory(sm);
+ sm = 0;
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) {
+ int size = PySequence_Size(args);
+ const char* worldFileName = "";
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ if (size == 1) {
+ if (!PyArg_ParseTuple(args, "s", &worldFileName))
+ {
+ return NULL;
+ }
+ else
+ {
+ b3SharedMemoryCommandHandle command;
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+
+ command = b3SaveWorldCommandInit(sm, worldFileName);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ statusType = b3GetStatusType(statusHandle);
+ if (statusType != CMD_SAVE_WORLD_COMPLETED) {
+ PyErr_SetString(SpamError, "saveWorld command execution failed.");
+ return NULL;
+ }
+ Py_INCREF(Py_None);
+ return Py_None;
+ }
+ }
+
+
+
+ PyErr_SetString(SpamError, "Cannot execute saveWorld command.");
+ return NULL;
+
+ return NULL;
+}
+
+
+
+// Load a URDF file indicating the links and joints of an object
+// function can be called without arguments and will default
+// to position (0,0,1) with orientation(0,0,0,1)
+// els(x,y,z) or
+// loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w)
+static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) {
+ int size = PySequence_Size(args);
+
+ int bodyIndex = -1;
+ const char* urdfFileName = "";
+
+ double startPosX = 0.0;
+ double startPosY = 0.0;
+ double startPosZ = 0.0;
+ double startOrnX = 0.0;
+ double startOrnY = 0.0;
+ double startOrnZ = 0.0;
+ double startOrnW = 1.0;
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+ if (size == 1) {
+ if (!PyArg_ParseTuple(args, "s", &urdfFileName)) return NULL;
+ }
+ if (size == 4) {
+ if (!PyArg_ParseTuple(args, "sddd", &urdfFileName, &startPosX, &startPosY,
+ &startPosZ))
+ return NULL;
+ }
+ if (size == 8) {
+ if (!PyArg_ParseTuple(args, "sddddddd", &urdfFileName, &startPosX,
+ &startPosY, &startPosZ, &startOrnX, &startOrnY,
+ &startOrnZ, &startOrnW))
+ return NULL;
+ }
+
+ if (strlen(urdfFileName)) {
+ // printf("(%f, %f, %f) (%f, %f, %f, %f)\n",
+ // startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW);
+
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ b3SharedMemoryCommandHandle command =
+ b3LoadUrdfCommandInit(sm, urdfFileName);
+
+ // setting the initial position, orientation and other arguments are
+ // optional
+ b3LoadUrdfCommandSetStartPosition(command, startPosX, startPosY, startPosZ);
+ b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY,
+ startOrnZ, startOrnW);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ statusType = b3GetStatusType(statusHandle);
+ if (statusType != CMD_URDF_LOADING_COMPLETED) {
+ PyErr_SetString(SpamError, "Cannot load URDF file.");
+ return NULL;
+ }
+ bodyIndex = b3GetStatusBodyIndex(statusHandle);
+ } else {
+ PyErr_SetString(SpamError,
+ "Empty filename, method expects 1, 4 or 8 arguments.");
+ return NULL;
+ }
+ return PyLong_FromLong(bodyIndex);
+}
+
+static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
+ double v = 0.0;
+ PyObject* item;
+
+ if (PyList_Check(seq)) {
+ item = PyList_GET_ITEM(seq, index);
+ v = PyFloat_AsDouble(item);
+ } else {
+ item = PyTuple_GET_ITEM(seq, index);
+ v = PyFloat_AsDouble(item);
+ }
+ return v;
+}
+
+#define MAX_SDF_BODIES 512
+
+static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) {
+ const char* sdfFileName = "";
+ int size = PySequence_Size(args);
+ int numBodies = 0;
+ int i;
+ int bodyIndicesOut[MAX_SDF_BODIES];
+ PyObject* pylist = 0;
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ b3SharedMemoryCommandHandle commandHandle;
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ if (size == 1) {
+ if (!PyArg_ParseTuple(args, "s", &sdfFileName)) return NULL;
+ }
+
+ commandHandle = b3LoadSdfCommandInit(sm, sdfFileName);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+ statusType = b3GetStatusType(statusHandle);
+ if (statusType != CMD_SDF_LOADING_COMPLETED) {
+ PyErr_SetString(SpamError, "Cannot load SDF file.");
+ return NULL;
+ }
+
+ numBodies =
+ b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
+ if (numBodies > MAX_SDF_BODIES) {
+ PyErr_SetString(SpamError, "SDF exceeds body capacity");
+ return NULL;
+ }
+
+ pylist = PyTuple_New(numBodies);
+
+ if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) {
+ for (i = 0; i < numBodies; i++) {
+ PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i]));
+ }
+ }
+ return pylist;
+}
+
+// Reset the simulation to remove all loaded objects
+static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ statusHandle = b3SubmitClientCommandAndWaitStatus(
+ sm, b3InitResetSimulationCommand(sm));
+ }
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) {
+ int size;
+ int bodyIndex, jointIndex, controlMode;
+
+ double targetPosition = 0.0;
+ double targetVelocity = 0.0;
+ double maxForce = 100000.0;
+ double appliedForce = 0.0;
+ double kp = 0.1;
+ double kd = 1.0;
+ int valid = 0;
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ size = PySequence_Size(args);
+ if (size == 4) {
+ double targetValue = 0.0;
+ // see switch statement below for convertsions dependent on controlMode
+ if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode,
+ &targetValue)) {
+ PyErr_SetString(SpamError, "Error parsing arguments");
+ return NULL;
+ }
+ valid = 1;
+ switch (controlMode) {
+ case CONTROL_MODE_POSITION_VELOCITY_PD: {
+ targetPosition = targetValue;
+ break;
+ }
+ case CONTROL_MODE_VELOCITY: {
+ targetVelocity = targetValue;
+ break;
+ }
+ case CONTROL_MODE_TORQUE: {
+ appliedForce = targetValue;
+ break;
+ }
+ default: { valid = 0; }
+ }
+ }
+ if (size == 5) {
+ double targetValue = 0.0;
+ // See switch statement for conversions
+ if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode,
+ &targetValue, &maxForce)) {
+ PyErr_SetString(SpamError, "Error parsing arguments");
+ return NULL;
+ }
+ valid = 1;
+
+ switch (controlMode) {
+ case CONTROL_MODE_POSITION_VELOCITY_PD: {
+ targetPosition = targetValue;
+ break;
+ }
+ case CONTROL_MODE_VELOCITY: {
+ targetVelocity = targetValue;
+ break;
+ }
+ case CONTROL_MODE_TORQUE: {
+ valid = 0;
+ break;
+ }
+ default: { valid = 0; }
+ }
+ }
+ if (size == 6) {
+ double gain = 0.0;
+ double targetValue = 0.0;
+ if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode,
+ &targetValue, &maxForce, &gain)) {
+ PyErr_SetString(SpamError, "Error parsing arguments");
+ return NULL;
+ }
+ valid = 1;
+
+ switch (controlMode) {
+ case CONTROL_MODE_POSITION_VELOCITY_PD: {
+ targetPosition = targetValue;
+ kp = gain;
+ break;
+ }
+ case CONTROL_MODE_VELOCITY: {
+ targetVelocity = targetValue;
+ kd = gain;
+ break;
+ }
+ case CONTROL_MODE_TORQUE: {
+ valid = 0;
+ break;
+ }
+ default: { valid = 0; }
+ }
+ }
+ if (size == 8) {
+ // only applicable for CONTROL_MODE_POSITION_VELOCITY_PD.
+ if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex,
+ &controlMode, &targetPosition, &targetVelocity,
+ &maxForce, &kp, &kd)) {
+ PyErr_SetString(SpamError, "Error parsing arguments");
+ return NULL;
+ }
+ valid = 1;
+ }
+
+ if (valid) {
+ int numJoints;
+ b3SharedMemoryCommandHandle commandHandle;
+ b3SharedMemoryStatusHandle statusHandle;
+ struct b3JointInfo info;
+
+ numJoints = b3GetNumJoints(sm, bodyIndex);
+ if ((jointIndex >= numJoints) || (jointIndex < 0)) {
+ PyErr_SetString(SpamError, "Joint index out-of-range.");
+ return NULL;
+ }
+
+ if ((controlMode != CONTROL_MODE_VELOCITY) &&
+ (controlMode != CONTROL_MODE_TORQUE) &&
+ (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) {
+ PyErr_SetString(SpamError, "Illegral control mode.");
+ return NULL;
+ }
+
+ commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode);
+
+ b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
+
+ switch (controlMode) {
+ case CONTROL_MODE_VELOCITY: {
+ b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,
+ targetVelocity);
+ b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+ b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
+ break;
+ }
+
+ case CONTROL_MODE_TORQUE: {
+ b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex,
+ appliedForce);
+ break;
+ }
+
+ case CONTROL_MODE_POSITION_VELOCITY_PD: {
+ b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex,
+ targetPosition);
+ b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
+ b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,
+ targetVelocity);
+ b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+ b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
+ break;
+ }
+ default: {}
+ };
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+
+ Py_INCREF(Py_None);
+ return Py_None;
+ }
+ PyErr_SetString(SpamError, "Error parsing arguments in setJointControl.");
+ return NULL;
+}
+
+static PyObject* pybullet_setRealTimeSimulation(PyObject* self,
+ PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ int enableRealTimeSimulation = 0;
+ int ret;
+
+ b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+ b3SharedMemoryStatusHandle statusHandle;
+
+ if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) {
+ PyErr_SetString(
+ SpamError,
+ "setRealTimeSimulation expected a single value (integer).");
+ return NULL;
+ }
+ ret =
+ b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+// Set the gravity of the world with (x, y, z) arguments
+static PyObject* pybullet_setGravity(PyObject* self, PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ double gravX = 0.0;
+ double gravY = 0.0;
+ double gravZ = -10.0;
+ int ret;
+
+ b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+ b3SharedMemoryStatusHandle statusHandle;
+
+ if (!PyArg_ParseTuple(args, "ddd", &gravX, &gravY, &gravZ)) {
+ PyErr_SetString(SpamError, "setGravity expected (x,y,z) values.");
+ return NULL;
+ }
+ ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ);
+ // ret = b3PhysicsParamSetTimeStep(command, timeStep);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ double timeStep = 0.001;
+ int ret;
+
+
+ b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+ b3SharedMemoryStatusHandle statusHandle;
+
+ if (!PyArg_ParseTuple(args, "d", &timeStep)) {
+ PyErr_SetString(SpamError,
+ "setTimeStep expected a single value (double).");
+ return NULL;
+ }
+ ret = b3PhysicsParamSetTimeStep(command, timeStep);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+static PyObject *
+pybullet_setDefaultContactERP(PyObject* self, PyObject* args)
+{
+ if (0==sm)
+ {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ double defaultContactERP=0.005;
+ int ret;
+
+ b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+ b3SharedMemoryStatusHandle statusHandle;
+
+ if (!PyArg_ParseTuple(args, "d", &defaultContactERP))
+ {
+ PyErr_SetString(SpamError, "default Contact ERP expected a single value (double).");
+ return NULL;
+ }
+ ret = b3PhysicsParamSetDefaultContactERP(command, defaultContactERP);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+
+// Internal function used to get the base position and orientation
+// Orientation is returned in quaternions
+static int pybullet_internalGetBasePositionAndOrientation(
+ int bodyIndex, double basePosition[3], double baseOrientation[3]) {
+ basePosition[0] = 0.;
+ basePosition[1] = 0.;
+ basePosition[2] = 0.;
+
+ baseOrientation[0] = 0.;
+ baseOrientation[1] = 0.;
+ baseOrientation[2] = 0.;
+ baseOrientation[3] = 1.;
+
+ {
+ {
+ b3SharedMemoryCommandHandle cmd_handle =
+ b3RequestActualStateCommandInit(sm, bodyIndex);
+ b3SharedMemoryStatusHandle status_handle =
+ b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
+
+ const int status_type = b3GetStatusType(status_handle);
+ const double* actualStateQ;
+ // const double* jointReactionForces[];
+
+
+ if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
+ PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
+ return 0;
+ }
+
+ b3GetStatusActualState(
+ status_handle, 0 /* body_unique_id */,
+ 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
+ 0 /*root_local_inertial_frame*/, &actualStateQ,
+ 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */);
+
+ // printf("joint reaction forces=");
+ // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) {
+ // printf("%f ", jointReactionForces[i]);
+ // }
+ // now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2]
+ // and orientation x,y,z,w =
+ // actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6]
+ basePosition[0] = actualStateQ[0];
+ basePosition[1] = actualStateQ[1];
+ basePosition[2] = actualStateQ[2];
+
+ baseOrientation[0] = actualStateQ[3];
+ baseOrientation[1] = actualStateQ[4];
+ baseOrientation[2] = actualStateQ[5];
+ baseOrientation[3] = actualStateQ[6];
+ }
+ }
+ return 1;
+}
+
+// Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion
+// values for the base link of your object
+// Object is retrieved based on body index, which is the order
+// the object was loaded into the simulation (0-based)
+static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
+ PyObject* args) {
+ int bodyIndex = -1;
+ double basePosition[3];
+ double baseOrientation[4];
+ PyObject* pylistPos;
+ PyObject* pylistOrientation;
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ if (!PyArg_ParseTuple(args, "i", &bodyIndex)) {
+ PyErr_SetString(SpamError, "Expected a body index (integer).");
+ return NULL;
+ }
+
+ if (0 == pybullet_internalGetBasePositionAndOrientation(
+ bodyIndex, basePosition, baseOrientation)) {
+ PyErr_SetString(SpamError,
+ "GetBasePositionAndOrientation failed (#joints/links "
+ "exceeds maximum?).");
+ return NULL;
+ }
+
+ {
+ PyObject* item;
+ int i;
+ int num = 3;
+ pylistPos = PyTuple_New(num);
+ for (i = 0; i < num; i++) {
+ item = PyFloat_FromDouble(basePosition[i]);
+ PyTuple_SetItem(pylistPos, i, item);
+ }
+ }
+
+ {
+ PyObject* item;
+ int i;
+ int num = 4;
+ pylistOrientation = PyTuple_New(num);
+ for (i = 0; i < num; i++) {
+ item = PyFloat_FromDouble(baseOrientation[i]);
+ PyTuple_SetItem(pylistOrientation, i, item);
+ }
+ }
+
+ {
+ PyObject* pylist;
+ pylist = PyTuple_New(2);
+ PyTuple_SetItem(pylist, 0, pylistPos);
+ PyTuple_SetItem(pylist, 1, pylistOrientation);
+ return pylist;
+ }
+}
+
+static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args)
+{
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ int numBodies = b3GetNumBodies(sm);
+
+#if PY_MAJOR_VERSION >= 3
+ return PyLong_FromLong(numBodies);
+#else
+ return PyInt_FromLong(numBodies);
+#endif
+ }
+}
+
+static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args)
+{
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ int serialIndex = -1;
+ int bodyUniqueId = -1;
+ if (!PyArg_ParseTuple(args, "i", &serialIndex)) {
+ PyErr_SetString(SpamError, "Expected a serialIndex in range [0..number of bodies).");
+ return NULL;
+ }
+ bodyUniqueId = b3GetBodyUniqueId(sm, serialIndex);
+
+#if PY_MAJOR_VERSION >= 3
+ return PyLong_FromLong(bodyUniqueId);
+#else
+ return PyInt_FromLong(bodyUniqueId);
+#endif
+ }
+}
+
+static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args)
+{
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ int bodyUniqueId= -1;
+ int numJoints = 0;
+ if (!PyArg_ParseTuple(args, "i", &bodyUniqueId))
+ {
+ PyErr_SetString(SpamError, "Expected a body unique id (integer).");
+ return NULL;
+ }
+ {
+ struct b3BodyInfo info;
+ if (b3GetBodyInfo(sm,bodyUniqueId,&info))
+ {
+ PyObject* pyListJointInfo = PyTuple_New(1);
+ PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName));
+ return pyListJointInfo;
+ } else
+ {
+ PyErr_SetString(SpamError, "Couldn't get body info");
+ return NULL;
+ }
+ }
+ }
+
+ PyErr_SetString(SpamError, "error in getBodyInfo.");
+ return NULL;
+}
+
+
+// Return the number of joints in an object based on
+// body index; body index is based on order of sequence
+// the object is loaded into simulation
+static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args)
+{
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ int bodyIndex = -1;
+ int numJoints = 0;
+ if (!PyArg_ParseTuple(args, "i", &bodyIndex)) {
+ PyErr_SetString(SpamError, "Expected a body index (integer).");
+ return NULL;
+ }
+ numJoints = b3GetNumJoints(sm, bodyIndex);
+
+#if PY_MAJOR_VERSION >= 3
+ return PyLong_FromLong(numJoints);
+#else
+ return PyInt_FromLong(numJoints);
+#endif
+ }
+}
+
+// Initalize all joint positions given a list of values
+static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) {
+ int size;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ size = PySequence_Size(args);
+
+ if (size == 3) {
+ int bodyIndex;
+ int jointIndex;
+ double targetValue;
+
+ if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue)) {
+ b3SharedMemoryCommandHandle commandHandle;
+ b3SharedMemoryStatusHandle statusHandle;
+ int numJoints;
+
+ numJoints = b3GetNumJoints(sm, bodyIndex);
+ if ((jointIndex >= numJoints) || (jointIndex < 0)) {
+ PyErr_SetString(SpamError, "Joint index out-of-range.");
+ return NULL;
+ }
+
+ commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
+
+ b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex,
+ targetValue);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+ Py_INCREF(Py_None);
+ return Py_None;
+ }
+ }
+ PyErr_SetString(SpamError, "error in resetJointState.");
+ return NULL;
+}
+
+// Reset the position and orientation of the base/root link, position [x,y,z]
+// and orientation quaternion [x,y,z,w]
+static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self,
+ PyObject* args) {
+ int size;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ size = PySequence_Size(args);
+
+ if (size == 3) {
+ int bodyIndex;
+ PyObject* posObj;
+ PyObject* ornObj;
+ double pos[3];
+ double orn[4]; // as a quaternion
+
+ if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) {
+ b3SharedMemoryCommandHandle commandHandle;
+ b3SharedMemoryStatusHandle statusHandle;
+
+ {
+ PyObject* seq;
+ int len, i;
+ seq = PySequence_Fast(posObj, "expected a sequence");
+ len = PySequence_Size(posObj);
+ if (len == 3) {
+ for (i = 0; i < 3; i++) {
+ pos[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ } else {
+ PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z].");
+ Py_DECREF(seq);
+ return NULL;
+ }
+ Py_DECREF(seq);
+ }
+
+ {
+ PyObject* seq;
+ int len, i;
+ seq = PySequence_Fast(ornObj, "expected a sequence");
+ len = PySequence_Size(ornObj);
+ if (len == 4) {
+ for (i = 0; i < 4; i++) {
+ orn[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ } else {
+ PyErr_SetString(
+ SpamError,
+ "orientation needs a 4 coordinates, quaternion [x,y,z,w].");
+ Py_DECREF(seq);
+ return NULL;
+ }
+ Py_DECREF(seq);
+ }
+
+ commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
+
+ b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]);
+ b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1],
+ orn[2], orn[3]);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+ Py_INCREF(Py_None);
+ return Py_None;
+ }
+ }
+ PyErr_SetString(SpamError, "error in resetJointState.");
+ return NULL;
+}
+
+// Get the a single joint info for a specific bodyIndex
+//
+// Args:
+// bodyIndex - integer indicating body in simulation
+// jointIndex - integer indicating joint for a specific body
+//
+// Joint information includes:
+// index, name, type, q-index, u-index,
+// flags, joint damping, joint friction
+//
+// The format of the returned list is
+// [int, str, int, int, int, int, float, float]
+//
+// TODO(hellojas): get joint positions for a body
+static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) {
+ PyObject* pyListJointInfo;
+
+ struct b3JointInfo info;
+
+ int bodyIndex = -1;
+ int jointIndex = -1;
+ int jointInfoSize = 8; // size of struct b3JointInfo
+
+ int size = PySequence_Size(args);
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ if (size == 2) // get body index and joint index
+ {
+ if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) {
+ // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex);
+
+ pyListJointInfo = PyTuple_New(jointInfoSize);
+
+ if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info)) {
+ // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n",
+ // info.m_jointIndex,
+ // info.m_jointName,
+ // info.m_jointType,
+ // info.m_qIndex,
+ // info.m_uIndex);
+ // printf(" flags=%d jointDamping=%f jointFriction=%f\n",
+ // info.m_flags,
+ // info.m_jointDamping,
+ // info.m_jointFriction);
+ PyTuple_SetItem(pyListJointInfo, 0, PyInt_FromLong(info.m_jointIndex));
+ PyTuple_SetItem(pyListJointInfo, 1,
+ PyString_FromString(info.m_jointName));
+ PyTuple_SetItem(pyListJointInfo, 2, PyInt_FromLong(info.m_jointType));
+ PyTuple_SetItem(pyListJointInfo, 3, PyInt_FromLong(info.m_qIndex));
+ PyTuple_SetItem(pyListJointInfo, 4, PyInt_FromLong(info.m_uIndex));
+ PyTuple_SetItem(pyListJointInfo, 5, PyInt_FromLong(info.m_flags));
+ PyTuple_SetItem(pyListJointInfo, 6,
+ PyFloat_FromDouble(info.m_jointDamping));
+ PyTuple_SetItem(pyListJointInfo, 7,
+ PyFloat_FromDouble(info.m_jointFriction));
+ return pyListJointInfo;
+ } else {
+ PyErr_SetString(SpamError, "GetJointInfo failed.");
+ return NULL;
+ }
+ }
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+// Returns the state of a specific joint in a given bodyIndex
+//
+// Args:
+// bodyIndex - integer indicating body in simulation
+// jointIndex - integer indicating joint for a specific body
+//
+// The state of a joint includes the following:
+// position, velocity, force torque (6 values), and motor torque
+// The returned pylist is an array of [float, float, float[6], float]
+
+// TODO(hellojas): check accuracy of position and velocity
+// TODO(hellojas): check force torque values
+
+static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) {
+ PyObject* pyListJointForceTorque;
+ PyObject* pyListJointState;
+
+
+ struct b3JointSensorState sensorState;
+
+ int bodyIndex = -1;
+ int jointIndex = -1;
+ int sensorStateSize = 4; // size of struct b3JointSensorState
+ int forceTorqueSize = 6; // size of force torque list from b3JointSensorState
+ int j;
+
+ int size = PySequence_Size(args);
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ if (size == 2) // get body index and joint index
+ {
+ if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) {
+ int status_type = 0;
+ b3SharedMemoryCommandHandle cmd_handle;
+ b3SharedMemoryStatusHandle status_handle;
+
+
+ if (bodyIndex < 0) {
+ PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex");
+ return NULL;
+ }
+ if (jointIndex < 0) {
+ PyErr_SetString(SpamError, "getJointState failed; invalid jointIndex");
+ return NULL;
+ }
+
+
+ cmd_handle =
+ b3RequestActualStateCommandInit(sm, bodyIndex);
+ status_handle =
+ b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
+
+ status_type = b3GetStatusType(status_handle);
+ if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
+ PyErr_SetString(SpamError, "getJointState failed.");
+ return NULL;
+ }
+
+ pyListJointState = PyTuple_New(sensorStateSize);
+ pyListJointForceTorque = PyTuple_New(forceTorqueSize);
+
+ b3GetJointState(sm, status_handle, jointIndex, &sensorState);
+
+ PyTuple_SetItem(pyListJointState, 0,
+ PyFloat_FromDouble(sensorState.m_jointPosition));
+ PyTuple_SetItem(pyListJointState, 1,
+ PyFloat_FromDouble(sensorState.m_jointVelocity));
+
+ for (j = 0; j < forceTorqueSize; j++) {
+ PyTuple_SetItem(pyListJointForceTorque, j,
+ PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
+ }
+
+ PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
+
+ PyTuple_SetItem(pyListJointState, 3,
+ PyFloat_FromDouble(sensorState.m_jointMotorTorque));
+
+ return pyListJointState;
+ }
+ } else {
+ PyErr_SetString(
+ SpamError,
+ "getJointState expects 2 arguments (objectUniqueId and joint index).");
+ return NULL;
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) {
+ PyObject* pyLinkState;
+ PyObject* pyLinkStateWorldPosition;
+ PyObject* pyLinkStateWorldOrientation;
+ PyObject* pyLinkStateLocalInertialPosition;
+ PyObject* pyLinkStateLocalInertialOrientation;
+
+ struct b3LinkState linkState;
+
+ int bodyIndex = -1;
+ int linkIndex = -1;
+ int i;
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ if (PySequence_Size(args) == 2) // body index and link index
+ {
+ if (PyArg_ParseTuple(args, "ii", &bodyIndex, &linkIndex)) {
+ int status_type = 0;
+ b3SharedMemoryCommandHandle cmd_handle;
+ b3SharedMemoryStatusHandle status_handle;
+
+ if (bodyIndex < 0) {
+ PyErr_SetString(SpamError, "getLinkState failed; invalid bodyIndex");
+ return NULL;
+ }
+ if (linkIndex < 0) {
+ PyErr_SetString(SpamError, "getLinkState failed; invalid jointIndex");
+ return NULL;
+ }
+
+
+ cmd_handle =
+ b3RequestActualStateCommandInit(sm, bodyIndex);
+ status_handle =
+ b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
+
+ status_type = b3GetStatusType(status_handle);
+ if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
+ PyErr_SetString(SpamError, "getLinkState failed.");
+ return NULL;
+ }
+
+ b3GetLinkState(sm, status_handle, linkIndex, &linkState);
+
+ pyLinkStateWorldPosition = PyTuple_New(3);
+ for (i = 0; i < 3; ++i) {
+ PyTuple_SetItem(pyLinkStateWorldPosition, i,
+ PyFloat_FromDouble(linkState.m_worldPosition[i]));
+ }
+
+ pyLinkStateWorldOrientation = PyTuple_New(4);
+ for (i = 0; i < 4; ++i) {
+ PyTuple_SetItem(pyLinkStateWorldOrientation, i,
+ PyFloat_FromDouble(linkState.m_worldOrientation[i]));
+ }
+
+ pyLinkStateLocalInertialPosition = PyTuple_New(3);
+ for (i = 0; i < 3; ++i) {
+ PyTuple_SetItem(pyLinkStateLocalInertialPosition, i,
+ PyFloat_FromDouble(linkState.m_localInertialPosition[i]));
+ }
+
+ pyLinkStateLocalInertialOrientation = PyTuple_New(4);
+ for (i = 0; i < 4; ++i) {
+ PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i,
+ PyFloat_FromDouble(linkState.m_localInertialOrientation[i]));
+ }
+
+ pyLinkState = PyTuple_New(4);
+ PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition);
+ PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation);
+ PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition);
+ PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation);
+
+ return pyLinkState;
+ }
+ } else {
+ PyErr_SetString(
+ SpamError,
+ "getLinkState expects 2 arguments (objectUniqueId and link index).");
+ return NULL;
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+// internal function to set a float matrix[16]
+// used to initialize camera position with
+// a view and projection matrix in renderImage()
+//
+// // Args:
+// matrix - float[16] which will be set by values from objMat
+static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
+ int i, len;
+ PyObject* seq;
+
+ seq = PySequence_Fast(objMat, "expected a sequence");
+ len = PySequence_Size(objMat);
+ if (len == 16) {
+ for (i = 0; i < len; i++) {
+ matrix[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ Py_DECREF(seq);
+ return 1;
+ }
+ Py_DECREF(seq);
+ return 0;
+}
+
+// internal function to set a float vector[3]
+// used to initialize camera position with
+// a view and projection matrix in renderImage()
+//
+// // Args:
+// vector - float[3] which will be set by values from objMat
+static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) {
+ int i, len;
+ PyObject* seq;
+
+ seq = PySequence_Fast(objMat, "expected a sequence");
+ len = PySequence_Size(objMat);
+ if (len == 3) {
+ for (i = 0; i < len; i++) {
+ vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ Py_DECREF(seq);
+ return 1;
+ }
+ Py_DECREF(seq);
+ return 0;
+}
+
+// vector - double[3] which will be set by values from obVec
+static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
+ int i, len;
+ PyObject* seq;
+
+ seq = PySequence_Fast(obVec, "expected a sequence");
+ len = PySequence_Size(obVec);
+ if (len == 3) {
+ for (i = 0; i < len; i++) {
+ vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ Py_DECREF(seq);
+ return 1;
+ }
+ Py_DECREF(seq);
+ return 0;
+}
+
+// vector - double[3] which will be set by values from obVec
+static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
+ int i, len;
+ PyObject* seq;
+
+ seq = PySequence_Fast(obVec, "expected a sequence");
+ len = PySequence_Size(obVec);
+ if (len == 4) {
+ for (i = 0; i < len; i++) {
+ vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ Py_DECREF(seq);
+ return 1;
+ }
+ Py_DECREF(seq);
+ return 0;
+}
+
+static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
+ int size = PySequence_Size(args);
+ int objectUniqueIdA = -1;
+ int objectUniqueIdB = -1;
+ b3SharedMemoryCommandHandle commandHandle;
+ struct b3ContactInformation contactPointData;
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ int i;
+ PyObject* pyResultList = 0;
+
+ if (size == 1) {
+ if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA)) {
+ PyErr_SetString(SpamError, "Error parsing object unique id");
+ return NULL;
+ }
+ }
+ if (size == 2) {
+ if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA, &objectUniqueIdB)) {
+ PyErr_SetString(SpamError, "Error parsing object unique id");
+ return NULL;
+ }
+ }
+
+ commandHandle = b3InitRequestContactPointInformation(sm);
+ b3SetContactFilterBodyA(commandHandle, objectUniqueIdA);
+ b3SetContactFilterBodyB(commandHandle, objectUniqueIdB);
+ b3SubmitClientCommand(sm, commandHandle);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+ statusType = b3GetStatusType(statusHandle);
+ if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
+ /*
+ 0 int m_contactFlags;
+ 1 int m_bodyUniqueIdA;
+ 2 int m_bodyUniqueIdB;
+ 3 int m_linkIndexA;
+ 4 int m_linkIndexB;
+ 5-6-7 double m_positionOnAInWS[3];//contact point location on object A,
+ in world space coordinates
+ 8-9-10 double m_positionOnBInWS[3];//contact point location on object
+ A, in world space coordinates
+ 11-12-13 double m_contactNormalOnBInWS[3];//the separating contact
+ normal, pointing from object B towards object A
+ 14 double m_contactDistance;//negative number is penetration, positive
+ is distance.
+
+ 15 double m_normalForce;
+ */
+
+ b3GetContactPointInformation(sm, &contactPointData);
+ pyResultList = PyTuple_New(contactPointData.m_numContactPoints);
+ for (i = 0; i < contactPointData.m_numContactPoints; i++) {
+ PyObject* contactObList = PyTuple_New(16); // see above 16 fields
+ PyObject* item;
+ item =
+ PyInt_FromLong(contactPointData.m_contactPointData[i].m_contactFlags);
+ PyTuple_SetItem(contactObList, 0, item);
+ item = PyInt_FromLong(
+ contactPointData.m_contactPointData[i].m_bodyUniqueIdA);
+ PyTuple_SetItem(contactObList, 1, item);
+ item = PyInt_FromLong(
+ contactPointData.m_contactPointData[i].m_bodyUniqueIdB);
+ PyTuple_SetItem(contactObList, 2, item);
+ item =
+ PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexA);
+ PyTuple_SetItem(contactObList, 3, item);
+ item =
+ PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexB);
+ PyTuple_SetItem(contactObList, 4, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnAInWS[0]);
+ PyTuple_SetItem(contactObList, 5, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnAInWS[1]);
+ PyTuple_SetItem(contactObList, 6, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnAInWS[2]);
+ PyTuple_SetItem(contactObList, 7, item);
+
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnBInWS[0]);
+ PyTuple_SetItem(contactObList, 8, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnBInWS[1]);
+ PyTuple_SetItem(contactObList, 9, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_positionOnBInWS[2]);
+ PyTuple_SetItem(contactObList, 10, item);
+
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[0]);
+ PyTuple_SetItem(contactObList, 11, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[1]);
+ PyTuple_SetItem(contactObList, 12, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[2]);
+ PyTuple_SetItem(contactObList, 13, item);
+
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_contactDistance);
+ PyTuple_SetItem(contactObList, 14, item);
+ item = PyFloat_FromDouble(
+ contactPointData.m_contactPointData[i].m_normalForce);
+ PyTuple_SetItem(contactObList, 15, item);
+
+ PyTuple_SetItem(pyResultList, i, contactObList);
+ }
+ return pyResultList;
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+// Render an image from the current timestep of the simulation
+//
+// Examples:
+// renderImage() - default image resolution and camera position
+// renderImage(w, h) - image resolution of (w,h), default camera
+// renderImage(w, h, view[16], projection[16]) - set both resolution
+// and initialize camera to the view and projection values
+// renderImage(w, h, cameraPos, targetPos, cameraUp, nearVal, farVal) - set
+// resolution and initialize camera based on camera position, target
+// position, camera up and fulstrum near/far values.
+// renderImage(w, h, cameraPos, targetPos, cameraUp, nearVal, farVal, fov) -
+// set resolution and initialize camera based on camera position, target
+// position, camera up, fulstrum near/far values and camera field of view.
+// renderImage(w, h, targetPos, distance, yaw, pitch, upAxisIndex, nearVal,
+// farVal, fov)
+
+//
+// Note if the (w,h) is too small, the objects may not appear based on
+// where the camera has been set
+//
+// TODO(hellojas): fix image is cut off at head
+// TODO(hellojas): should we add check to give minimum image resolution
+// to see object based on camera position?
+static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) {
+ /// request an image from a simulated camera, using a software renderer.
+ struct b3CameraImageData imageData;
+ PyObject* objViewMat, *objProjMat;
+ PyObject* objCameraPos, *objTargetPos, *objCameraUp;
+
+ int width, height;
+ int size = PySequence_Size(args);
+ float viewMatrix[16];
+ float projectionMatrix[16];
+
+ float cameraPos[3];
+ float targetPos[3];
+ float cameraUp[3];
+
+ float left, right, bottom, top, aspect;
+ float nearVal, farVal;
+ float fov;
+
+ // inialize cmd
+ b3SharedMemoryCommandHandle command;
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ command = b3InitRequestCameraImage(sm);
+
+ if (size == 2) // only set camera resolution
+ {
+ if (PyArg_ParseTuple(args, "ii", &width, &height)) {
+ b3RequestCameraImageSetPixelResolution(command, width, height);
+ }
+ } else if (size == 4) // set camera resolution and view and projection matrix
+ {
+ if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat,
+ &objProjMat)) {
+ b3RequestCameraImageSetPixelResolution(command, width, height);
+
+ // set camera matrices only if set matrix function succeeds
+ if (pybullet_internalSetMatrix(objViewMat, viewMatrix) &&
+ (pybullet_internalSetMatrix(objProjMat, projectionMatrix))) {
+ b3RequestCameraImageSetCameraMatrices(command, viewMatrix,
+ projectionMatrix);
+ } else {
+ PyErr_SetString(SpamError, "Error parsing view or projection matrix.");
+ return NULL;
+ }
+ }
+ } else if (size == 7) // set camera resolution, camera positions and
+ // calculate projection using near/far values.
+ {
+ if (PyArg_ParseTuple(args, "iiOOOff", &width, &height, &objCameraPos,
+ &objTargetPos, &objCameraUp, &nearVal, &farVal)) {
+ b3RequestCameraImageSetPixelResolution(command, width, height);
+ if (pybullet_internalSetVector(objCameraPos, cameraPos) &&
+ pybullet_internalSetVector(objTargetPos, targetPos) &&
+ pybullet_internalSetVector(objCameraUp, cameraUp)) {
+ b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos,
+ cameraUp);
+ } else {
+ PyErr_SetString(SpamError,
+ "Error parsing camera position, target or up.");
+ return NULL;
+ }
+
+ aspect = width / height;
+ left = -aspect * nearVal;
+ right = aspect * nearVal;
+ bottom = -nearVal;
+ top = nearVal;
+ b3RequestCameraImageSetProjectionMatrix(command, left, right, bottom, top,
+ nearVal, farVal);
+ }
+ } else if (size == 8) // set camera resolution, camera positions and
+ // calculate projection using near/far values & field
+ // of view
+ {
+ if (PyArg_ParseTuple(args, "iiOOOfff", &width, &height, &objCameraPos,
+ &objTargetPos, &objCameraUp, &nearVal, &farVal,
+ &fov)) {
+ b3RequestCameraImageSetPixelResolution(command, width, height);
+ if (pybullet_internalSetVector(objCameraPos, cameraPos) &&
+ pybullet_internalSetVector(objTargetPos, targetPos) &&
+ pybullet_internalSetVector(objCameraUp, cameraUp)) {
+ b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos,
+ cameraUp);
+ } else {
+ PyErr_SetString(SpamError,
+ "Error parsing camera position, target or up.");
+ return NULL;
+ }
+
+ aspect = width / height;
+ b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal,
+ farVal);
+ }
+ } else if (size == 11) {
+ int upAxisIndex = 1;
+ float camDistance, yaw, pitch, roll;
+
+ // sometimes more arguments are better :-)
+ if (PyArg_ParseTuple(args, "iiOffffifff", &width, &height, &objTargetPos,
+ &camDistance, &yaw, &pitch, &roll, &upAxisIndex,
+ &nearVal, &farVal, &fov)) {
+ b3RequestCameraImageSetPixelResolution(command, width, height);
+ if (pybullet_internalSetVector(objTargetPos, targetPos)) {
+ // printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f,
+ // yaw = %f, pitch = %f, upAxisIndex = %d, near=%f, far=%f,
+ // fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov);
+
+ b3RequestCameraImageSetViewMatrix2(command, targetPos, camDistance, yaw,
+ pitch, roll, upAxisIndex);
+ aspect = width / height;
+ b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect,
+ nearVal, farVal);
+ } else {
+ PyErr_SetString(SpamError, "Error parsing camera target pos");
+ }
+ } else {
+ PyErr_SetString(SpamError, "Error parsing arguments");
+ }
+
+ } else {
+ PyErr_SetString(SpamError, "Invalid number of args passed to renderImage.");
+ return NULL;
+ }
+
+ if (b3CanSubmitCommand(sm)) {
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+
+ // b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ statusType = b3GetStatusType(statusHandle);
+ if (statusType == CMD_CAMERA_IMAGE_COMPLETED) {
+ PyObject* item2;
+ PyObject* pyResultList; // store 4 elements in this result: width,
+ // height, rgbData, depth
+
+#ifdef PYBULLET_USE_NUMPY
+ PyObject* pyRGB;
+ PyObject* pyDep;
+ PyObject* pySeg;
+
+ int i, j, p;
+
+ b3GetCameraImageData(sm, &imageData);
+ // TODO(hellojas): error handling if image size is 0
+ pyResultList = PyTuple_New(5);
+ PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
+ PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
+
+ int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values
+
+ npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth,
+ bytesPerPixel};
+ npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth};
+ npy_intp seg_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth};
+
+ pyRGB = PyArray_SimpleNew(3, rgb_dims, NPY_UINT8);
+ pyDep = PyArray_SimpleNew(2, dep_dims, NPY_FLOAT32);
+ pySeg = PyArray_SimpleNew(2, seg_dims, NPY_INT32);
+
+ memcpy(PyArray_DATA(pyRGB), imageData.m_rgbColorData,
+ imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel);
+ memcpy(PyArray_DATA(pyDep), imageData.m_depthValues,
+ imageData.m_pixelHeight * imageData.m_pixelWidth);
+ memcpy(PyArray_DATA(pySeg), imageData.m_segmentationMaskValues,
+ imageData.m_pixelHeight * imageData.m_pixelWidth);
+
+ PyTuple_SetItem(pyResultList, 2, pyRGB);
+ PyTuple_SetItem(pyResultList, 3, pyDep);
+ PyTuple_SetItem(pyResultList, 4, pySeg);
+#else//PYBULLET_USE_NUMPY
+ PyObject* pylistRGB;
+ PyObject* pylistDep;
+ PyObject* pylistSeg;
+
+ int i, j, p;
+
+ b3GetCameraImageData(sm, &imageData);
+ // TODO(hellojas): error handling if image size is 0
+ pyResultList = PyTuple_New(5);
+ PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
+ PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
+
+ {
+ PyObject* item;
+ int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values
+ int num =
+ bytesPerPixel * imageData.m_pixelWidth * imageData.m_pixelHeight;
+ pylistRGB = PyTuple_New(num);
+ pylistDep =
+ PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight);
+ pylistSeg =
+ PyTuple_New(imageData.m_pixelWidth * imageData.m_pixelHeight);
+ for (i = 0; i < imageData.m_pixelWidth; i++) {
+ for (j = 0; j < imageData.m_pixelHeight; j++) {
+ // TODO(hellojas): validate depth values make sense
+ int depIndex = i + j * imageData.m_pixelWidth;
+ {
+ item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]);
+ PyTuple_SetItem(pylistDep, depIndex, item);
+ }
+ {
+ item2 =
+ PyLong_FromLong(imageData.m_segmentationMaskValues[depIndex]);
+ PyTuple_SetItem(pylistSeg, depIndex, item2);
+ }
+
+ for (p = 0; p < bytesPerPixel; p++) {
+ int pixelIndex =
+ bytesPerPixel * (i + j * imageData.m_pixelWidth) + p;
+ item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]);
+ PyTuple_SetItem(pylistRGB, pixelIndex, item);
+ }
+ }
+ }
+ }
+
+ PyTuple_SetItem(pyResultList, 2, pylistRGB);
+ PyTuple_SetItem(pyResultList, 3, pylistDep);
+ PyTuple_SetItem(pyResultList, 4, pylistSeg);
+ return pyResultList;
+#endif//PYBULLET_USE_NUMPY
+
+ return pyResultList;
+ }
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+ {
+ int objectUniqueId, linkIndex, flags;
+ double force[3];
+ double position[3] = {0.0, 0.0, 0.0};
+ PyObject* forceObj, *posObj;
+
+ b3SharedMemoryCommandHandle command;
+ b3SharedMemoryStatusHandle statusHandle;
+ int size = PySequence_Size(args);
+
+ if (size == 5) {
+ if (!PyArg_ParseTuple(args, "iiOOi", &objectUniqueId, &linkIndex,
+ &forceObj, &posObj, &flags)) {
+ PyErr_SetString(SpamError, "applyBaseForce couldn't parse arguments");
+ return NULL;
+ }
+ } else {
+ PyErr_SetString(SpamError,
+ "applyBaseForce needs 5 arguments: objectUniqueId, "
+ "linkIndex (-1 for base/root link), force [x,y,z], "
+ "position [x,y,z], flags");
+
+ return NULL;
+ }
+
+ {
+ PyObject* seq;
+ int len, i;
+ seq = PySequence_Fast(forceObj, "expected a sequence");
+ len = PySequence_Size(forceObj);
+ if (len == 3) {
+ for (i = 0; i < 3; i++) {
+ force[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ } else {
+ PyErr_SetString(SpamError, "force needs a 3 coordinates [x,y,z].");
+ Py_DECREF(seq);
+ return NULL;
+ }
+ Py_DECREF(seq);
+ }
+ {
+ PyObject* seq;
+ int len, i;
+ seq = PySequence_Fast(posObj, "expected a sequence");
+ len = PySequence_Size(posObj);
+ if (len == 3) {
+ for (i = 0; i < 3; i++) {
+ position[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ } else {
+ PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z].");
+ Py_DECREF(seq);
+ return NULL;
+ }
+ Py_DECREF(seq);
+ }
+ if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) {
+ PyErr_SetString(SpamError,
+ "flag has to be either WORLD_FRAME or LINK_FRAME");
+ return NULL;
+ }
+ command = b3ApplyExternalForceCommandInit(sm);
+ b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position,
+ flags);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ }
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args) {
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+ {
+ int objectUniqueId, linkIndex, flags;
+ double torque[3];
+ PyObject* torqueObj;
+
+ if (PyArg_ParseTuple(args, "iiOi", &objectUniqueId, &linkIndex, &torqueObj,
+ &flags)) {
+ PyObject* seq;
+ int len, i;
+ seq = PySequence_Fast(torqueObj, "expected a sequence");
+ len = PySequence_Size(torqueObj);
+ if (len == 3) {
+ for (i = 0; i < 3; i++) {
+ torque[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ } else {
+ PyErr_SetString(SpamError, "torque needs a 3 coordinates [x,y,z].");
+ Py_DECREF(seq);
+ return NULL;
+ }
+ Py_DECREF(seq);
+
+ if (linkIndex < -1) {
+ PyErr_SetString(SpamError,
+ "Invalid link index, has to be -1 or larger");
+ return NULL;
+ }
+ if ((flags != EF_WORLD_FRAME) && (flags != EF_LINK_FRAME)) {
+ PyErr_SetString(SpamError,
+ "flag has to be either WORLD_FRAME or LINK_FRAME");
+ return NULL;
+ }
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ b3SharedMemoryCommandHandle command =
+ b3ApplyExternalForceCommandInit(sm);
+ b3ApplyExternalTorque(command, objectUniqueId, -1, torque, flags);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ }
+ }
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+static PyObject* pybullet_getQuaternionFromEuler(PyObject* self,
+ PyObject* args) {
+ double rpy[3];
+
+ PyObject* eulerObj;
+
+ if (PyArg_ParseTuple(args, "O", &eulerObj)) {
+ PyObject* seq;
+ int len, i;
+ seq = PySequence_Fast(eulerObj, "expected a sequence");
+ len = PySequence_Size(eulerObj);
+ if (len == 3) {
+ for (i = 0; i < 3; i++) {
+ rpy[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ } else {
+ PyErr_SetString(SpamError,
+ "Euler angles need a 3 coordinates [roll, pitch, yaw].");
+ Py_DECREF(seq);
+ return NULL;
+ }
+ Py_DECREF(seq);
+ } else {
+ PyErr_SetString(SpamError,
+ "Euler angles need a 3 coordinates [roll, pitch, yaw].");
+ return NULL;
+ }
+
+ {
+ double phi, the, psi;
+ double roll = rpy[0];
+ double pitch = rpy[1];
+ double yaw = rpy[2];
+ phi = roll / 2.0;
+ the = pitch / 2.0;
+ psi = yaw / 2.0;
+ {
+ double quat[4] = {
+ sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
+ cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
+ cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
+ cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)};
+
+ // normalize the quaternion
+ double len = sqrt(quat[0] * quat[0] + quat[1] * quat[1] +
+ quat[2] * quat[2] + quat[3] * quat[3]);
+ quat[0] /= len;
+ quat[1] /= len;
+ quat[2] /= len;
+ quat[3] /= len;
+ {
+ PyObject* pylist;
+ int i;
+ pylist = PyTuple_New(4);
+ for (i = 0; i < 4; i++)
+ PyTuple_SetItem(pylist, i, PyFloat_FromDouble(quat[i]));
+ return pylist;
+ }
+ }
+ }
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+/// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
+/// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
+static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
+ PyObject* args) {
+ double squ;
+ double sqx;
+ double sqy;
+ double sqz;
+
+ double quat[4];
+
+ PyObject* quatObj;
+
+ if (PyArg_ParseTuple(args, "O", &quatObj)) {
+ PyObject* seq;
+ int len, i;
+ seq = PySequence_Fast(quatObj, "expected a sequence");
+ len = PySequence_Size(quatObj);
+ if (len == 4) {
+ for (i = 0; i < 4; i++) {
+ quat[i] = pybullet_internalGetFloatFromSequence(seq, i);
+ }
+ } else {
+ PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
+ Py_DECREF(seq);
+ return NULL;
+ }
+ Py_DECREF(seq);
+ } else {
+ PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
+ return NULL;
+ }
+
+ {
+ double rpy[3];
+ double sarg;
+ sqx = quat[0] * quat[0];
+ sqy = quat[1] * quat[1];
+ sqz = quat[2] * quat[2];
+ squ = quat[3] * quat[3];
+ rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]),
+ squ - sqx - sqy + sqz);
+ sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]);
+ rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538
+ : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg));
+ rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]),
+ squ + sqx - sqy - sqz);
+ {
+ PyObject* pylist;
+ int i;
+ pylist = PyTuple_New(3);
+ for (i = 0; i < 3; i++)
+ PyTuple_SetItem(pylist, i, PyFloat_FromDouble(rpy[i]));
+ return pylist;
+ }
+ }
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+
+///Experimental Inverse Kinematics binding ,7-dof KUKA IIWA only
+static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self,
+ PyObject* args) {
+ int size;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ size = PySequence_Size(args);
+ if (size == 2)
+ {
+ int bodyIndex;
+ int endEffectorLinkIndex;
+
+ PyObject* targetPosObj;
+ PyObject* targetOrnObj;
+
+ if (PyArg_ParseTuple(args, "iiOO", &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj))
+ {
+ double pos[3];
+ double ori[4]={0,1.0,0,0};
+
+ if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4(targetOrnObj,ori))
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ int numPos=0;
+ int resultBodyIndex;
+ int result;
+
+ b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex);
+ b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+
+ result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
+ &resultBodyIndex,
+ &numPos,
+ 0);
+ if (result && numPos)
+ {
+ int i;
+ PyObject* pylist;
+ double* ikOutPutJointPos = (double*)malloc(numPos*sizeof(double));
+ result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
+ &resultBodyIndex,
+ &numPos,
+ ikOutPutJointPos);
+ pylist = PyTuple_New(numPos);
+ for (i = 0; i < numPos; i++)
+ {
+ PyTuple_SetItem(pylist, i,
+ PyFloat_FromDouble(ikOutPutJointPos[i]));
+ }
+
+ free(ikOutPutJointPos);
+ return pylist;
+ }
+ else
+ {
+ PyErr_SetString(SpamError,
+ "Error in calculateInverseKinematics");
+ return NULL;
+ }
+ } else
+ {
+ PyErr_SetString(SpamError,
+ "calculateInverseKinematics couldn't extract position vector3");
+ return NULL;
+ }
+ }
+ } else
+ {
+ PyErr_SetString(SpamError,
+ "calculateInverseKinematics expects 2 arguments, body index, "
+ "and target position for end effector");
+ return NULL;
+ }
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+
+
+/// Given an object id, joint positions, joint velocities and joint
+/// accelerations,
+/// compute the joint forces using Inverse Dynamics
+static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
+ PyObject* args) {
+ int size;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ size = PySequence_Size(args);
+ if (size == 4) {
+ int bodyIndex;
+ PyObject* objPositionsQ;
+ PyObject* objVelocitiesQdot;
+ PyObject* objAccelerations;
+
+ if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ,
+ &objVelocitiesQdot, &objAccelerations)) {
+ int szObPos = PySequence_Size(objPositionsQ);
+ int szObVel = PySequence_Size(objVelocitiesQdot);
+ int szObAcc = PySequence_Size(objAccelerations);
+ int numJoints = b3GetNumJoints(sm, bodyIndex);
+ if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) &&
+ (szObAcc == numJoints)) {
+ int szInBytes = sizeof(double) * numJoints;
+ int i;
+ PyObject* pylist = 0;
+ double* jointPositionsQ = (double*)malloc(szInBytes);
+ double* jointVelocitiesQdot = (double*)malloc(szInBytes);
+ double* jointAccelerations = (double*)malloc(szInBytes);
+ double* jointForcesOutput = (double*)malloc(szInBytes);
+
+ for (i = 0; i < numJoints; i++) {
+ jointPositionsQ[i] =
+ pybullet_internalGetFloatFromSequence(objPositionsQ, i);
+ jointVelocitiesQdot[i] =
+ pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i);
+ jointAccelerations[i] =
+ pybullet_internalGetFloatFromSequence(objAccelerations, i);
+ }
+
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ b3SharedMemoryCommandHandle commandHandle =
+ b3CalculateInverseDynamicsCommandInit(
+ sm, bodyIndex, jointPositionsQ, jointVelocitiesQdot,
+ jointAccelerations);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+
+ statusType = b3GetStatusType(statusHandle);
+
+ if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) {
+ int bodyUniqueId;
+ int dofCount;
+
+ b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,
+ &dofCount, 0);
+
+ if (dofCount) {
+ b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,
+ jointForcesOutput);
+ {
+ {
+ int i;
+ pylist = PyTuple_New(dofCount);
+ for (i = 0; i < dofCount; i++)
+ PyTuple_SetItem(pylist, i,
+ PyFloat_FromDouble(jointForcesOutput[i]));
+ }
+ }
+ }
+
+ } else {
+ PyErr_SetString(SpamError,
+ "Internal error in calculateInverseDynamics");
+ }
+ }
+ free(jointPositionsQ);
+ free(jointVelocitiesQdot);
+ free(jointAccelerations);
+ free(jointForcesOutput);
+ if (pylist) return pylist;
+ } else {
+ PyErr_SetString(SpamError,
+ "calculateInverseDynamics numJoints needs to be "
+ "positive and [joint positions], [joint velocities], "
+ "[joint accelerations] need to match the number of "
+ "joints.");
+ return NULL;
+ }
+
+ } else {
+ PyErr_SetString(SpamError,
+ "calculateInverseDynamics expects 4 arguments, body "
+ "index, [joint positions], [joint velocities], [joint "
+ "accelerations].");
+ return NULL;
+ }
+ } else {
+ PyErr_SetString(SpamError,
+ "calculateInverseDynamics expects 4 arguments, body index, "
+ "[joint positions], [joint velocities], [joint "
+ "accelerations].");
+ return NULL;
+ }
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+static PyMethodDef SpamMethods[] = {
+
+ {"connect", pybullet_connectPhysicsServer, METH_VARARGS,
+ "Connect to an existing physics server (using shared memory by default)."},
+
+ {"disconnect", pybullet_disconnectPhysicsServer, METH_VARARGS,
+ "Disconnect from the physics server."},
+
+ {"resetSimulation", pybullet_resetSimulation, METH_VARARGS,
+ "Reset the simulation: remove all objects and start from an empty world."},
+
+ {"stepSimulation", pybullet_stepSimulation, METH_VARARGS,
+ "Step the simulation using forward dynamics."},
+
+ {"setGravity", pybullet_setGravity, METH_VARARGS,
+ "Set the gravity acceleration (x,y,z)."},
+
+ {"setTimeStep", pybullet_setTimeStep, METH_VARARGS,
+ "Set the amount of time to proceed at each call to stepSimulation. (unit "
+ "is seconds, typically range is 0.01 or 0.001)"},
+
+
+ {"setTimeStep", pybullet_setTimeStep, METH_VARARGS,
+ "Set the amount of time to proceed at each call to stepSimulation."
+ " (unit is seconds, typically range is 0.01 or 0.001)"},
+
+ {"setDefaultContactERP", pybullet_setDefaultContactERP, METH_VARARGS,
+ "Set the amount of contact penetration Error Recovery Paramater "
+ "(ERP) in each time step. \
+ This is an tuning parameter to control resting contact stability. "
+ "This value depends on the time step."},
+
+ { "setRealTimeSimulation", pybullet_setRealTimeSimulation, METH_VARARGS,
+ "Enable or disable real time simulation (using the real time clock,"
+ " RTC) in the physics server. Expects one integer argument, 0 or 1" },
+
+ {"loadURDF", pybullet_loadURDF, METH_VARARGS,
+ "Create a multibody by loading a URDF file."},
+
+ {"loadSDF", pybullet_loadSDF, METH_VARARGS,
+ "Load multibodies from an SDF file."},
+
+ {"saveWorld", pybullet_saveWorld, METH_VARARGS,
+ "Save an approximate Python file to reproduce the current state of the world: saveWorld"
+ "(filename). (very preliminary and approximately)"},
+
+ {"getNumBodies", pybullet_getNumBodies, METH_VARARGS,
+ "Get the number of bodies in the simulation."},
+
+ {"getBodyUniqueId", pybullet_getBodyUniqueId, METH_VARARGS,
+ "Get the unique id of the body, given a integer serial index in range [0.. number of bodies)."},
+
+ {"getBodyInfo", pybullet_getBodyInfo, METH_VARARGS,
+ "Get the body info, given a body unique id."},
+
+ {"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation,
+ METH_VARARGS,
+ "Get the world position and orientation of the base of the object. "
+ "(x,y,z) position vector and (x,y,z,w) quaternion orientation."},
+
+ {"resetBasePositionAndOrientation",
+ pybullet_resetBasePositionAndOrientation, METH_VARARGS,
+ "Reset the world position and orientation of the base of the object "
+ "instantaneously, not through physics simulation. (x,y,z) position vector "
+ "and (x,y,z,w) quaternion orientation."},
+
+ {"getNumJoints", pybullet_getNumJoints, METH_VARARGS,
+ "Get the number of joints for an object."},
+
+ {"getJointInfo", pybullet_getJointInfo, METH_VARARGS,
+ "Get the name and type info for a joint on a body."},
+
+ {"getJointState", pybullet_getJointState, METH_VARARGS,
+ "Get the state (position, velocity etc) for a joint on a body."},
+
+ {"getLinkState", pybullet_getLinkState, METH_VARARGS,
+ "Provides extra information such as the Cartesian world coordinates"
+ " center of mass (COM) of the link, relative to the world reference"
+ " frame."},
+
+ {"resetJointState", pybullet_resetJointState, METH_VARARGS,
+ "Reset the state (position, velocity etc) for a joint on a body "
+ "instantaneously, not through physics simulation."},
+
+ {"setJointMotorControl", pybullet_setJointMotorControl, METH_VARARGS,
+ "Set a single joint motor control mode and desired target value. There is "
+ "no immediate state change, stepSimulation will process the motors."},
+
+ {"applyExternalForce", pybullet_applyExternalForce, METH_VARARGS,
+ "for objectUniqueId, linkIndex (-1 for base/root link), apply a force "
+ "[x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or "
+ "FORCE_IN_WORLD_FRAME coordinates"},
+
+ {"applyExternalTorque", pybullet_applyExternalTorque, METH_VARARGS,
+ "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque "
+ "[x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or "
+ "TORQUE_IN_WORLD_FRAME coordinates"},
+
+ {"renderImage", pybullet_renderImage, METH_VARARGS,
+ "Render an image (given the pixel resolution width, height, camera view "
+ "matrix, projection matrix, near, and far values), and return the "
+ "8-8-8bit RGB pixel data and floating point depth values"
+#ifdef PYBULLET_USE_NUMPY
+ " as NumPy arrays"
+#endif
+ },
+
+ {"getContactPointData", pybullet_getContactPointData, METH_VARARGS,
+ "Return the contact point information for all or some of pairwise "
+ "object-object collisions. Optional arguments one or two object unique "
+ "ids, that need to be involved in the contact."},
+
+ {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS,
+ "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to "
+ "quaternion [x,y,z,w]"},
+
+ {"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS,
+ "Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF "
+ "convention"},
+
+ {"calculateInverseDynamics", pybullet_calculateInverseDynamics,
+ METH_VARARGS,
+ "Given an object id, joint positions, joint velocities and joint "
+ "accelerations, compute the joint forces using Inverse Dynamics"},
+
+ {"calculateInverseKinematicsKuka", pybullet_calculateInverseKinematicsKuka,
+ METH_VARARGS,
+ "Experimental, KUKA IIWA only: Given an object id, "
+ "current joint positions and target position"
+ " for the end effector,"
+ "compute the inverse kinematics and return the new joint state"},
+
+ // todo(erwincoumans)
+ // saveSnapshot
+ // loadSnapshot
+ // raycast info
+ // object names
+
+ {NULL, NULL, 0, NULL} /* Sentinel */
+};
+
+#if PY_MAJOR_VERSION >= 3
+static struct PyModuleDef moduledef = {
+ PyModuleDef_HEAD_INIT, "pybullet", /* m_name */
+ "Python bindings for Bullet Physics Robotics API (also known as Shared "
+ "Memory API)", /* m_doc */
+ -1, /* m_size */
+ SpamMethods, /* m_methods */
+ NULL, /* m_reload */
+ NULL, /* m_traverse */
+ NULL, /* m_clear */
+ NULL, /* m_free */
+};
+#endif
+
+PyMODINIT_FUNC
+#if PY_MAJOR_VERSION >= 3
+PyInit_pybullet(void)
+#else
+initpybullet(void)
+#endif
+{
+
+ PyObject* m;
+#if PY_MAJOR_VERSION >= 3
+ m = PyModule_Create(&moduledef);
+#else
+ m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet");
+#endif
+
+#if PY_MAJOR_VERSION >= 3
+ if (m == NULL) return m;
+#else
+ if (m == NULL) return;
+#endif
+
+ PyModule_AddIntConstant(m, "SHARED_MEMORY",
+ eCONNECT_SHARED_MEMORY); // user read
+ PyModule_AddIntConstant(m, "DIRECT", eCONNECT_DIRECT); // user read
+ PyModule_AddIntConstant(m, "GUI", eCONNECT_GUI); // user read
+
+ PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
+ PyModule_AddIntConstant(m, "VELOCITY_CONTROL",
+ CONTROL_MODE_VELOCITY); // user read
+ PyModule_AddIntConstant(m, "POSITION_CONTROL",
+ CONTROL_MODE_POSITION_VELOCITY_PD); // user read
+
+ PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME);
+ PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME);
+
+ SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
+ Py_INCREF(SpamError);
+ PyModule_AddObject(m, "error", SpamError);
+
+#ifdef PYBULLET_USE_NUMPY
+ // Initialize numpy array.
+ import_array();
+#endif //PYBULLET_USE_NUMPY
+
+
+#if PY_MAJOR_VERSION >= 3
+ return m;
+#endif
+}
diff --git a/examples/pybullet/robotcontrol.py b/examples/pybullet/robotcontrol.py
new file mode 100644
index 0000000..e94d6d5
--- /dev/null
+++ b/examples/pybullet/robotcontrol.py
@@ -0,0 +1,30 @@
+import pybullet as p
+p.connect(p.GUI) #or p.SHARED_MEMORY or p.DIRECT
+
+p.loadURDF("plane.urdf")
+p.setGravity(0,0,-10)
+huskypos = [0,0,0.1]
+
+husky = p.loadURDF("husky/husky.urdf",huskypos[0],huskypos[1],huskypos[2])
+numJoints = p.getNumJoints(husky)
+for joint in range (numJoints) :
+ print (p.getJointInfo(husky,joint))
+targetVel = 10 #rad/s
+maxForce = 100 #Newton
+
+for joint in range (2,6) :
+ p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce)
+for step in range (300):
+ p.stepSimulation()
+
+targetVel=-10
+for joint in range (2,6) :
+ p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce)
+for step in range (400):
+ p.stepSimulation()
+
+p.getContactPointData(husky)
+
+p.disconnect()
+
+
diff --git a/examples/pybullet/saveWorld.py b/examples/pybullet/saveWorld.py
new file mode 100644
index 0000000..f6f6278
--- /dev/null
+++ b/examples/pybullet/saveWorld.py
@@ -0,0 +1,8 @@
+import pybullet as p
+import time
+
+p.connect(p.SHARED_MEMORY)
+timestr = time.strftime("%Y%m%d-%H%M%S")
+filename = "saveWorld" + timestr + ".py"
+p.saveWorld(filename)
+p.disconnect()
diff --git a/examples/pybullet/test.py b/examples/pybullet/test.py
new file mode 100644
index 0000000..1a18498
--- /dev/null
+++ b/examples/pybullet/test.py
@@ -0,0 +1,36 @@
+import pybullet
+import time
+
+#choose connection method: GUI, DIRECT, SHARED_MEMORY
+pybullet.connect(pybullet.GUI)
+
+#load URDF, given a relative or absolute file+path
+obj = pybullet.loadURDF("r2d2.urdf")
+
+posX=0
+posY=3
+posZ=2
+obj2 = pybullet.loadURDF("kuka_lwr/kuka.urdf",posX,posY,posZ)
+
+#query the number of joints of the object
+numJoints = pybullet.getNumJoints(obj)
+
+print (numJoints)
+
+#set the gravity acceleration
+pybullet.setGravity(0,0,-9.8)
+
+#step the simulation for 5 seconds
+t_end = time.time() + 5
+while time.time() < t_end:
+ pybullet.stepSimulation()
+ posAndOrn = pybullet.getBasePositionAndOrientation(obj)
+ print (posAndOrn)
+
+print ("finished")
+#remove all objects
+pybullet.resetSimulation()
+
+#disconnect from the physics server
+pybullet.disconnect()
+
diff --git a/examples/pybullet/testrender.py b/examples/pybullet/testrender.py
new file mode 100644
index 0000000..da96bbb
--- /dev/null
+++ b/examples/pybullet/testrender.py
@@ -0,0 +1,46 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import pybullet
+
+pybullet.connect(pybullet.GUI)
+pybullet.loadURDF("r2d2.urdf")
+
+camTargetPos = [0,0,0]
+cameraUp = [0,0,1]
+cameraPos = [1,1,1]
+yaw = 40
+pitch = 10.0
+
+roll=0
+upAxisIndex = 2
+camDistance = 4
+pixelWidth = 320
+pixelHeight = 240
+nearPlane = 0.01
+farPlane = 1000
+
+fov = 60
+
+#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
+#renderImage(w, h, view[16], projection[16])
+#img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane)
+for pitch in range (0,360,10) :
+ img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex, nearPlane, farPlane, fov)
+
+ w=img_arr[0] #width of the image, in pixels
+ h=img_arr[1] #height of the image, in pixels
+ rgb=img_arr[2] #color data RGB
+ dep=img_arr[3] #depth data
+
+ #print 'width = %d height = %d' % (w,h)
+
+ # reshape creates np array
+ np_img_arr = np.reshape(rgb, (h, w, 4))
+ np_img_arr = np_img_arr*(1./255.)
+
+ #show
+ plt.imshow(np_img_arr,interpolation='none')
+ #plt.show()
+ plt.pause(0.01)
+
+pybullet.resetSimulation()
diff --git a/examples/pybullet/testrender_np.py b/examples/pybullet/testrender_np.py
new file mode 100644
index 0000000..56439dd
--- /dev/null
+++ b/examples/pybullet/testrender_np.py
@@ -0,0 +1,50 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import pybullet
+import time
+
+pybullet.connect(pybullet.DIRECT)
+pybullet.loadURDF("r2d2.urdf")
+
+camTargetPos = [0,0,0]
+cameraUp = [0,0,1]
+cameraPos = [1,1,1]
+yaw = 40
+pitch = 10.0
+
+roll=0
+upAxisIndex = 2
+camDistance = 4
+pixelWidth = 1920
+pixelHeight = 1080
+nearPlane = 0.01
+farPlane = 1000
+
+fov = 60
+
+main_start = time.time()
+#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
+#renderImage(w, h, view[16], projection[16])
+#img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane)
+for pitch in range (0,360,10) :
+ start = time.time()
+ img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex, nearPlane, farPlane, fov)
+ stop = time.time()
+ print "renderImage %f" % (stop - start)
+
+ w=img_arr[0] #width of the image, in pixels
+ h=img_arr[1] #height of the image, in pixels
+ rgb=img_arr[2] #color data RGB
+ dep=img_arr[3] #depth data
+
+ #print 'width = %d height = %d' % (w,h)
+
+ #show
+ plt.imshow(rgb,interpolation='none')
+ #plt.show()
+ plt.pause(0.01)
+
+main_stop = time.time()
+print "Total time %f" % (main_stop - main_start)
+
+pybullet.resetSimulation()
diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h
index 78d20a9..ce9941e 100644
--- a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h
+++ b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h
@@ -965,8 +965,6 @@ inline void b3DynamicBvh::rayTestInternal( const b3DbvtNode* root,
B3_DBVT_CHECKTYPE
if(root)
{
- b3Vector3 resultNormal;
-
int depth=1;
int treshold=B3_DOUBLE_STACKSIZE-2;
b3AlignedObjectArray<const b3DbvtNode*>& stack = m_rayTestStack;
diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h
index 74e6ef0..373c853 100644
--- a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h
+++ b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h
@@ -160,7 +160,7 @@ struct b3DynamicBvhBroadphase
#endif
/* Methods */
b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache=0);
- ~b3DynamicBvhBroadphase();
+ virtual ~b3DynamicBvhBroadphase();
void collide(b3Dispatcher* dispatcher);
void optimize();
diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h
index b36ac4b..083b0b5 100644
--- a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h
+++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h
@@ -21,6 +21,7 @@
#ifndef B3_MPR_PENETRATION_H
#define B3_MPR_PENETRATION_H
+#include "Bullet3Common/shared/b3PlatformDefinitions.h"
#include "Bullet3Common/shared/b3Float4.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
@@ -151,7 +152,7 @@ inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global con
}
-static void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
b3ConstArray(b3Collidable_t) cpuCollidables,
b3ConstArray(b3Float4) cpuVertices,
@@ -370,7 +371,7 @@ inline void b3ExpandPortal(b3MprSimplex_t *portal,
-static int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
b3ConstArray(b3Collidable_t) cpuCollidables,
b3ConstArray(b3Float4) cpuVertices,
@@ -505,7 +506,7 @@ static int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3Co
}
-static int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
b3ConstArray(b3Collidable_t) cpuCollidables,
b3ConstArray(b3Float4) cpuVertices,
@@ -547,7 +548,7 @@ static int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3Const
return -1;
}
-static void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
+B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
{
b3Float4 zero = b3MakeFloat4(0,0,0,0);
@@ -758,7 +759,7 @@ inline float b3MprVec3PointTriDist2(const b3Float4 *P,
}
-static void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
b3ConstArray(b3Collidable_t) cpuCollidables,
b3ConstArray(b3Float4) cpuVertices,
@@ -811,7 +812,7 @@ static void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstA
}
}
-static void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)
+B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)
{
// Touching contact on portal's v1 - so depth is zero and direction
// is unimportant and pos can be guessed
@@ -827,7 +828,7 @@ static void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir
b3MprVec3Scale(pos, 0.5);
}
-static void b3FindPenetrSegment(b3MprSimplex_t *portal,
+B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,
float *depth, b3Float4 *dir, b3Float4 *pos)
{
diff --git a/src/Bullet3Common/b3CommandLineArgs.h b/src/Bullet3Common/b3CommandLineArgs.h
index ffcee03..eeba545 100644
--- a/src/Bullet3Common/b3CommandLineArgs.h
+++ b/src/Bullet3Common/b3CommandLineArgs.h
@@ -29,20 +29,20 @@ public:
for (int i = 1; i < argc; i++)
{
- string arg = argv[i];
+ std::string arg = argv[i];
if ((arg[0] != '-') || (arg[1] != '-')) {
continue;
}
- string::size_type pos;
- string key, val;
- if ((pos = arg.find( '=')) == string::npos) {
- key = string(arg, 2, arg.length() - 2);
+ std::string::size_type pos;
+ std::string key, val;
+ if ((pos = arg.find( '=')) == std::string::npos) {
+ key = std::string(arg, 2, arg.length() - 2);
val = "";
} else {
- key = string(arg, 2, pos - 2);
- val = string(arg, pos + 1, arg.length() - 1);
+ key = std::string(arg, 2, pos - 2);
+ val = std::string(arg, pos + 1, arg.length() - 1);
}
//only add new keys, don't replace existing
@@ -56,7 +56,7 @@ public:
bool CheckCmdLineFlag(const char* arg_name)
{
using namespace std;
- map<string, string>::iterator itr;
+ map<std::string, std::string>::iterator itr;
if ((itr = pairs.find(arg_name)) != pairs.end()) {
return true;
}
@@ -76,7 +76,7 @@ template <typename T>
inline bool b3CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
{
using namespace std;
- map<string, string>::iterator itr;
+ map<std::string, std::string>::iterator itr;
if ((itr = pairs.find(arg_name)) != pairs.end()) {
istringstream strstream(itr->second);
strstream >> val;
@@ -89,10 +89,10 @@ template <>
inline bool b3CommandLineArgs::GetCmdLineArgument<char*>(const char* arg_name, char* &val)
{
using namespace std;
- map<string, string>::iterator itr;
+ map<std::string, std::string>::iterator itr;
if ((itr = pairs.find(arg_name)) != pairs.end()) {
- string s = itr->second;
+ std::string s = itr->second;
val = (char*) malloc(sizeof(char) * (s.length() + 1));
std::strcpy(val, s.c_str());
return true;
diff --git a/src/Bullet3Common/b3Scalar.h b/src/Bullet3Common/b3Scalar.h
index 60b7f1c..16a9ac7 100644
--- a/src/Bullet3Common/b3Scalar.h
+++ b/src/Bullet3Common/b3Scalar.h
@@ -55,7 +55,7 @@ inline int b3GetVersion()
//#define B3_HAS_ALIGNED_ALLOCATOR
#pragma warning(disable : 4324) // disable padding warning
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
-// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
+ #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
// #pragma warning(disable:4786) // Disable the "debug name too long" warning
#define B3_FORCE_INLINE __forceinline
diff --git a/src/Bullet3Common/shared/b3PlatformDefinitions.h b/src/Bullet3Common/shared/b3PlatformDefinitions.h
index a3287ae..1c133fb 100644
--- a/src/Bullet3Common/shared/b3PlatformDefinitions.h
+++ b/src/Bullet3Common/shared/b3PlatformDefinitions.h
@@ -20,6 +20,8 @@ inline int b3AtomicAdd (volatile int *p, int val)
}
#define __global
+
+#define B3_STATIC static
#else
//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX
#define B3_LARGE_FLOAT 1e18f
@@ -32,6 +34,8 @@ inline int b3AtomicAdd (volatile int *p, int val)
#define b3Sqrt native_sqrt
#define b3Sin native_sin
#define b3Cos native_cos
+
+#define B3_STATIC
#endif
#endif
diff --git a/src/Bullet3OpenCL/CMakeLists.txt b/src/Bullet3OpenCL/CMakeLists.txt
index e604a90..45df06f 100644
--- a/src/Bullet3OpenCL/CMakeLists.txt
+++ b/src/Bullet3OpenCL/CMakeLists.txt
@@ -45,7 +45,7 @@ ADD_LIBRARY(Bullet3OpenCL_clew ${Bullet3OpenCL_clew_SRCS} ${Bullet3OpenCL_clew_H
SET_TARGET_PROPERTIES(Bullet3OpenCL_clew PROPERTIES VERSION ${BULLET_VERSION})
SET_TARGET_PROPERTIES(Bullet3OpenCL_clew PROPERTIES SOVERSION ${BULLET_VERSION})
IF (BUILD_SHARED_LIBS)
- TARGET_LINK_LIBRARIES(Bullet3OpenCL_clew LinearMath Bullet3Dynamics)
+ TARGET_LINK_LIBRARIES(Bullet3OpenCL_clew LinearMath Bullet3Dynamics ${CMAKE_DL_LIBS})
ENDIF (BUILD_SHARED_LIBS)
diff --git a/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp b/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp
index 264c7b9..0287e0e 100644
--- a/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp
+++ b/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp
@@ -560,7 +560,7 @@ void b3OpenCLUtils_printDeviceInfo(cl_device_id device)
b3Printf("\t\t\t\t\t3D_MAX_WIDTH\t %u\n", info.m_image3dMaxWidth);
b3Printf("\t\t\t\t\t3D_MAX_HEIGHT\t %u\n", info.m_image3dMaxHeight);
b3Printf("\t\t\t\t\t3D_MAX_DEPTH\t %u\n", info.m_image3dMaxDepth);
- if (info.m_deviceExtensions != 0)
+ if (*info.m_deviceExtensions != 0)
{
b3Printf("\n CL_DEVICE_EXTENSIONS:%s\n",info.m_deviceExtensions);
}
@@ -636,10 +636,10 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
FILETIME modtimeBinary;
- CreateDirectory(sCachedBinaryPath,0);
+ CreateDirectoryA(sCachedBinaryPath,0);
{
- HANDLE binaryFileHandle = CreateFile(binaryFileName,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
+ HANDLE binaryFileHandle = CreateFileA(binaryFileName,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
if (binaryFileHandle ==INVALID_HANDLE_VALUE)
{
DWORD errorCode;
@@ -677,7 +677,7 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
if (binaryFileValid)
{
- HANDLE srcFileHandle = CreateFile(clFileNameForCaching,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
+ HANDLE srcFileHandle = CreateFileA(clFileNameForCaching,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
if (srcFileHandle==INVALID_HANDLE_VALUE)
{
@@ -686,7 +686,7 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
{
char relativeFileName[1024];
sprintf(relativeFileName,"%s%s",prefix[i],clFileNameForCaching);
- srcFileHandle = CreateFile(relativeFileName,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
+ srcFileHandle = CreateFileA(relativeFileName,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
}
}
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
index a0cc004..b7201d2 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
@@ -692,8 +692,6 @@ static bool findSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3Con
}
}
- b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
-
int curEdgeEdge = 0;
// Test edges
for(int e0=0;e0<hullA.m_numUniqueEdges;e0++)
@@ -1292,7 +1290,6 @@ int clipHullHullSingle(
B3_PROFILE("overlap");
float4 normalOnSurfaceB = (float4&)hostNormal;
- float4 centerOut;
b3Int4 contactIdx;
contactIdx.x = 0;
@@ -2776,9 +2773,7 @@ int computeContactConvexConvex2(
b3Collidable colB = collidables[collidableIndexB];
hullB = convexShapes[colB.m_shapeIndex];
//printf("numvertsB = %d\n",hullB.m_numVertices);
-
-
- float4 contactsOut[MAX_VERTS];
+
int contactCapacity = MAX_VERTS;
int numContactsOut=0;
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp
index 625abd2..d636f98 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp
@@ -34,7 +34,7 @@ namespace gjkepa2_impl2
/* GJK */
#define GJK_MAX_ITERATIONS 128
-#define GJK_ACCURARY ((b3Scalar)0.0001)
+#define GJK_ACCURACY ((b3Scalar)0.0001)
#define GJK_MIN_DISTANCE ((b3Scalar)0.0001)
#define GJK_DUPLICATED_EPS ((b3Scalar)0.0001)
#define GJK_SIMPLEX2_EPS ((b3Scalar)0.0)
@@ -216,7 +216,7 @@ namespace gjkepa2_impl2
/* Check for termination */
const b3Scalar omega=b3Dot(m_ray,w)/rl;
alpha=b3Max(omega,alpha);
- if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+ if(((rl-alpha)-(GJK_ACCURACY*rl))<=0)
{/* Return old simplex */
removevertice(m_simplices[m_current]);
break;
@@ -998,7 +998,7 @@ bool b3GjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
/* Symbols cleanup */
#undef GJK_MAX_ITERATIONS
-#undef GJK_ACCURARY
+#undef GJK_ACCURACY
#undef GJK_MIN_DISTANCE
#undef GJK_DUPLICATED_EPS
#undef GJK_SIMPLEX2_EPS
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp
index b6eb9c3..52027e1 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp
@@ -619,8 +619,8 @@ void b3QuantizedBvh::walkStacklessQuantizedTreeAgainstRay(b3NodeOverlapCallback*
/* Add box cast extents */
bounds[0] -= aabbMax;
bounds[1] -= aabbMin;
- b3Vector3 normal;
#if 0
+ b3Vector3 normal;
bool ra2 = b3RayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max);
bool ra = b3RayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal);
if (ra2 != ra)
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h
index c4c5c16..7ed4b38 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h
@@ -17,8 +17,6 @@ static const char* mprKernelsCL= \
" */\n"
"#ifndef B3_MPR_PENETRATION_H\n"
"#define B3_MPR_PENETRATION_H\n"
-"#ifndef B3_FLOAT4_H\n"
-"#define B3_FLOAT4_H\n"
"#ifndef B3_PLATFORM_DEFINITIONS_H\n"
"#define B3_PLATFORM_DEFINITIONS_H\n"
"struct MyTest\n"
@@ -38,6 +36,14 @@ static const char* mprKernelsCL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
+"#endif\n"
+"#endif\n"
+"#ifndef B3_FLOAT4_H\n"
+"#define B3_FLOAT4_H\n"
+"#ifndef B3_PLATFORM_DEFINITIONS_H\n"
+"#ifdef __cplusplus\n"
+"#else\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
@@ -509,7 +515,7 @@ static const char* mprKernelsCL= \
" }\n"
" return supVec;\n"
"}\n"
-"static void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n"
+"B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n"
" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n"
" b3ConstArray(b3Collidable_t) cpuCollidables,\n"
" b3ConstArray(b3Float4) cpuVertices,\n"
@@ -682,7 +688,7 @@ static const char* mprKernelsCL= \
" }\n"
" }\n"
"}\n"
-"static int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n"
+"B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n"
" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n"
" b3ConstArray(b3Collidable_t) cpuCollidables,\n"
" b3ConstArray(b3Float4) cpuVertices,\n"
@@ -792,7 +798,7 @@ static const char* mprKernelsCL= \
" }\n"
" return 0;\n"
"}\n"
-"static int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n"
+"B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n"
" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n"
" b3ConstArray(b3Collidable_t) cpuCollidables,\n"
" b3ConstArray(b3Float4) cpuVertices,\n"
@@ -826,7 +832,7 @@ static const char* mprKernelsCL= \
" }\n"
" return -1;\n"
"}\n"
-"static void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)\n"
+"B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)\n"
"{\n"
" b3Float4 zero = b3MakeFloat4(0,0,0,0);\n"
" b3Float4* b3mpr_vec3_origin = &zero;\n"
@@ -1000,7 +1006,7 @@ static const char* mprKernelsCL= \
" }\n"
" return dist;\n"
"}\n"
-"static void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n"
+"B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n"
" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n"
" b3ConstArray(b3Collidable_t) cpuCollidables,\n"
" b3ConstArray(b3Float4) cpuVertices,\n"
@@ -1043,7 +1049,7 @@ static const char* mprKernelsCL= \
" iterations++;\n"
" }\n"
"}\n"
-"static void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)\n"
+"B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)\n"
"{\n"
" // Touching contact on portal's v1 - so depth is zero and direction\n"
" // is unimportant and pos can be guessed\n"
@@ -1055,7 +1061,7 @@ static const char* mprKernelsCL= \
" b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);\n"
" b3MprVec3Scale(pos, 0.5);\n"
"}\n"
-"static void b3FindPenetrSegment(b3MprSimplex_t *portal,\n"
+"B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,\n"
" float *depth, b3Float4 *dir, b3Float4 *pos)\n"
"{\n"
" \n"
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h
index 8ac1774..b0103fe 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h
@@ -23,6 +23,7 @@ static const char* primitiveContactsKernelsCL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h
index 234942e..f0ecfc7 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h
@@ -50,6 +50,7 @@ static const char* satClipKernelsCL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcaveKernels.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcaveKernels.h
index 22b26af..611569c 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcaveKernels.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcaveKernels.h
@@ -164,6 +164,7 @@ static const char* satConcaveKernelsCL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h
index 74f4efa..6f8b0a9 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h
@@ -164,6 +164,7 @@ static const char* satKernelsCL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp
index 4d14bc4..ad477e2 100644
--- a/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp
+++ b/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp
@@ -1071,7 +1071,7 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b
int orgConstraintIndex = m_gpuData->m_cpuConstraintRows[rowIndex].m_originalConstraintIndex;
float breakingThreshold = m_gpuData->m_cpuConstraints[orgConstraintIndex].m_breakingImpulseThreshold;
// printf("rows[%d].m_appliedImpulse=%f\n",rowIndex,rows[rowIndex].m_appliedImpulse);
- if (b3Fabs((m_gpuData->m_cpuConstraintRows[rowIndex].m_appliedImpulse) >= breakingThreshold))
+ if (b3Fabs(m_gpuData->m_cpuConstraintRows[rowIndex].m_appliedImpulse) >= breakingThreshold)
{
m_gpuData->m_cpuConstraints[orgConstraintIndex].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED;
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h b/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h
index f52131b..150eedc 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h
@@ -35,6 +35,7 @@ static const char* batchingKernelsCL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h b/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h
index b7bf334..1e5957a 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h
@@ -35,6 +35,7 @@ static const char* batchingKernelsNewCL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h b/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h
index f96c337..a5a4329 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h
@@ -35,6 +35,7 @@ static const char* integrateKernelCL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl b/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl
index 0ad5161..7f5dabe 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl
+++ b/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl
@@ -410,8 +410,8 @@ __kernel void initBatchConstraintsKernel(__global unsigned int* numConstraintRow
int rbA = constraints[i].m_rbA;
int rbB = constraints[i].m_rbB;
- batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass? rbA : -rbA;
- batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass? rbB : -rbB;
+ batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass != 0.f ? rbA : -rbA;
+ batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass != 0.f ? rbB : -rbB;
batchConstraints[i].m_batchId = -1;
batchConstraints[i].m_originalConstraintIndex = i;
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h b/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h
index e011ac7..d48ecf6 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h
@@ -325,8 +325,8 @@ static const char* solveConstraintRowsCL= \
" return;\n"
" int rbA = constraints[i].m_rbA;\n"
" int rbB = constraints[i].m_rbB;\n"
-" batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass? rbA : -rbA;\n"
-" batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass? rbB : -rbB;\n"
+" batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass != 0.f ? rbA : -rbA;\n"
+" batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass != 0.f ? rbB : -rbB;\n"
" batchConstraints[i].m_batchId = -1;\n"
" batchConstraints[i].m_originalConstraintIndex = i;\n"
"}\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h b/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h
index e833e2e..eb1834e 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h
@@ -35,6 +35,7 @@ static const char* solverSetupCL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl b/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl
index 518f708..3dc48d4 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl
+++ b/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl
@@ -510,7 +510,7 @@ typedef struct
} ConstBufferSSD;
-static __constant const int gridTable4x4[] =
+__constant const int gridTable4x4[] =
{
0,1,17,16,
1,2,18,19,
@@ -518,7 +518,7 @@ static __constant const int gridTable4x4[] =
16,19,3,34
};
-static __constant const int gridTable8x8[] =
+__constant const int gridTable8x8[] =
{
0, 2, 3, 16, 17, 18, 19, 1,
66, 64, 80, 67, 82, 81, 65, 83,
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h b/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h
index a64be4b..1b5819f 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h
@@ -35,6 +35,7 @@ static const char* solverSetup2CL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
@@ -514,14 +515,14 @@ static const char* solverSetup2CL= \
" float m_scale;\n"
" int m_nSplit;\n"
"} ConstBufferSSD;\n"
-"static __constant const int gridTable4x4[] = \n"
+"__constant const int gridTable4x4[] = \n"
"{\n"
" 0,1,17,16,\n"
" 1,2,18,19,\n"
" 17,18,32,3,\n"
" 16,19,3,34\n"
"};\n"
-"static __constant const int gridTable8x8[] = \n"
+"__constant const int gridTable8x8[] = \n"
"{\n"
" 0, 2, 3, 16, 17, 18, 19, 1,\n"
" 66, 64, 80, 67, 82, 81, 65, 83,\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl b/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl
index 9c4ac4c..a21a08c 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl
+++ b/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl
@@ -954,8 +954,8 @@ float positionConstraintCoeff
Constraint4 cs;
- float countA = invMassA ? (float)bodyCount[aIdx] : 1;
- float countB = invMassB ? (float)bodyCount[bIdx] : 1;
+ float countA = invMassA != 0.f ? (float)bodyCount[aIdx] : 1;
+ float countB = invMassB != 0.f ? (float)bodyCount[bIdx] : 1;
setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,
&gContact[gIdx], dt, positionDrift, positionConstraintCoeff,countA,countB,
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h b/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h
index 13f7b7a..c0173ad 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h
@@ -35,6 +35,7 @@ static const char* solverUtilsCL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
@@ -895,8 +896,8 @@ static const char* solverUtilsCL= \
" float invMassB = gBodies[bIdx].m_invMass;\n"
" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n"
" Constraint4 cs;\n"
-" float countA = invMassA ? (float)bodyCount[aIdx] : 1;\n"
-" float countB = invMassB ? (float)bodyCount[bIdx] : 1;\n"
+" float countA = invMassA != 0.f ? (float)bodyCount[aIdx] : 1;\n"
+" float countB = invMassB != 0.f ? (float)bodyCount[bIdx] : 1;\n"
" setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,\n"
" &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,countA,countB,\n"
" &cs );\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h b/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h
index 0791b6d..d70e740 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h
@@ -25,6 +25,7 @@ static const char* updateAabbsKernelCL= \
"#define b3Sqrt native_sqrt\n"
"#define b3Sin native_sin\n"
"#define b3Cos native_cos\n"
+"#define B3_STATIC\n"
"#endif\n"
"#endif\n"
"#ifdef __cplusplus\n"
@@ -362,17 +363,33 @@ static const char* updateAabbsKernelCL= \
" int m_compoundBvhIndex;\n"
" };\n"
" int m_shapeType;\n"
-" int m_shapeIndex;\n"
+" union\n"
+" {\n"
+" int m_shapeIndex;\n"
+" float m_height;\n"
+" };\n"
"};\n"
"typedef struct b3GpuChildShape b3GpuChildShape_t;\n"
"struct b3GpuChildShape\n"
"{\n"
" b3Float4 m_childPosition;\n"
" b3Quat m_childOrientation;\n"
-" int m_shapeIndex;\n"
-" int m_unused0;\n"
-" int m_unused1;\n"
-" int m_unused2;\n"
+" union\n"
+" {\n"
+" int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS\n"
+" int m_capsuleAxis;\n"
+" };\n"
+" union \n"
+" {\n"
+" float m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES\n"
+" int m_numChildShapes;//used for compound shape\n"
+" };\n"
+" union \n"
+" {\n"
+" float m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES\n"
+" int m_collidableShapeIndex;\n"
+" };\n"
+" int m_shapeType;\n"
"};\n"
"struct b3CompoundOverlappingPair\n"
"{\n"
diff --git a/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp b/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp
index 6f47f3b..c3ceb83 100644
--- a/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp
+++ b/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp
@@ -215,7 +215,7 @@ void b3BulletFile::parseData()
// }
} else
{
- printf("unknown chunk\n");
+ //printf("unknown chunk\n");
mLibPointers.insert(dataChunk.oldPtr, (bStructHandle*)dataPtrHead);
}
@@ -420,4 +420,4 @@ void b3BulletFile::addStruct(const char* structType,void* data, int len, void* o
mLibPointers.insert(dataChunk.oldPtr, (bStructHandle*)data);
m_chunks.push_back(dataChunk);
-}
\ No newline at end of file
+}
diff --git a/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp b/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp
index 6b332f5..b78f763 100644
--- a/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp
+++ b/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp
@@ -389,16 +389,8 @@ void bDNA::init(char *data, int len, bool swap)
}
- {
- nr= (long)cp;
- //long mask=3;
- nr= ((nr+3)&~3)-nr;
- while (nr--)
- {
- cp++;
- }
- }
-
+ cp = b3AlignPointer(cp,4);
+
/*
TYPE (4 bytes)
@@ -425,16 +417,8 @@ void bDNA::init(char *data, int len, bool swap)
cp++;
}
-{
- nr= (long)cp;
- // long mask=3;
- nr= ((nr+3)&~3)-nr;
- while (nr--)
- {
- cp++;
- }
- }
+ cp = b3AlignPointer(cp,4);
/*
TLEN (4 bytes)
diff --git a/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp b/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp
index e684fcc..71d3b9e 100644
--- a/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp
+++ b/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp
@@ -428,16 +428,7 @@ void bFile::swapDNA(char* ptr)
}
- {
- nr= (long)cp;
- //long mask=3;
- nr= ((nr+3)&~3)-nr;
- while (nr--)
- {
- cp++;
- }
- }
-
+ cp = b3AlignPointer(cp,4);
/*
TYPE (4 bytes)
@@ -465,16 +456,7 @@ void bFile::swapDNA(char* ptr)
cp++;
}
-{
- nr= (long)cp;
- // long mask=3;
- nr= ((nr+3)&~3)-nr;
- while (nr--)
- {
- cp++;
- }
- }
-
+ cp = b3AlignPointer(cp,4);
/*
TLEN (4 bytes)
@@ -612,7 +594,7 @@ void bFile::preSwap()
swap(dataPtrHead, dataChunk,ignoreEndianFlag);
} else
{
- printf("unknown chunk\n");
+ //printf("unknown chunk\n");
}
}
diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.h b/src/BulletCollision/BroadphaseCollision/btDbvt.h
index db4e482..bee17e5 100644
--- a/src/BulletCollision/BroadphaseCollision/btDbvt.h
+++ b/src/BulletCollision/BroadphaseCollision/btDbvt.h
@@ -122,6 +122,7 @@ subject to the following restrictions:
#error "DBVT_INT0_IMPL undefined"
#endif
+
//
// Defaults volumes
//
@@ -188,6 +189,9 @@ struct btDbvtNode
};
};
+typedef btAlignedObjectArray<const btDbvtNode*> btNodeStack;
+
+
///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
@@ -325,6 +329,16 @@ struct btDbvt
void collideTV( const btDbvtNode* root,
const btDbvtVolume& volume,
DBVT_IPOLICY) const;
+
+ DBVT_PREFIX
+ void collideTVNoStackAlloc( const btDbvtNode* root,
+ const btDbvtVolume& volume,
+ btNodeStack& stack,
+ DBVT_IPOLICY) const;
+
+
+
+
///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
DBVT_PREFIX
@@ -917,39 +931,72 @@ inline void btDbvt::collideTT( const btDbvtNode* root0,
}
#endif
-//
DBVT_PREFIX
inline void btDbvt::collideTV( const btDbvtNode* root,
const btDbvtVolume& vol,
DBVT_IPOLICY) const
{
DBVT_CHECKTYPE
- if(root)
- {
- ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol);
- btAlignedObjectArray<const btDbvtNode*> stack;
- stack.resize(0);
- stack.reserve(SIMPLE_STACKSIZE);
- stack.push_back(root);
- do {
- const btDbvtNode* n=stack[stack.size()-1];
- stack.pop_back();
- if(Intersect(n->volume,volume))
+ if(root)
+ {
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol);
+ btAlignedObjectArray<const btDbvtNode*> stack;
+ stack.resize(0);
+ stack.reserve(SIMPLE_STACKSIZE);
+ stack.push_back(root);
+ do {
+ const btDbvtNode* n=stack[stack.size()-1];
+ stack.pop_back();
+ if(Intersect(n->volume,volume))
+ {
+ if(n->isinternal())
{
- if(n->isinternal())
- {
- stack.push_back(n->childs[0]);
- stack.push_back(n->childs[1]);
- }
- else
- {
- policy.Process(n);
- }
+ stack.push_back(n->childs[0]);
+ stack.push_back(n->childs[1]);
}
- } while(stack.size()>0);
- }
+ else
+ {
+ policy.Process(n);
+ }
+ }
+ } while(stack.size()>0);
+ }
}
+//
+DBVT_PREFIX
+inline void btDbvt::collideTVNoStackAlloc( const btDbvtNode* root,
+ const btDbvtVolume& vol,
+ btNodeStack& stack,
+ DBVT_IPOLICY) const
+{
+ DBVT_CHECKTYPE
+ if(root)
+ {
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol);
+ stack.resize(0);
+ stack.reserve(SIMPLE_STACKSIZE);
+ stack.push_back(root);
+ do {
+ const btDbvtNode* n=stack[stack.size()-1];
+ stack.pop_back();
+ if(Intersect(n->volume,volume))
+ {
+ if(n->isinternal())
+ {
+ stack.push_back(n->childs[0]);
+ stack.push_back(n->childs[1]);
+ }
+ else
+ {
+ policy.Process(n);
+ }
+ }
+ } while(stack.size()>0);
+ }
+}
+
+
DBVT_PREFIX
inline void btDbvt::rayTestInternal( const btDbvtNode* root,
const btVector3& rayFrom,
diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
index ad69fcb..55ebf06 100644
--- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
@@ -372,10 +372,10 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
return userData;
}
//#include <stdio.h>
-
+#include "LinearMath/btQuickprof.h"
void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
{
-
+ BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
int i;
// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
diff --git a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
index 889216d..93de499 100644
--- a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
@@ -107,6 +107,8 @@ void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btV
v = unQuantize(vecIn);
m_bvhAabbMin.setMin(v-clampValue);
}
+ aabbSize = m_bvhAabbMax - m_bvhAabbMin;
+ m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize;
{
quantize(vecIn,m_bvhAabbMax,true);
v = unQuantize(vecIn);
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
index 3b6913c..d41e98c 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
@@ -16,7 +16,7 @@ subject to the following restrictions:
#include "btCollisionDispatcher.h"
-
+#include "LinearMath/btQuickprof.h"
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
@@ -227,6 +227,8 @@ public:
virtual bool processOverlap(btBroadphasePair& pair)
{
+ BT_PROFILE("btCollisionDispatcher::processOverlap");
+
(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
return false;
@@ -249,7 +251,6 @@ void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pa
-
//by default, Bullet will use this near callback
void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
{
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
index 395df3a..25cefb1 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
@@ -33,8 +33,12 @@ btCollisionObject::btCollisionObject()
m_friction(btScalar(0.5)),
m_restitution(btScalar(0.)),
m_rollingFriction(0.0f),
+ m_spinningFriction(0.f),
+ m_contactDamping(.1),
+ m_contactStiffness(1e4),
m_internalType(CO_COLLISION_OBJECT),
m_userObjectPointer(0),
+ m_userIndex2(-1),
m_userIndex(-1),
m_hitFraction(btScalar(1.)),
m_ccdSweptSphereRadius(btScalar(0.)),
@@ -91,6 +95,8 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
dataOut->m_deactivationTime = m_deactivationTime;
dataOut->m_friction = m_friction;
dataOut->m_rollingFriction = m_rollingFriction;
+ dataOut->m_contactDamping = m_contactDamping;
+ dataOut->m_contactStiffness = m_contactStiffness;
dataOut->m_restitution = m_restitution;
dataOut->m_internalType = m_internalType;
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
index c684024..d042115 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
@@ -85,7 +85,12 @@ protected:
btScalar m_friction;
btScalar m_restitution;
- btScalar m_rollingFriction;
+ btScalar m_rollingFriction;//torsional friction orthogonal to contact normal (useful to stop spheres rolling forever)
+ btScalar m_spinningFriction; // torsional friction around the contact normal (useful for grasping)
+ btScalar m_contactDamping;
+ btScalar m_contactStiffness;
+
+
///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
///do not assign your own m_internalType unless you write a new dynamics object class.
@@ -93,8 +98,10 @@ protected:
///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
- void* m_userObjectPointer;
-
+ void* m_userObjectPointer;
+
+ int m_userIndex2;
+
int m_userIndex;
///time of impact calculation
@@ -127,7 +134,8 @@ public:
CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
CF_CHARACTER_OBJECT = 16,
CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
- CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing
+ CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
+ CF_HAS_CONTACT_STIFFNESS_DAMPING = 128
};
enum CollisionObjectTypes
@@ -316,8 +324,40 @@ public:
{
return m_rollingFriction;
}
-
-
+ void setSpinningFriction(btScalar frict)
+ {
+ m_updateRevision++;
+ m_spinningFriction = frict;
+ }
+ btScalar getSpinningFriction() const
+ {
+ return m_spinningFriction;
+ }
+ void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping)
+ {
+ m_updateRevision++;
+ m_contactStiffness = stiffness;
+ m_contactDamping = damping;
+
+ m_collisionFlags |=CF_HAS_CONTACT_STIFFNESS_DAMPING;
+
+ //avoid divisions by zero...
+ if (m_contactStiffness< SIMD_EPSILON)
+ {
+ m_contactStiffness = SIMD_EPSILON;
+ }
+ }
+
+ btScalar getContactStiffness() const
+ {
+ return m_contactStiffness;
+ }
+
+ btScalar getContactDamping() const
+ {
+ return m_contactDamping;
+ }
+
///reserved for Bullet internal usage
int getInternalType() const
{
@@ -476,6 +516,12 @@ public:
{
return m_userIndex;
}
+
+ int getUserIndex2() const
+ {
+ return m_userIndex2;
+ }
+
///users can point to their objects, userPointer is not used by Bullet
void setUserPointer(void* userPointer)
{
@@ -487,6 +533,11 @@ public:
{
m_userIndex = index;
}
+
+ void setUserIndex2(int index)
+ {
+ m_userIndex2 = index;
+ }
int getUpdateRevisionInternal() const
{
@@ -528,6 +579,8 @@ struct btCollisionObjectDoubleData
double m_deactivationTime;
double m_friction;
double m_rollingFriction;
+ double m_contactDamping;
+ double m_contactStiffness;
double m_restitution;
double m_hitFraction;
double m_ccdSweptSphereRadius;
@@ -561,7 +614,8 @@ struct btCollisionObjectFloatData
float m_deactivationTime;
float m_friction;
float m_rollingFriction;
-
+ float m_contactDamping;
+ float m_contactStiffness;
float m_restitution;
float m_hitFraction;
float m_ccdSweptSphereRadius;
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index c505ed5..fa4cac6 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -790,23 +790,50 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
}
}
} else {
- ///@todo : use AABB tree or other BVH acceleration structure!
if (collisionShape->isCompound())
{
- BT_PROFILE("convexSweepCompound");
- const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
- int i=0;
- for (i=0;i<compoundShape->getNumChildShapes();i++)
+ struct btCompoundLeafCallback : btDbvt::ICollide
{
- btTransform childTrans = compoundShape->getChildTransform(i);
- const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
- btTransform childWorldTrans = colObjWorldTransform * childTrans;
-
- struct LocalInfoAdder : public ConvexResultCallback {
- ConvexResultCallback* m_userCallback;
+ btCompoundLeafCallback(
+ const btCollisionObjectWrapper* colObjWrap,
+ const btConvexShape* castShape,
+ const btTransform& convexFromTrans,
+ const btTransform& convexToTrans,
+ btScalar allowedPenetration,
+ const btCompoundShape* compoundShape,
+ const btTransform& colObjWorldTransform,
+ ConvexResultCallback& resultCallback)
+ :
+ m_colObjWrap(colObjWrap),
+ m_castShape(castShape),
+ m_convexFromTrans(convexFromTrans),
+ m_convexToTrans(convexToTrans),
+ m_allowedPenetration(allowedPenetration),
+ m_compoundShape(compoundShape),
+ m_colObjWorldTransform(colObjWorldTransform),
+ m_resultCallback(resultCallback) {
+ }
+
+ const btCollisionObjectWrapper* m_colObjWrap;
+ const btConvexShape* m_castShape;
+ const btTransform& m_convexFromTrans;
+ const btTransform& m_convexToTrans;
+ btScalar m_allowedPenetration;
+ const btCompoundShape* m_compoundShape;
+ const btTransform& m_colObjWorldTransform;
+ ConvexResultCallback& m_resultCallback;
+
+ public:
+
+ void ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape)
+ {
+ btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
+
+ struct LocalInfoAdder : public ConvexResultCallback {
+ ConvexResultCallback* m_userCallback;
int m_i;
- LocalInfoAdder (int i, ConvexResultCallback *user)
+ LocalInfoAdder(int i, ConvexResultCallback *user)
: m_userCallback(user), m_i(i)
{
m_closestHitFraction = m_userCallback->m_closestHitFraction;
@@ -815,27 +842,66 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
{
return m_userCallback->needsCollision(p);
}
- virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b)
- {
- btCollisionWorld::LocalShapeInfo shapeInfo;
- shapeInfo.m_shapePart = -1;
- shapeInfo.m_triangleIndex = m_i;
- if (r.m_localShapeInfo == NULL)
- r.m_localShapeInfo = &shapeInfo;
- const btScalar result = m_userCallback->addSingleResult(r, b);
- m_closestHitFraction = m_userCallback->m_closestHitFraction;
- return result;
-
- }
- };
+ virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& r, bool b)
+ {
+ btCollisionWorld::LocalShapeInfo shapeInfo;
+ shapeInfo.m_shapePart = -1;
+ shapeInfo.m_triangleIndex = m_i;
+ if (r.m_localShapeInfo == NULL)
+ r.m_localShapeInfo = &shapeInfo;
+ const btScalar result = m_userCallback->addSingleResult(r, b);
+ m_closestHitFraction = m_userCallback->m_closestHitFraction;
+ return result;
- LocalInfoAdder my_cb(i, &resultCallback);
-
- btCollisionObjectWrapper tmpObj(colObjWrap,childCollisionShape,colObjWrap->getCollisionObject(),childWorldTrans,-1,i);
+ }
+ };
- objectQuerySingleInternal(castShape, convexFromTrans,convexToTrans,
- &tmpObj,my_cb, allowedPenetration);
-
+ LocalInfoAdder my_cb(index, &m_resultCallback);
+
+ btCollisionObjectWrapper tmpObj(m_colObjWrap, childCollisionShape, m_colObjWrap->getCollisionObject(), childWorldTrans, -1, index);
+
+ objectQuerySingleInternal(m_castShape, m_convexFromTrans, m_convexToTrans, &tmpObj, my_cb, m_allowedPenetration);
+ }
+
+ void Process(const btDbvtNode* leaf)
+ {
+ // Processing leaf node
+ int index = leaf->dataAsInt;
+
+ btTransform childTrans = m_compoundShape->getChildTransform(index);
+ const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(index);
+
+ ProcessChild(index, childTrans, childCollisionShape);
+ }
+ };
+
+ BT_PROFILE("convexSweepCompound");
+ const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+
+ btVector3 fromLocalAabbMin, fromLocalAabbMax;
+ btVector3 toLocalAabbMin, toLocalAabbMax;
+
+ castShape->getAabb(colObjWorldTransform.inverse() * convexFromTrans, fromLocalAabbMin, fromLocalAabbMax);
+ castShape->getAabb(colObjWorldTransform.inverse() * convexToTrans, toLocalAabbMin, toLocalAabbMax);
+
+ fromLocalAabbMin.setMin(toLocalAabbMin);
+ fromLocalAabbMax.setMax(toLocalAabbMax);
+
+ btCompoundLeafCallback callback(colObjWrap, castShape, convexFromTrans, convexToTrans,
+ allowedPenetration, compoundShape, colObjWorldTransform, resultCallback);
+
+ const btDbvt* tree = compoundShape->getDynamicAabbTree();
+ if (tree) {
+ const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax);
+ tree->collideTV(tree->m_root, bounds, callback);
+ } else {
+ int i;
+ for (i=0;i<compoundShape->getNumChildShapes();i++)
+ {
+ const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
+ btTransform childTrans = compoundShape->getChildTransform(i);
+ callback.ProcessChild(i, childTrans, childCollisionShape);
+ }
}
}
}
diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
index a80c438..13cddc1 100644
--- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
@@ -244,7 +244,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{
int i;
- btManifoldArray manifoldArray;
+ manifoldArray.resize(0);
for (i=0;i<m_childCollisionAlgorithms.size();i++)
{
if (m_childCollisionAlgorithms[i])
@@ -274,7 +274,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
//process all children, that overlap with the given AABB bounds
- tree->collideTV(tree->m_root,bounds,callback);
+ tree->collideTVNoStackAlloc(tree->m_root,bounds,stack2,callback);
} else
{
@@ -291,7 +291,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.size();
int i;
- btManifoldArray manifoldArray;
+ manifoldArray.resize(0);
const btCollisionShape* childShape = 0;
btTransform orgTrans;
diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
index 7d792c1..d2086fb 100644
--- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
@@ -26,6 +26,7 @@ class btDispatcher;
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "btCollisionCreateFunc.h"
#include "LinearMath/btAlignedObjectArray.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
class btDispatcher;
class btCollisionObject;
@@ -36,6 +37,9 @@ extern btShapePairCallback gCompoundChildShapePairCallback;
/// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes
class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
+ btNodeStack stack2;
+ btManifoldArray manifoldArray;
+
protected:
btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
bool m_isSwapped;
diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
index 1d64d84..ab2632e 100644
--- a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
@@ -15,6 +15,7 @@ subject to the following restrictions:
*/
#include "btCompoundCompoundCollisionAlgorithm.h"
+#include "LinearMath/btQuickprof.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
@@ -124,6 +125,7 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
{
+ BT_PROFILE("btCompoundCompoundLeafCallback::Process");
m_numOverlapPairs++;
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
index 912a528..322b128 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
@@ -15,6 +15,7 @@ subject to the following restrictions:
#include "btConvexConcaveCollisionAlgorithm.h"
+#include "LinearMath/btQuickprof.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
@@ -79,6 +80,7 @@ void btConvexTriangleCallback::clearCache()
void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
partId, int triangleIndex)
{
+ BT_PROFILE("btConvexTriangleCallback::processTriangle");
if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
{
@@ -184,7 +186,7 @@ void btConvexConcaveCollisionAlgorithm::clearCache()
void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
-
+ BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
@@ -265,6 +267,7 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
{
+ BT_PROFILE("processTriangle");
(void)partId;
(void)triangleIndex;
//do a swept sphere for now
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
index e90d06e..93d842e 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
@@ -26,14 +26,16 @@ class btDispatcher;
#include "btCollisionCreateFunc.h"
///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called.
-class btConvexTriangleCallback : public btTriangleCallback
+ATTRIBUTE_ALIGNED16(class) btConvexTriangleCallback : public btTriangleCallback
{
- const btCollisionObjectWrapper* m_convexBodyWrap;
- const btCollisionObjectWrapper* m_triBodyWrap;
btVector3 m_aabbMin;
btVector3 m_aabbMax ;
+ const btCollisionObjectWrapper* m_convexBodyWrap;
+ const btCollisionObjectWrapper* m_triBodyWrap;
+
+
btManifoldResult* m_resultOut;
btDispatcher* m_dispatcher;
@@ -41,6 +43,8 @@ class btConvexTriangleCallback : public btTriangleCallback
btScalar m_collisionMarginTriangle;
public:
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
int m_triangleCount;
btPersistentManifold* m_manifoldPtr;
@@ -75,17 +79,19 @@ int m_triangleCount;
/// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes.
-class btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm
+ATTRIBUTE_ALIGNED16(class) btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
- bool m_isSwapped;
-
btConvexTriangleCallback m_btConvexTriangleCallback;
+ bool m_isSwapped;
+
public:
+ BT_DECLARE_ALIGNED_ALLOCATOR();
+
btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
virtual ~btConvexConcaveCollisionAlgorithm();
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
index 7f2722a..b9cc041 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
@@ -503,9 +503,11 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
+ worldVertsB1.resize(0);
btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(),
- body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut);
+ body1Wrap->getWorldTransform(), minDist-threshold, threshold, worldVertsB1,worldVertsB2,
+ *resultOut);
}
if (m_ownManifold)
@@ -568,8 +570,9 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
if (foundSepAxis)
{
+ worldVertsB2.resize(0);
btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
- body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
+ body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
}
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
index 51db0c6..d0ff3b3 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
+++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
@@ -24,6 +24,7 @@ subject to the following restrictions:
#include "btCollisionCreateFunc.h"
#include "btCollisionDispatcher.h"
#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
+#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
class btConvexPenetrationDepthSolver;
@@ -45,6 +46,8 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
btSimplexSolverInterface* m_simplexSolver;
btConvexPenetrationDepthSolver* m_pdSolver;
+ btVertexArray worldVertsB1;
+ btVertexArray worldVertsB2;
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
index d42f00a..9a2e339 100644
--- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
+++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
@@ -123,6 +123,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize);
}
+ collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize+16)&0xffffffffffff0;
if (constructionInfo.m_collisionAlgorithmPool)
{
m_ownsCollisionAlgorithmPool = false;
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
index 4b2986a..96c1eb5 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
@@ -24,10 +24,9 @@ ContactAddedCallback gContactAddedCallback=0;
-///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
-inline btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
{
- btScalar friction = body0->getRollingFriction() * body1->getRollingFriction();
+ btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction();
const btScalar MAX_FRICTION = btScalar(10.);
if (friction < -MAX_FRICTION)
@@ -38,6 +37,17 @@ inline btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,
}
+btScalar btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+ btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction();
+
+ const btScalar MAX_FRICTION = btScalar(10.);
+ if (friction < -MAX_FRICTION)
+ friction = -MAX_FRICTION;
+ if (friction > MAX_FRICTION)
+ friction = MAX_FRICTION;
+ return friction;
+}
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
@@ -58,6 +68,22 @@ btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject*
return body0->getRestitution() * body1->getRestitution();
}
+btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+ return body0->getContactDamping() + body1->getContactDamping();
+}
+
+btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+
+ btScalar s0 = body0->getContactStiffness();
+ btScalar s1 = body1->getContactStiffness();
+
+ btScalar tmp0 = btScalar(1)/s0;
+ btScalar tmp1 = btScalar(1)/s1;
+ btScalar combinedStiffness = btScalar(1) / (tmp0+tmp1);
+ return combinedStiffness;
+}
btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
@@ -109,6 +135,16 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+ newPt.m_combinedSpinningFriction = calculateCombinedSpinningFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+
+ if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
+ (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
+ {
+ newPt.m_combinedContactDamping1 = calculateCombinedContactDamping(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+ newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+ newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
+ }
+
btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
index 977b9a0..55b6380 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.h
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
@@ -145,6 +145,10 @@ public:
/// in the future we can let the user override the methods to combine restitution and friction
static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+ static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+ static btScalar calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+ static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1);
+ static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1);
};
#endif //BT_MANIFOLD_RESULT_H
diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
index ace4cfa..0940da1 100644
--- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
@@ -245,16 +245,18 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
btStridingMeshInterface* m_meshInterface;
btTriangleCallback* m_callback;
btVector3 m_triangle[3];
-
+ int m_numOverlap;
MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
:m_meshInterface(meshInterface),
- m_callback(callback)
+ m_callback(callback),
+ m_numOverlap(0)
{
}
virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
{
+ m_numOverlap++;
const unsigned char *vertexbase;
int numverts;
PHY_ScalarType type;
@@ -321,8 +323,7 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface);
m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
-
-
+
#endif//DISABLE_BVH
diff --git a/src/BulletCollision/CollisionShapes/btConeShape.h b/src/BulletCollision/CollisionShapes/btConeShape.h
index 4a0df0d..46d78d1 100644
--- a/src/BulletCollision/CollisionShapes/btConeShape.h
+++ b/src/BulletCollision/CollisionShapes/btConeShape.h
@@ -43,6 +43,15 @@ public:
btScalar getRadius() const { return m_radius;}
btScalar getHeight() const { return m_height;}
+ void setRadius(const btScalar radius)
+ {
+ m_radius = radius;
+ }
+ void setHeight(const btScalar height)
+ {
+ m_height = height;
+ }
+
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const
{
diff --git a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
index 0623e35..c1aa6ca 100644
--- a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
@@ -22,6 +22,8 @@ subject to the following restrictions:
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btSerializer.h"
+#include "btConvexPolyhedron.h"
+#include "LinearMath/btConvexHullComputer.h"
btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape ()
{
@@ -121,10 +123,17 @@ btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const
}
-
-
-
-
+void btConvexHullShape::optimizeConvexHull()
+{
+ btConvexHullComputer conv;
+ conv.compute(&m_unscaledPoints[0].getX(), sizeof(btVector3),m_unscaledPoints.size(),0.f,0.f);
+ int numVerts = conv.vertices.size();
+ m_unscaledPoints.resize(0);
+ for (int i=0;i<numVerts;i++)
+ {
+ m_unscaledPoints.push_back(conv.vertices[i]);
+ }
+}
diff --git a/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/src/BulletCollision/CollisionShapes/btConvexHullShape.h
index 3bd598e..0c12aee 100644
--- a/src/BulletCollision/CollisionShapes/btConvexHullShape.h
+++ b/src/BulletCollision/CollisionShapes/btConvexHullShape.h
@@ -55,9 +55,8 @@ public:
return getUnscaledPoints();
}
-
-
-
+ void optimizeConvexHull();
+
SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const
{
return m_unscaledPoints[i] * m_localScaling;
diff --git a/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
index a7362ea..88f6c4d 100644
--- a/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
@@ -75,7 +75,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala
int inner_count = MIN( numSpheres - k, 128 );
for( long i = 0; i < inner_count; i++ )
{
- temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
+ temp[i] = (*pos)*m_localScaling +vec*m_localScaling*(*rad) - vec * getMargin();
pos++;
rad++;
}
@@ -113,7 +113,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala
int inner_count = MIN( numSpheres - k, 128 );
for( long i = 0; i < inner_count; i++ )
{
- temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
+ temp[i] = (*pos)*m_localScaling +vec*m_localScaling*(*rad) - vec * getMargin();
pos++;
rad++;
}
diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
index 3268f06..eefb974 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
+++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
@@ -41,21 +41,38 @@ namespace gjkepa2_impl
/* GJK */
#define GJK_MAX_ITERATIONS 128
-#define GJK_ACCURARY ((btScalar)0.0001)
-#define GJK_MIN_DISTANCE ((btScalar)0.0001)
-#define GJK_DUPLICATED_EPS ((btScalar)0.0001)
+
+#ifdef BT_USE_DOUBLE_PRECISION
+ #define GJK_ACCURACY ((btScalar)1e-12)
+ #define GJK_MIN_DISTANCE ((btScalar)1e-12)
+ #define GJK_DUPLICATED_EPS ((btScalar)1e-12)
+#else
+ #define GJK_ACCURACY ((btScalar)0.0001)
+ #define GJK_MIN_DISTANCE ((btScalar)0.0001)
+ #define GJK_DUPLICATED_EPS ((btScalar)0.0001)
+#endif //BT_USE_DOUBLE_PRECISION
+
+
#define GJK_SIMPLEX2_EPS ((btScalar)0.0)
#define GJK_SIMPLEX3_EPS ((btScalar)0.0)
#define GJK_SIMPLEX4_EPS ((btScalar)0.0)
/* EPA */
-#define EPA_MAX_VERTICES 64
-#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
+#define EPA_MAX_VERTICES 128
#define EPA_MAX_ITERATIONS 255
-#define EPA_ACCURACY ((btScalar)0.0001)
-#define EPA_FALLBACK (10*EPA_ACCURACY)
-#define EPA_PLANE_EPS ((btScalar)0.00001)
-#define EPA_INSIDE_EPS ((btScalar)0.01)
+
+#ifdef BT_USE_DOUBLE_PRECISION
+ #define EPA_ACCURACY ((btScalar)1e-12)
+ #define EPA_PLANE_EPS ((btScalar)1e-14)
+ #define EPA_INSIDE_EPS ((btScalar)1e-9)
+#else
+ #define EPA_ACCURACY ((btScalar)0.0001)
+ #define EPA_PLANE_EPS ((btScalar)0.00001)
+ #define EPA_INSIDE_EPS ((btScalar)0.01)
+#endif
+
+#define EPA_FALLBACK (10*EPA_ACCURACY)
+#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
// Shorthands
@@ -242,7 +259,7 @@ namespace gjkepa2_impl
/* Check for termination */
const btScalar omega=btDot(m_ray,w)/rl;
alpha=btMax(omega,alpha);
- if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+ if(((rl-alpha)-(GJK_ACCURACY*rl))<=0)
{/* Return old simplex */
removevertice(m_simplices[m_current]);
break;
@@ -1015,7 +1032,7 @@ bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
/* Symbols cleanup */
#undef GJK_MAX_ITERATIONS
-#undef GJK_ACCURARY
+#undef GJK_ACCURACY
#undef GJK_MIN_DISTANCE
#undef GJK_DUPLICATED_EPS
#undef GJK_SIMPLEX2_EPS
diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index 759443a..a23ee3d 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -30,12 +30,16 @@ subject to the following restrictions:
#endif
//must be above the machine epsilon
-#define REL_ERROR2 btScalar(1.0e-6)
+#ifdef BT_USE_DOUBLE_PRECISION
+ #define REL_ERROR2 btScalar(1.0e-12)
+#else
+ #define REL_ERROR2 btScalar(1.0e-6)
+#endif
//temp globals, to improve GJK/EPA/penetration calculations
int gNumDeepPenetrationChecks = 0;
int gNumGjkChecks = 0;
-
+btScalar gGjkEpaPenetrationTolerance = 0.001;
btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
@@ -300,7 +304,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
}
bool catchDegeneratePenetrationCase =
- (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
+ (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
index e40fb1d..04ab54e 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
@@ -35,7 +35,13 @@ typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow;
typedef btConstraintRow PfxConstraintRow;
#endif //PFX_USE_FREE_VECTORMATH
-
+enum btContactPointFlags
+{
+ BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1,
+ BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
+ BT_CONTACT_FLAG_HAS_CONTACT_ERP=4,
+ BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8,
+};
/// ManifoldContactPoint collects and maintains persistent contactpoints.
/// used to improve stability and performance of rigidbody dynamics response.
@@ -44,14 +50,15 @@ class btManifoldPoint
public:
btManifoldPoint()
:m_userPersistentData(0),
- m_lateralFrictionInitialized(false),
- m_appliedImpulse(0.f),
+ m_contactPointFlags(0),
+ m_appliedImpulse(0.f),
m_appliedImpulseLateral1(0.f),
m_appliedImpulseLateral2(0.f),
m_contactMotion1(0.f),
m_contactMotion2(0.f),
- m_contactCFM1(0.f),
- m_contactCFM2(0.f),
+ m_contactCFM(0.f),
+ m_contactERP(0.f),
+ m_frictionCFM(0.f),
m_lifeTime(0)
{
}
@@ -65,16 +72,18 @@ class btManifoldPoint
m_distance1( distance ),
m_combinedFriction(btScalar(0.)),
m_combinedRollingFriction(btScalar(0.)),
- m_combinedRestitution(btScalar(0.)),
+ m_combinedSpinningFriction(btScalar(0.)),
+ m_combinedRestitution(btScalar(0.)),
m_userPersistentData(0),
- m_lateralFrictionInitialized(false),
- m_appliedImpulse(0.f),
+ m_contactPointFlags(0),
+ m_appliedImpulse(0.f),
m_appliedImpulseLateral1(0.f),
m_appliedImpulseLateral2(0.f),
m_contactMotion1(0.f),
m_contactMotion2(0.f),
- m_contactCFM1(0.f),
- m_contactCFM2(0.f),
+ m_contactCFM(0.f),
+ m_contactERP(0.f),
+ m_frictionCFM(0.f),
m_lifeTime(0)
{
@@ -91,8 +100,9 @@ class btManifoldPoint
btScalar m_distance1;
btScalar m_combinedFriction;
- btScalar m_combinedRollingFriction;
- btScalar m_combinedRestitution;
+ btScalar m_combinedRollingFriction;//torsional friction orthogonal to contact normal, useful to make spheres stop rolling forever
+ btScalar m_combinedSpinningFriction;//torsional friction around contact normal, useful for grasping objects
+ btScalar m_combinedRestitution;
//BP mod, store contact triangles.
int m_partId0;
@@ -101,15 +111,28 @@ class btManifoldPoint
int m_index1;
mutable void* m_userPersistentData;
- bool m_lateralFrictionInitialized;
-
+ //bool m_lateralFrictionInitialized;
+ int m_contactPointFlags;
+
btScalar m_appliedImpulse;
btScalar m_appliedImpulseLateral1;
btScalar m_appliedImpulseLateral2;
btScalar m_contactMotion1;
btScalar m_contactMotion2;
- btScalar m_contactCFM1;
- btScalar m_contactCFM2;
+
+ union
+ {
+ btScalar m_contactCFM;
+ btScalar m_combinedContactStiffness1;
+ };
+
+ union
+ {
+ btScalar m_contactERP;
+ btScalar m_combinedContactDamping1;
+ };
+
+ btScalar m_frictionCFM;
int m_lifeTime;//lifetime of the contactpoint in frames
diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
index 5026397..d220f29 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
@@ -163,7 +163,7 @@ public:
//get rid of duplicated userPersistentData pointer
m_pointCache[lastUsedIndex].m_userPersistentData = 0;
m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
- m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false;
+ m_pointCache[lastUsedIndex].m_contactPointFlags = 0;
m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
m_pointCache[lastUsedIndex].m_lifeTime = 0;
@@ -190,7 +190,6 @@ public:
void* cache = m_pointCache[insertIndex].m_userPersistentData;
m_pointCache[insertIndex] = newPoint;
-
m_pointCache[insertIndex].m_userPersistentData = cache;
m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
diff --git a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
index d5f4a96..ea380bc 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
+++ b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
@@ -411,9 +411,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
return true;
}
-void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
+void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
- btVertexArray worldVertsB2;
+ worldVertsB2.resize(0);
btVertexArray* pVtxIn = &worldVertsB1;
btVertexArray* pVtxOut = &worldVertsB2;
pVtxOut->reserve(pVtxIn->size());
@@ -527,7 +527,7 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
-void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
+void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
btVector3 separatingNormal = separatingNormal1.normalized();
@@ -552,7 +552,7 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
}
}
}
- btVertexArray worldVertsB1;
+ worldVertsB1.resize(0);
{
const btFace& polyB = hullB.m_faces[closestFaceB];
const int numVertices = polyB.m_indices.size();
@@ -565,6 +565,6 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
if (closestFaceB>=0)
- clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
+ clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, worldVertsB2,minDist, maxDist,resultOut);
}
diff --git a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
index b87bd4f..30e3db6 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
@@ -32,8 +32,11 @@ typedef btAlignedObjectArray<btVector3> btVertexArray;
// Clips a face to the back of a plane
struct btPolyhedralContactClipping
{
- static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut);
- static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut);
+
+ static void clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut);
+
+ static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut);
+
static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut);
diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt
index b8c37b0..656fc84 100644
--- a/src/BulletDynamics/CMakeLists.txt
+++ b/src/BulletDynamics/CMakeLists.txt
@@ -32,6 +32,8 @@ SET(BulletDynamics_SRCS
Featherstone/btMultiBodyJointLimitConstraint.cpp
Featherstone/btMultiBodyConstraint.cpp
Featherstone/btMultiBodyPoint2Point.cpp
+ Featherstone/btMultiBodyFixedConstraint.cpp
+ Featherstone/btMultiBodySliderConstraint.cpp
Featherstone/btMultiBodyJointMotor.cpp
MLCPSolvers/btDantzigLCP.cpp
MLCPSolvers/btMLCPSolver.cpp
@@ -89,6 +91,8 @@ SET(Featherstone_HDRS
Featherstone/btMultiBodyJointLimitConstraint.h
Featherstone/btMultiBodyConstraint.h
Featherstone/btMultiBodyPoint2Point.h
+ Featherstone/btMultiBodyFixedConstraint.h
+ Featherstone/btMultiBodySliderConstraint.h
Featherstone/btMultiBodyJointMotor.h
)
diff --git a/src/BulletDynamics/Character/btCharacterControllerInterface.h b/src/BulletDynamics/Character/btCharacterControllerInterface.h
index dffb06d..c3a3ac6 100644
--- a/src/BulletDynamics/Character/btCharacterControllerInterface.h
+++ b/src/BulletDynamics/Character/btCharacterControllerInterface.h
@@ -37,7 +37,7 @@ public:
virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
virtual bool canJump () const = 0;
- virtual void jump () = 0;
+ virtual void jump(const btVector3& dir = btVector3()) = 0;
virtual bool onGround () const = 0;
virtual void setUpInterpolate (bool value) = 0;
diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/src/BulletDynamics/Character/btKinematicCharacterController.cpp
index 31faf1d..68fa520 100644
--- a/src/BulletDynamics/Character/btKinematicCharacterController.cpp
+++ b/src/BulletDynamics/Character/btKinematicCharacterController.cpp
@@ -132,30 +132,37 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector
return direction - parallelComponent(direction, normal);
}
-btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
+btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up)
{
- m_upAxis = upAxis;
- m_addedMargin = 0.02;
- m_walkDirection.setValue(0,0,0);
- m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
- m_stepHeight = stepHeight;
+ m_up.setValue(0.0f, 0.0f, 1.0f);
+ m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
+ setUp(up);
+ setStepHeight(stepHeight);
+ m_addedMargin = 0.02;
+ m_walkDirection.setValue(0.0,0.0,0.0);
+ m_AngVel.setValue(0.0, 0.0, 0.0);
+ m_useGhostObjectSweepTest = true;
m_turnAngle = btScalar(0.0);
m_convexShape=convexShape;
m_useWalkDirection = true; // use walk direction by default, legacy behavior
m_velocityTimeInterval = 0.0;
m_verticalVelocity = 0.0;
m_verticalOffset = 0.0;
- m_gravity = 9.8 * 3 ; // 3G acceleration.
+ m_gravity = 9.8 * 3.0 ; // 3G acceleration.
m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
m_jumpSpeed = 10.0; // ?
+ m_SetjumpSpeed = m_jumpSpeed;
m_wasOnGround = false;
m_wasJumping = false;
m_interpolateUp = true;
setMaxSlope(btRadians(45.0));
- m_currentStepOffset = 0;
+ m_currentStepOffset = 0.0;
+ m_maxPenetrationDepth = 0.2;
full_drop = false;
bounce_fix = false;
+ m_linearDamping = btScalar(0.0);
+ m_angularDamping = btScalar(0.0);
}
btKinematicCharacterController::~btKinematicCharacterController ()
@@ -190,7 +197,7 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
- btScalar maxPen = btScalar(0.0);
+// btScalar maxPen = btScalar(0.0);
for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
{
m_manifoldArray.resize(0);
@@ -198,10 +205,13 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
- btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
+ btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
continue;
+
+ if (!needsCollision(obj0, obj1))
+ continue;
if (collisionPair->m_algorithm)
collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
@@ -217,14 +227,15 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
btScalar dist = pt.getDistance();
- if (dist < 0.0)
+ if (dist < -m_maxPenetrationDepth)
{
- if (dist < maxPen)
- {
- maxPen = dist;
- m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+ // TODO: cause problems on slopes, not sure if it is needed
+ //if (dist < maxPen)
+ //{
+ // maxPen = dist;
+ // m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
- }
+ //}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
penetration = true;
} else {
@@ -244,18 +255,28 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
{
+ btScalar stepHeight = 0.0f;
+ if (m_verticalVelocity < 0.0)
+ stepHeight = m_stepHeight;
+
// phase 1: up
btTransform start, end;
- m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
start.setIdentity ();
end.setIdentity ();
/* FIXME: Handle penetration properly */
- start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
+ start.setOrigin(m_currentPosition);
+
+ m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
+ m_currentPosition = m_targetPosition;
+
end.setOrigin (m_targetPosition);
- btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
+ start.setRotation(m_currentOrientation);
+ end.setRotation(m_targetOrientation);
+
+ btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -265,29 +286,61 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
}
else
{
- world->convexSweepTest (m_convexShape, start, end, callback);
+ world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
}
-
- if (callback.hasHit())
+
+ if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// Only modify the position if the hit was a slope and not a wall or ceiling.
- if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
+ if (callback.m_hitNormalWorld.dot(m_up) > 0.0)
{
// we moved up only a fraction of the step height
- m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+ m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
if (m_interpolateUp == true)
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
else
m_currentPosition = m_targetPosition;
}
- m_verticalVelocity = 0.0;
- m_verticalOffset = 0.0;
+
+ btTransform& xform = m_ghostObject->getWorldTransform();
+ xform.setOrigin(m_currentPosition);
+ m_ghostObject->setWorldTransform(xform);
+
+ // fix penetration if we hit a ceiling for example
+ int numPenetrationLoops = 0;
+ m_touchingContact = false;
+ while (recoverFromPenetration(world))
+ {
+ numPenetrationLoops++;
+ m_touchingContact = true;
+ if (numPenetrationLoops > 4)
+ {
+ //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+ break;
+ }
+ }
+ m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
+ m_currentPosition = m_targetPosition;
+
+ if (m_verticalOffset > 0)
+ {
+ m_verticalOffset = 0.0;
+ m_verticalVelocity = 0.0;
+ m_currentStepOffset = m_stepHeight;
+ }
} else {
- m_currentStepOffset = m_stepHeight;
+ m_currentStepOffset = stepHeight;
m_currentPosition = m_targetPosition;
}
}
+bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
+{
+ bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
+ collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
+ return collides;
+}
+
void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
{
btVector3 movementDirection = m_targetPosition - m_currentPosition;
@@ -330,6 +383,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
// phase 2: forward and strafe
btTransform start, end;
+
m_targetPosition = m_currentPosition + walkMove;
start.setIdentity ();
@@ -339,15 +393,6 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
// printf("distance2=%f\n",distance2);
- if (m_touchingContact)
- {
- if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
- {
- //interferes with step movement
- //updateTargetPositionBasedOnCollision (m_touchingNormal);
- }
- }
-
int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0)
@@ -356,6 +401,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
end.setOrigin (m_targetPosition);
btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
+ start.setRotation(m_currentOrientation);
+ end.setRotation(m_targetOrientation);
+
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -364,21 +412,23 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
btScalar margin = m_convexShape->getMargin();
m_convexShape->setMargin(margin + m_addedMargin);
-
- if (m_useGhostObjectSweepTest)
- {
- m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
- } else
+ if (!(start == end))
{
- collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ if (m_useGhostObjectSweepTest)
+ {
+ m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ }
+ else
+ {
+ collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ }
}
-
m_convexShape->setMargin(margin);
fraction -= callback.m_closestHitFraction;
- if (callback.hasHit())
+ if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
{
// we moved only a fraction
//btScalar hitDistance;
@@ -403,14 +453,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
break;
}
- } else {
- // we moved whole way
- m_currentPosition = m_targetPosition;
}
-
- // if (callback.m_closestHitFraction == 0.f)
- // break;
-
+ else
+ {
+ m_currentPosition = m_targetPosition;
+ }
}
}
@@ -421,27 +468,30 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
// phase 3: down
/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
- btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
+ btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
- btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
+ btVector3 gravity_drop = m_up * downVelocity;
m_targetPosition -= (step_drop + gravity_drop);*/
btVector3 orig_position = m_targetPosition;
btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+ if (m_verticalVelocity > 0.0)
+ return;
+
if(downVelocity > 0.0 && downVelocity > m_fallSpeed
&& (m_wasOnGround || !m_wasJumping))
downVelocity = m_fallSpeed;
- btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+ btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
- btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+ btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
- btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+ btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
@@ -455,6 +505,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition);
+ start.setRotation(m_currentOrientation);
+ end.setRotation(m_targetOrientation);
+
//set double test for 2x the step drop, to check for a large drop vs small drop
end_double.setOrigin (m_targetPosition - step_drop);
@@ -462,7 +515,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
- if (!callback.hasHit())
+ if (!callback.hasHit() && m_ghostObject->hasContactResponse())
{
//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
@@ -471,30 +524,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
- if (!callback.hasHit())
- {
- //test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
- collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
- }
+ if (!callback.hasHit() && m_ghostObject->hasContactResponse())
+ {
+ //test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
+ collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+ }
}
btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
- bool has_hit = false;
+ bool has_hit;
if (bounce_fix == true)
- has_hit = callback.hasHit() || callback2.hasHit();
+ has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
else
- has_hit = callback2.hasHit();
+ has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject);
+
+ btScalar stepHeight = 0.0f;
+ if (m_verticalVelocity < 0.0)
+ stepHeight = m_stepHeight;
- if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
+ if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false
&& (m_wasOnGround || !m_wasJumping))
{
//redo the velocity calculation when falling a small amount, for fast stairs motion
//for larger falls, use the smoother/slower interpolated movement by not touching the target position
m_targetPosition = orig_position;
- downVelocity = m_stepHeight;
+ downVelocity = stepHeight;
- btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+ step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
runonce = true;
continue; //re-run previous tests
@@ -502,10 +559,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
break;
}
- if (callback.hasHit() || runonce == true)
+ if (m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) || runonce == true)
{
// we dropped a fraction of the height -> hit floor
-
btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
@@ -513,10 +569,10 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
if (bounce_fix == true)
{
if (full_drop == true)
- m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
- else
- //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
- m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
+ m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+ else
+ //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
+ m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
}
else
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
@@ -528,7 +584,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
m_wasJumping = false;
} else {
// we dropped the full height
-
+
full_drop = true;
if (bounce_fix == true)
@@ -538,7 +594,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
{
m_targetPosition += step_drop; //undo previous target change
downVelocity = m_fallSpeed;
- step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+ step_drop = m_up * (m_currentStepOffset + downVelocity);
m_targetPosition -= step_drop;
}
}
@@ -579,21 +635,63 @@ btScalar timeInterval
m_velocityTimeInterval += timeInterval;
}
-void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
+void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
{
- m_verticalVelocity = 0.0;
- m_verticalOffset = 0.0;
- m_wasOnGround = false;
- m_wasJumping = false;
- m_walkDirection.setValue(0,0,0);
- m_velocityTimeInterval = 0.0;
+ m_AngVel = velocity;
+}
- //clear pair cache
- btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
- while (cache->getOverlappingPairArray().size() > 0)
- {
- cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
- }
+const btVector3& btKinematicCharacterController::getAngularVelocity() const
+{
+ return m_AngVel;
+}
+
+void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity)
+{
+ m_walkDirection = velocity;
+
+ // HACK: if we are moving in the direction of the up, treat it as a jump :(
+ if (m_walkDirection.length2() > 0)
+ {
+ btVector3 w = velocity.normalized();
+ btScalar c = w.dot(m_up);
+ if (c != 0)
+ {
+ //there is a component in walkdirection for vertical velocity
+ btVector3 upComponent = m_up * (sinf(SIMD_HALF_PI - acosf(c)) * m_walkDirection.length());
+ m_walkDirection -= upComponent;
+ m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
+
+ if (c > 0.0f)
+ {
+ m_wasJumping = true;
+ m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+ }
+ }
+ }
+ else
+ m_verticalVelocity = 0.0f;
+}
+
+btVector3 btKinematicCharacterController::getLinearVelocity() const
+{
+ return m_walkDirection + (m_verticalVelocity * m_up);
+}
+
+void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
+{
+ m_verticalVelocity = 0.0;
+ m_verticalOffset = 0.0;
+ m_wasOnGround = false;
+ m_wasJumping = false;
+ m_walkDirection.setValue(0,0,0);
+ m_velocityTimeInterval = 0.0;
+
+ //clear pair cache
+ btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
+ while (cache->getOverlappingPairArray().size() > 0)
+ {
+ cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
+ }
}
void btKinematicCharacterController::warp (const btVector3& origin)
@@ -607,62 +705,99 @@ void btKinematicCharacterController::warp (const btVector3& origin)
void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
{
-
- int numPenetrationLoops = 0;
- m_touchingContact = false;
- while (recoverFromPenetration (collisionWorld))
- {
- numPenetrationLoops++;
- m_touchingContact = true;
- if (numPenetrationLoops > 4)
- {
- //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
- break;
- }
- }
-
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition;
-// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
-
+ m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
+ m_targetOrientation = m_currentOrientation;
+// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
}
-#include <stdio.h>
-
void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
{
// printf("playerStep(): ");
// printf(" dt = %f", dt);
+ if (m_AngVel.length2() > 0.0f)
+ {
+ m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
+ }
+
+ // integrate for angular velocity
+ if (m_AngVel.length2() > 0.0f)
+ {
+ btTransform xform;
+ xform = m_ghostObject->getWorldTransform();
+
+ btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
+
+ btQuaternion orn = rot * xform.getRotation();
+
+ xform.setRotation(orn);
+ m_ghostObject->setWorldTransform(xform);
+
+ m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
+ m_targetPosition = m_currentPosition;
+ m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
+ m_targetOrientation = m_currentOrientation;
+ }
+
// quick check...
- if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
+ if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) {
// printf("\n");
return; // no motion
}
m_wasOnGround = onGround();
+ //btVector3 lvel = m_walkDirection;
+ btScalar c = 0.0f;
+
+ if (m_walkDirection.length2() > 0)
+ {
+ // apply damping
+ m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
+ }
+
+ m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
+
// Update fall velocity.
m_verticalVelocity -= m_gravity * dt;
- if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+ if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
{
m_verticalVelocity = m_jumpSpeed;
}
- if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+ if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
{
m_verticalVelocity = -btFabs(m_fallSpeed);
}
m_verticalOffset = m_verticalVelocity * dt;
-
btTransform xform;
- xform = m_ghostObject->getWorldTransform ();
+ xform = m_ghostObject->getWorldTransform();
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
- stepUp (collisionWorld);
+ stepUp(collisionWorld);
+ //todo: Experimenting with behavior of controller when it hits a ceiling..
+ //bool hitUp = stepUp (collisionWorld);
+ //if (hitUp)
+ //{
+ // m_verticalVelocity -= m_gravity * dt;
+ // if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+ // {
+ // m_verticalVelocity = m_jumpSpeed;
+ // }
+ // if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+ // {
+ // m_verticalVelocity = -btFabs(m_fallSpeed);
+ // }
+ // m_verticalOffset = m_verticalVelocity * dt;
+
+ // xform = m_ghostObject->getWorldTransform();
+ //}
+
if (m_useWalkDirection) {
stepForwardAndStrafe (collisionWorld, m_walkDirection);
} else {
@@ -682,10 +817,38 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo
}
stepDown (collisionWorld, dt);
+ //todo: Experimenting with max jump height
+ //if (m_wasJumping)
+ //{
+ // btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
+ // if (ds > m_maxJumpHeight)
+ // {
+ // // substract the overshoot
+ // m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
+
+ // // max height was reached, so potential energy is at max
+ // // and kinematic energy is 0, thus velocity is 0.
+ // if (m_verticalVelocity > 0.0)
+ // m_verticalVelocity = 0.0;
+ // }
+ //}
// printf("\n");
xform.setOrigin (m_currentPosition);
m_ghostObject->setWorldTransform (xform);
+
+ int numPenetrationLoops = 0;
+ m_touchingContact = false;
+ while (recoverFromPenetration(collisionWorld))
+ {
+ numPenetrationLoops++;
+ m_touchingContact = true;
+ if (numPenetrationLoops > 4)
+ {
+ //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+ break;
+ }
+ }
}
void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
@@ -696,6 +859,7 @@ void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
{
m_jumpSpeed = jumpSpeed;
+ m_SetjumpSpeed = m_jumpSpeed;
}
void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
@@ -708,14 +872,16 @@ bool btKinematicCharacterController::canJump () const
return onGround();
}
-void btKinematicCharacterController::jump ()
+void btKinematicCharacterController::jump(const btVector3& v)
{
- if (!canJump())
- return;
-
+ m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
m_verticalVelocity = m_jumpSpeed;
m_wasJumping = true;
+ m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
+
+ m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+
#if 0
currently no jumping.
btTransform xform;
@@ -727,14 +893,16 @@ void btKinematicCharacterController::jump ()
#endif
}
-void btKinematicCharacterController::setGravity(btScalar gravity)
+void btKinematicCharacterController::setGravity(const btVector3& gravity)
{
- m_gravity = gravity;
+ if (gravity.length2() > 0) setUpVector(-gravity);
+
+ m_gravity = gravity.length();
}
-btScalar btKinematicCharacterController::getGravity() const
+btVector3 btKinematicCharacterController::getGravity() const
{
- return m_gravity;
+ return -m_gravity * m_up;
}
void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
@@ -748,11 +916,25 @@ btScalar btKinematicCharacterController::getMaxSlope() const
return m_maxSlopeRadians;
}
+void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
+{
+ m_maxPenetrationDepth = d;
+}
+
+btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
+{
+ return m_maxPenetrationDepth;
+}
+
bool btKinematicCharacterController::onGround () const
{
- return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
+ return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
}
+void btKinematicCharacterController::setStepHeight(btScalar h)
+{
+ m_stepHeight = h;
+}
btVector3* btKinematicCharacterController::getUpAxisDirections()
{
@@ -769,3 +951,49 @@ void btKinematicCharacterController::setUpInterpolate(bool value)
{
m_interpolateUp = value;
}
+
+void btKinematicCharacterController::setUp(const btVector3& up)
+{
+ if (up.length2() > 0 && m_gravity > 0.0f)
+ {
+ setGravity(-m_gravity * up.normalized());
+ return;
+ }
+
+ setUpVector(up);
+}
+
+void btKinematicCharacterController::setUpVector(const btVector3& up)
+{
+ if (m_up == up)
+ return;
+
+ btVector3 u = m_up;
+
+ if (up.length2() > 0)
+ m_up = up.normalized();
+ else
+ m_up = btVector3(0.0, 0.0, 0.0);
+
+ if (!m_ghostObject) return;
+ btQuaternion rot = getRotation(m_up, u);
+
+ //set orientation with new up
+ btTransform xform;
+ xform = m_ghostObject->getWorldTransform();
+ btQuaternion orn = rot.inverse() * xform.getRotation();
+ xform.setRotation(orn);
+ m_ghostObject->setWorldTransform(xform);
+}
+
+btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const
+{
+ if (v0.length2() == 0.0f || v1.length2() == 0.0f)
+ {
+ btQuaternion q;
+ return q;
+ }
+
+ return shortestArcQuatNormalize2(v0, v1);
+}
+
diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.h b/src/BulletDynamics/Character/btKinematicCharacterController.h
index add6f30..3d677e6 100644
--- a/src/BulletDynamics/Character/btKinematicCharacterController.h
+++ b/src/BulletDynamics/Character/btKinematicCharacterController.h
@@ -43,10 +43,12 @@ protected:
btPairCachingGhostObject* m_ghostObject;
btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
+ btScalar m_maxPenetrationDepth;
btScalar m_verticalVelocity;
btScalar m_verticalOffset;
btScalar m_fallSpeed;
btScalar m_jumpSpeed;
+ btScalar m_SetjumpSpeed;
btScalar m_maxJumpHeight;
btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
@@ -61,24 +63,34 @@ protected:
///this is the desired walk direction, set by the user
btVector3 m_walkDirection;
btVector3 m_normalizedDirection;
+ btVector3 m_AngVel;
+
+ btVector3 m_jumpPosition;
//some internal variables
btVector3 m_currentPosition;
btScalar m_currentStepOffset;
btVector3 m_targetPosition;
+ btQuaternion m_currentOrientation;
+ btQuaternion m_targetOrientation;
+
///keep track of the contact manifolds
btManifoldArray m_manifoldArray;
bool m_touchingContact;
btVector3 m_touchingNormal;
+ btScalar m_linearDamping;
+ btScalar m_angularDamping;
+
bool m_wasOnGround;
bool m_wasJumping;
bool m_useGhostObjectSweepTest;
bool m_useWalkDirection;
btScalar m_velocityTimeInterval;
- int m_upAxis;
+ btVector3 m_up;
+ btVector3 m_jumpAxis;
static btVector3* getUpAxisDirections();
bool m_interpolateUp;
@@ -94,11 +106,18 @@ protected:
void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
+
+ virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
+
+ void setUpVector(const btVector3& up);
+
+ btQuaternion getRotation(btVector3& v0, btVector3& v1) const;
+
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
- btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
+ btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0));
~btKinematicCharacterController ();
@@ -112,14 +131,9 @@ public:
///btActionInterface interface
void debugDraw(btIDebugDraw* debugDrawer);
- void setUpAxis (int axis)
- {
- if (axis < 0)
- axis = 0;
- if (axis > 2)
- axis = 2;
- m_upAxis = axis;
- }
+ void setUp(const btVector3& up);
+
+ const btVector3& getUp() { return m_up; }
/// This should probably be called setPositionIncrementPerSimulatorStep.
/// This is neither a direction nor a velocity, but the amount to
@@ -136,27 +150,47 @@ public:
virtual void setVelocityForTimeInterval(const btVector3& velocity,
btScalar timeInterval);
+ virtual void setAngularVelocity(const btVector3& velocity);
+ virtual const btVector3& getAngularVelocity() const;
+
+ virtual void setLinearVelocity(const btVector3& velocity);
+ virtual btVector3 getLinearVelocity() const;
+
+ void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+ btScalar getLinearDamping() const { return m_linearDamping; }
+ void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+ btScalar getAngularDamping() const { return m_angularDamping; }
+
void reset ( btCollisionWorld* collisionWorld );
void warp (const btVector3& origin);
void preStep ( btCollisionWorld* collisionWorld);
void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
+ void setStepHeight(btScalar h);
+ btScalar getStepHeight() const { return m_stepHeight; }
void setFallSpeed (btScalar fallSpeed);
+ btScalar getFallSpeed() const { return m_fallSpeed; }
void setJumpSpeed (btScalar jumpSpeed);
+ btScalar getJumpSpeed() const { return m_jumpSpeed; }
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
- void jump ();
+ void jump(const btVector3& v = btVector3());
+
+ void applyImpulse(const btVector3& v) { jump(v); }
- void setGravity(btScalar gravity);
- btScalar getGravity() const;
+ void setGravity(const btVector3& gravity);
+ btVector3 getGravity() const;
/// The max slope determines the maximum angle that the controller can walk up.
/// The slope angle is measured in radians.
void setMaxSlope(btScalar slopeRadians);
btScalar getMaxSlope() const;
+ void setMaxPenetrationDepth(btScalar d);
+ btScalar getMaxPenetrationDepth() const;
+
btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{
diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
index a3a0fa6..cf310da 100644
--- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
+++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
@@ -77,7 +77,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_maxErrorReduction = btScalar(20.);
m_numIterations = 10;
m_erp = btScalar(0.2);
- m_erp2 = btScalar(0.8);
+ m_erp2 = btScalar(0.2);
m_globalCfm = btScalar(0.);
m_sor = btScalar(1.);
m_splitImpulse = true;
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
index 738d52c..57b4e19 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
@@ -319,14 +319,6 @@ protected:
const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
- static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
- static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
- static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
- static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
- static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
- static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
- static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
-
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -489,6 +481,14 @@ public:
//If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
virtual btScalar getParam(int num, int axis = -1) const;
+
+ static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
+ static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
+ static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
+ static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
+ static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
+ static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
+ static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
};
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 8da572b..aced8fa 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -627,8 +627,8 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
}
-void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
- btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
+void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
+ btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity, btScalar cfmSlip)
@@ -647,8 +647,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
- solverConstraint.m_friction = cp.m_combinedRollingFriction;
- solverConstraint.m_originalContactPoint = 0;
+ solverConstraint.m_friction = combinedTorsionalFriction;
+ solverConstraint.m_originalContactPoint = 0;
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
@@ -704,11 +704,11 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv
-btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
{
btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
- setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
+ setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction,rel_pos1, rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
return solverConstraint;
}
@@ -775,7 +775,37 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
- relaxation = 1.f;
+ relaxation = infoGlobal.m_sor;
+ btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
+
+ //cfm = 1 / ( dt * kp + kd )
+ //erp = dt * kp / ( dt * kp + kd )
+
+ btScalar cfm = infoGlobal.m_globalCfm;
+ btScalar erp = infoGlobal.m_erp2;
+
+ if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
+ {
+ if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
+ cfm = cp.m_contactCFM;
+ if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
+ erp = cp.m_contactERP;
+ } else
+ {
+ if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
+ {
+ btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
+ if (denom < SIMD_EPSILON)
+ {
+ denom = SIMD_EPSILON;
+ }
+ cfm = btScalar(1) / denom;
+ erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
+ }
+ }
+
+ cfm *= invTimeStep;
+
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -802,7 +832,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
}
#endif //COMPUTE_IMPULSE_DENOM
- btScalar denom = relaxation/(denom0+denom1);
+ btScalar denom = relaxation/(denom0+denom1+cfm);
solverConstraint.m_jacDiagABInv = denom;
}
@@ -884,20 +914,16 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
btScalar velocityError = restitution - rel_vel;// * damping;
- btScalar erp = infoGlobal.m_erp2;
- if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
- {
- erp = infoGlobal.m_erp;
- }
-
+
if (penetration>0)
{
positionalError = 0;
- velocityError -= penetration / infoGlobal.m_timeStep;
+ velocityError -= penetration *invTimeStep;
} else
{
- positionalError = -penetration * erp/infoGlobal.m_timeStep;
+ positionalError = -penetration * erp*invTimeStep;
+
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
@@ -915,7 +941,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
- solverConstraint.m_cfm = 0.f;
+ solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
@@ -1043,38 +1069,26 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
- btVector3 angVelA(0,0,0),angVelB(0,0,0);
- if (rb0)
- angVelA = rb0->getAngularVelocity();
- if (rb1)
- angVelB = rb1->getAngularVelocity();
- btVector3 relAngVel = angVelB-angVelA;
-
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{
- //only a single rollingFriction per manifold
- rollingFriction--;
- if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
- {
- relAngVel.normalize();
- applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
- applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
- if (relAngVel.length()>0.001)
- addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
- } else
+
{
- addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
btVector3 axis0,axis1;
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
+ axis0.normalize();
+ axis1.normalize();
+
applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (axis0.length()>0.001)
- addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
+ cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if (axis1.length()>0.001)
- addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
+ cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
}
@@ -1094,7 +1108,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
- if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+ if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
@@ -1132,16 +1146,16 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
{
- cp.m_lateralFrictionInitialized = true;
+ cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
}
}
} else
{
- addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
+ addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
- addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
+ addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM);
}
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index a602918..7eafa3a 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -54,13 +54,13 @@ protected:
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
- void setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
- btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
+ void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
+ btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
- btSolverConstraint& addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
+ btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
index 628ada4..1957f08 100755
--- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
@@ -25,6 +25,8 @@ TODO:
#ifndef BT_SLIDER_CONSTRAINT_H
#define BT_SLIDER_CONSTRAINT_H
+#include "LinearMath/btScalar.h"//for BT_USE_DOUBLE_PRECISION
+
#ifdef BT_USE_DOUBLE_PRECISION
#define btSliderConstraintData2 btSliderConstraintDoubleData
#define btSliderConstraintDataName "btSliderConstraintDoubleData"
diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
index b55db0c..8a2a2d1 100644
--- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
@@ -145,11 +145,6 @@ public:
// lo and hi limits for variables (set to -/+ infinity on entry).
btScalar *m_lowerLimit,*m_upperLimit;
- // findex vector for variables. see the LCP solver interface for a
- // description of what this does. this is set to -1 on entry.
- // note that the returned indexes are relative to the first index of
- // the constraint.
- int *findex;
// number of solver iterations
int m_numIterations;
diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp
index e0e8bc7..9402a65 100644
--- a/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -79,6 +79,8 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
//moved to btCollisionObject
m_friction = constructionInfo.m_friction;
m_rollingFriction = constructionInfo.m_rollingFriction;
+ m_spinningFriction = constructionInfo.m_spinningFriction;
+
m_restitution = constructionInfo.m_restitution;
setCollisionShape( constructionInfo.m_collisionShape );
diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h
index 1d177db..3722450 100644
--- a/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -135,6 +135,8 @@ public:
///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever.
///See Bullet/Demos/RollingFrictionDemo for usage
btScalar m_rollingFriction;
+ btScalar m_spinningFriction;//torsional friction around contact normal
+
///best simulation results using zero restitution.
btScalar m_restitution;
@@ -158,6 +160,7 @@ public:
m_angularDamping(btScalar(0.)),
m_friction(btScalar(0.5)),
m_rollingFriction(btScalar(0)),
+ m_spinningFriction(btScalar(0)),
m_restitution(btScalar(0.)),
m_linearSleepingThreshold(btScalar(0.8)),
m_angularSleepingThreshold(btScalar(1.f)),
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 5d62da7..b856568 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -28,7 +28,7 @@
#include "btMultiBodyJointFeedback.h"
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btSerializer.h"
-#include "Bullet3Common/b3Logging.h"
+//#include "Bullet3Common/b3Logging.h"
// #define INCLUDE_GYRO_TERM
///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
@@ -106,7 +106,9 @@ btMultiBody::btMultiBody(int n_links,
m_awake(true),
m_canSleep(canSleep),
m_sleepTimer(0),
-
+ m_userObjectPointer(0),
+ m_userIndex2(-1),
+ m_userIndex(-1),
m_linearDamping(0.04f),
m_angularDamping(0.04f),
m_useGyroTerm(true),
@@ -120,10 +122,15 @@ btMultiBody::btMultiBody(int n_links,
m_useGlobalVelocities(false),
m_internalNeedsJointFeedback(false)
{
+ m_cachedInertiaTopLeft.setValue(0,0,0,0,0,0,0,0,0);
+ m_cachedInertiaTopRight.setValue(0,0,0,0,0,0,0,0,0);
+ m_cachedInertiaLowerLeft.setValue(0,0,0,0,0,0,0,0,0);
+ m_cachedInertiaLowerRight.setValue(0,0,0,0,0,0,0,0,0);
+ m_cachedInertiaValid=false;
+
m_links.resize(n_links);
m_matrixBuf.resize(n_links + 1);
-
m_baseForce.setValue(0, 0, 0);
m_baseTorque.setValue(0, 0, 0);
}
@@ -299,7 +306,7 @@ void btMultiBody::setupPlanar(int i,
m_links[i].m_eVector = parentComToThisComOffset;
//
- static btVector3 vecNonParallelToRotAxis(1, 0, 0);
+ btVector3 vecNonParallelToRotAxis(1, 0, 0);
if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
vecNonParallelToRotAxis.setValue(0, 1, 0);
//
@@ -466,6 +473,16 @@ btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
}
}
+btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
+{
+ btMatrix3x3 result = local_frame;
+ btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));
+ btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));
+ btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));
+ result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
+ return result;
+}
+
void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
{
int num_links = getNumLinks();
@@ -714,15 +731,15 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
btScalar * Y = &scratch_r[0];
//
//aux variables
- static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
- static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
- static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
- static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
- static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
- static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
- static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
- static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
- static btSpatialTransformationMatrix fromWorld;
+ btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
+ btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
+ btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+ btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+ btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
+ btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
+ btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
+ btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
+ btSpatialTransformationMatrix fromWorld;
fromWorld.m_trnVec.setZero();
/////////////////
@@ -752,8 +769,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.;
- zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
- linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
+ zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
+ linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
//
//p += vhat x Ihat vhat - done in a simpler way
@@ -839,8 +856,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
//
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.;
- zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
- linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
+ zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().safeNorm()),
+ linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().safeNorm()));
// calculate Ihat_i^A
//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
@@ -919,8 +936,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
case btMultibodyLink::eSpherical:
case btMultibodyLink::ePlanar:
{
- static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
- static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
+ btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
+ btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
//unroll the loop?
for(int row = 0; row < 3; ++row)
@@ -1000,6 +1017,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
{
if (num_links > 0)
{
+ m_cachedInertiaValid = true;
m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
@@ -1203,6 +1221,14 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo
result[5] = rhs_top[2] / m_baseMass;
} else
{
+ if (!m_cachedInertiaValid)
+ {
+ for (int i=0;i<6;i++)
+ {
+ result[i] = 0.f;
+ }
+ return;
+ }
/// Special routine for calculating the inverse of a spatial inertia matrix
///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
@@ -1249,6 +1275,13 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV
{
/// Special routine for calculating the inverse of a spatial inertia matrix
///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
+ if (!m_cachedInertiaValid)
+ {
+ result.setLinear(btVector3(0,0,0));
+ result.setAngular(btVector3(0,0,0));
+ result.setVector(btVector3(0,0,0),btVector3(0,0,0));
+ return;
+ }
btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
@@ -1323,11 +1356,11 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
btScalar * Y = r_ptr;
////////////////
//aux variables
- static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
- static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
- static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
- static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
- static btSpatialTransformationMatrix fromParent;
+ btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+ btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+ btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
+ btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
+ btSpatialTransformationMatrix fromParent;
/////////////////
// First 'upward' loop.
@@ -1522,8 +1555,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
btScalar *pBaseQuat = pq ? pq : m_baseQuat;
btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
//
- static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
- static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+ btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+ btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
pQuatUpdateFun(baseOmega, baseQuat, true, dt);
pBaseQuat[0] = baseQuat.x();
pBaseQuat[1] = baseQuat.y();
@@ -1557,8 +1590,8 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
}
case btMultibodyLink::eSpherical:
{
- static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
- static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+ btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+ btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
pQuatUpdateFun(jointVel, jointOri, false, dt);
pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
break;
@@ -1899,6 +1932,9 @@ const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
memPtr->m_linkMass = getLink(i).m_mass;
memPtr->m_parentIndex = getLink(i).m_parent;
+ memPtr->m_jointDamping = getLink(i).m_jointDamping;
+ memPtr->m_jointFriction = getLink(i).m_jointFriction;
+
getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index 62bed98..82bfbf3 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -272,7 +272,11 @@ public:
btVector3 localDirToWorld(int i, const btVector3 &vec) const;
btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
-
+
+ //
+ // transform a frame in local coordinate to a frame in world coordinate
+ //
+ btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const;
//
// calculate kinetic energy and angular momentum
@@ -573,6 +577,38 @@ void addJointTorque(int i, btScalar Q);
m_baseName = name;
}
+ ///users can point to their objects, userPointer is not used by Bullet
+ void* getUserPointer() const
+ {
+ return m_userObjectPointer;
+ }
+
+ int getUserIndex() const
+ {
+ return m_userIndex;
+ }
+
+ int getUserIndex2() const
+ {
+ return m_userIndex2;
+ }
+ ///users can point to their objects, userPointer is not used by Bullet
+ void setUserPointer(void* userPointer)
+ {
+ m_userObjectPointer = userPointer;
+ }
+
+ ///users can point to their objects, userPointer is not used by Bullet
+ void setUserIndex(int index)
+ {
+ m_userIndex = index;
+ }
+
+ void setUserIndex2(int index)
+ {
+ m_userIndex2 = index;
+ }
+
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented
@@ -641,6 +677,7 @@ private:
btMatrix3x3 m_cachedInertiaTopRight;
btMatrix3x3 m_cachedInertiaLowerLeft;
btMatrix3x3 m_cachedInertiaLowerRight;
+ bool m_cachedInertiaValid;
bool m_fixedBase;
@@ -649,6 +686,10 @@ private:
bool m_canSleep;
btScalar m_sleepTimer;
+ void* m_userObjectPointer;
+ int m_userIndex2;
+ int m_userIndex;
+
int m_companionId;
btScalar m_linearDamping;
btScalar m_angularDamping;
@@ -673,27 +714,25 @@ struct btMultiBodyLinkDoubleData
btVector3DoubleData m_jointAxisTop[6];
btVector3DoubleData m_jointAxisBottom[6];
-
- char *m_linkName;
- char *m_jointName;
- btCollisionObjectDoubleData *m_linkCollider;
-
btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal)
double m_linkMass;
int m_parentIndex;
int m_jointType;
-
-
-
int m_dofCount;
int m_posVarCount;
double m_jointPos[7];
double m_jointVel[6];
double m_jointTorque[6];
-
-
-
+
+ double m_jointDamping;
+ double m_jointFriction;
+
+ char *m_linkName;
+ char *m_jointName;
+ btCollisionObjectDoubleData *m_linkCollider;
+ char *m_paddingPtr;
+
};
struct btMultiBodyLinkFloatData
@@ -703,12 +742,6 @@ struct btMultiBodyLinkFloatData
btVector3FloatData m_thisPivotToThisComOffset;
btVector3FloatData m_jointAxisTop[6];
btVector3FloatData m_jointAxisBottom[6];
-
-
- char *m_linkName;
- char *m_jointName;
- btCollisionObjectFloatData *m_linkCollider;
-
btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal)
int m_dofCount;
float m_linkMass;
@@ -721,25 +754,29 @@ struct btMultiBodyLinkFloatData
float m_jointVel[6];
float m_jointTorque[6];
int m_posVarCount;
-
+ float m_jointDamping;
+ float m_jointFriction;
+
+ char *m_linkName;
+ char *m_jointName;
+ btCollisionObjectFloatData *m_linkCollider;
+ char *m_paddingPtr;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btMultiBodyDoubleData
{
- char *m_baseName;
- btMultiBodyLinkDoubleData *m_links;
- btCollisionObjectDoubleData *m_baseCollider;
-
btTransformDoubleData m_baseWorldTransform;
btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal)
-
- int m_numLinks;
double m_baseMass;
- char m_padding[4];
-
+ char *m_baseName;
+ btMultiBodyLinkDoubleData *m_links;
+ btCollisionObjectDoubleData *m_baseCollider;
+ char *m_paddingPtr;
+ int m_numLinks;
+ char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
index 12997d2..01a5989 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
@@ -53,323 +53,359 @@ void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScala
}
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
- btMultiBodyJacobianData& data,
- btScalar* jacOrgA, btScalar* jacOrgB,
- const btVector3& contactNormalOnB,
- const btVector3& posAworld, const btVector3& posBworld,
- btScalar posError,
- const btContactSolverInfo& infoGlobal,
- btScalar lowerLimit, btScalar upperLimit,
- btScalar relaxation,
- bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+ btMultiBodyJacobianData& data,
+ btScalar* jacOrgA, btScalar* jacOrgB,
+ const btVector3& constraintNormalAng,
+ const btVector3& constraintNormalLin,
+ const btVector3& posAworld, const btVector3& posBworld,
+ btScalar posError,
+ const btContactSolverInfo& infoGlobal,
+ btScalar lowerLimit, btScalar upperLimit,
+ bool angConstraint,
+ btScalar relaxation,
+ bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
-
-
- solverConstraint.m_multiBodyA = m_bodyA;
- solverConstraint.m_multiBodyB = m_bodyB;
- solverConstraint.m_linkA = m_linkA;
- solverConstraint.m_linkB = m_linkB;
-
- btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
- btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
-
- btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
- btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
-
- btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
- btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
-
- btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
- if (bodyA)
- rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
- if (bodyB)
- rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
-
- if (multiBodyA)
- {
- if (solverConstraint.m_linkA<0)
- {
- rel_pos1 = posAworld - multiBodyA->getBasePos();
- } else
- {
- rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
- }
-
- const int ndofA = multiBodyA->getNumDofs() + 6;
-
- solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
-
- if (solverConstraint.m_deltaVelAindex <0)
- {
- solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
- multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
- data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
- } else
- {
- btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
- }
-
- //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
- //resize..
- solverConstraint.m_jacAindex = data.m_jacobians.size();
- data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
- //copy/determine
- if(jacOrgA)
- {
- for (int i=0;i<ndofA;i++)
- data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
- }
- else
- {
- btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
- multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
- }
-
- //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
- //resize..
- data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
- btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
- btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
- //determine..
- multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-
- btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
- solverConstraint.m_relpos1CrossNormal = torqueAxis0;
- solverConstraint.m_contactNormal1 = contactNormalOnB;
- }
- else //if(rb0)
- {
- btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
- solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
- solverConstraint.m_relpos1CrossNormal = torqueAxis0;
- solverConstraint.m_contactNormal1 = contactNormalOnB;
- }
-
- if (multiBodyB)
- {
- if (solverConstraint.m_linkB<0)
- {
- rel_pos2 = posBworld - multiBodyB->getBasePos();
- } else
- {
- rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
- }
-
- const int ndofB = multiBodyB->getNumDofs() + 6;
-
- solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
- if (solverConstraint.m_deltaVelBindex <0)
- {
- solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
- multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
- data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
- }
-
- //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
- //resize..
- solverConstraint.m_jacBindex = data.m_jacobians.size();
- data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
- //copy/determine..
- if(jacOrgB)
- {
- for (int i=0;i<ndofB;i++)
- data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
- }
- else
- {
- multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
- }
-
- //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
- //resize..
- data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
- btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
- btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
- //determine..
- multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
-
- btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
- solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
- solverConstraint.m_contactNormal2 = -contactNormalOnB;
-
- }
- else //if(rb1)
- {
- btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
- solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
- solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
- solverConstraint.m_contactNormal2 = -contactNormalOnB;
- }
- {
-
- btVector3 vec;
- btScalar denom0 = 0.f;
- btScalar denom1 = 0.f;
- btScalar* jacB = 0;
- btScalar* jacA = 0;
- btScalar* deltaVelA = 0;
- btScalar* deltaVelB = 0;
- int ndofA = 0;
- //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
- if (multiBodyA)
- {
- ndofA = multiBodyA->getNumDofs() + 6;
- jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
- deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
- for (int i = 0; i < ndofA; ++i)
- {
- btScalar j = jacA[i] ;
- btScalar l = deltaVelA[i];
- denom0 += j*l;
- }
- }
- else if(rb0)
- {
- vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
- denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
- }
- //
- if (multiBodyB)
- {
- const int ndofB = multiBodyB->getNumDofs() + 6;
- jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
- deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
- for (int i = 0; i < ndofB; ++i)
- {
- btScalar j = jacB[i] ;
- btScalar l = deltaVelB[i];
- denom1 += j*l;
- }
-
- }
- else if(rb1)
- {
- vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
- denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
- }
-
- //
- btScalar d = denom0+denom1;
- if (d>SIMD_EPSILON)
- {
- solverConstraint.m_jacDiagABInv = relaxation/(d);
- }
- else
- {
- //disable the constraint row to handle singularity/redundant constraint
- solverConstraint.m_jacDiagABInv = 0.f;
- }
- }
-
-
- //compute rhs and remaining solverConstraint fields
- btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
-
- btScalar rel_vel = 0.f;
- int ndofA = 0;
- int ndofB = 0;
- {
- btVector3 vel1,vel2;
- if (multiBodyA)
- {
- ndofA = multiBodyA->getNumDofs() + 6;
- btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
- for (int i = 0; i < ndofA ; ++i)
- rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
- }
- else if(rb0)
- {
- rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
- }
- if (multiBodyB)
- {
- ndofB = multiBodyB->getNumDofs() + 6;
- btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
- for (int i = 0; i < ndofB ; ++i)
- rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
- }
- else if(rb1)
- {
- rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
- }
-
- solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
- }
-
-
- ///warm starting (or zero if disabled)
- /*
- if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
- {
- solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-
- if (solverConstraint.m_appliedImpulse)
- {
- if (multiBodyA)
- {
- btScalar impulse = solverConstraint.m_appliedImpulse;
- btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
- multiBodyA->applyDeltaVee(deltaV,impulse);
- applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
- } else
- {
- if (rb0)
+ solverConstraint.m_multiBodyA = m_bodyA;
+ solverConstraint.m_multiBodyB = m_bodyB;
+ solverConstraint.m_linkA = m_linkA;
+ solverConstraint.m_linkB = m_linkB;
+
+ btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+ btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+
+ btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
+ btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
+
+ btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
+ btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
+
+ btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
+ if (bodyA)
+ rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
+ if (bodyB)
+ rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
+
+ if (multiBodyA)
+ {
+ if (solverConstraint.m_linkA<0)
+ {
+ rel_pos1 = posAworld - multiBodyA->getBasePos();
+ } else
+ {
+ rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
+ }
+
+ const int ndofA = multiBodyA->getNumDofs() + 6;
+
+ solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
+
+ if (solverConstraint.m_deltaVelAindex <0)
+ {
+ solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
+ multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
+ data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
+ } else
+ {
+ btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
+ }
+
+ //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
+ //resize..
+ solverConstraint.m_jacAindex = data.m_jacobians.size();
+ data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
+ //copy/determine
+ if(jacOrgA)
+ {
+ for (int i=0;i<ndofA;i++)
+ data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
+ }
+ else
+ {
+ btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
+ //multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+ multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+ }
+
+ //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+ //resize..
+ data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
+ btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+ btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+ //determine..
+ multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+
+ btVector3 torqueAxis0;
+ if (angConstraint) {
+ torqueAxis0 = constraintNormalAng;
+ }
+ else {
+ torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+
+ }
+ solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+ solverConstraint.m_contactNormal1 = constraintNormalLin;
+ }
+ else //if(rb0)
+ {
+ btVector3 torqueAxis0;
+ if (angConstraint) {
+ torqueAxis0 = constraintNormalAng;
+ }
+ else {
+ torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+ }
+ solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+ solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+ solverConstraint.m_contactNormal1 = constraintNormalLin;
+ }
+
+ if (multiBodyB)
+ {
+ if (solverConstraint.m_linkB<0)
+ {
+ rel_pos2 = posBworld - multiBodyB->getBasePos();
+ } else
+ {
+ rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
+ }
+
+ const int ndofB = multiBodyB->getNumDofs() + 6;
+
+ solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
+ if (solverConstraint.m_deltaVelBindex <0)
+ {
+ solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
+ multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
+ data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
+ }
+
+ //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
+ //resize..
+ solverConstraint.m_jacBindex = data.m_jacobians.size();
+ data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+ //copy/determine..
+ if(jacOrgB)
+ {
+ for (int i=0;i<ndofB;i++)
+ data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
+ }
+ else
+ {
+ //multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+ multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+ }
+
+ //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+ //resize..
+ data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+ btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+ btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+ //determine..
+ multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+
+ btVector3 torqueAxis1;
+ if (angConstraint) {
+ torqueAxis1 = constraintNormalAng;
+ }
+ else {
+ torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+ }
+ solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+ solverConstraint.m_contactNormal2 = -constraintNormalLin;
+ }
+ else //if(rb1)
+ {
+ btVector3 torqueAxis1;
+ if (angConstraint) {
+ torqueAxis1 = constraintNormalAng;
+ }
+ else {
+ torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+ }
+ solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+ solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+ solverConstraint.m_contactNormal2 = -constraintNormalLin;
+ }
+ {
+
+ btVector3 vec;
+ btScalar denom0 = 0.f;
+ btScalar denom1 = 0.f;
+ btScalar* jacB = 0;
+ btScalar* jacA = 0;
+ btScalar* deltaVelA = 0;
+ btScalar* deltaVelB = 0;
+ int ndofA = 0;
+ //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
+ if (multiBodyA)
+ {
+ ndofA = multiBodyA->getNumDofs() + 6;
+ jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+ deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+ for (int i = 0; i < ndofA; ++i)
+ {
+ btScalar j = jacA[i] ;
+ btScalar l = deltaVelA[i];
+ denom0 += j*l;
+ }
+ }
+ else if(rb0)
+ {
+ vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+ if (angConstraint) {
+ denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec);
+ }
+ else {
+ denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
+ }
+ }
+ //
+ if (multiBodyB)
+ {
+ const int ndofB = multiBodyB->getNumDofs() + 6;
+ jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+ deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+ for (int i = 0; i < ndofB; ++i)
+ {
+ btScalar j = jacB[i] ;
+ btScalar l = deltaVelB[i];
+ denom1 += j*l;
+ }
+
+ }
+ else if(rb1)
+ {
+ vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+ if (angConstraint) {
+ denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec);
+ }
+ else {
+ denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
+ }
+ }
+
+ //
+ btScalar d = denom0+denom1;
+ if (d>SIMD_EPSILON)
+ {
+ solverConstraint.m_jacDiagABInv = relaxation/(d);
+ }
+ else
+ {
+ //disable the constraint row to handle singularity/redundant constraint
+ solverConstraint.m_jacDiagABInv = 0.f;
+ }
+ }
+
+
+ //compute rhs and remaining solverConstraint fields
+ btScalar penetration = isFriction? 0 : posError;
+
+ btScalar rel_vel = 0.f;
+ int ndofA = 0;
+ int ndofB = 0;
+ {
+ btVector3 vel1,vel2;
+ if (multiBodyA)
+ {
+ ndofA = multiBodyA->getNumDofs() + 6;
+ btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+ for (int i = 0; i < ndofA ; ++i)
+ rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+ }
+ else if(rb0)
+ {
+ rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+ }
+ if (multiBodyB)
+ {
+ ndofB = multiBodyB->getNumDofs() + 6;
+ btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+ for (int i = 0; i < ndofB ; ++i)
+ rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+
+ }
+ else if(rb1)
+ {
+ rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+ }
+
+ solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
+ }
+
+
+ ///warm starting (or zero if disabled)
+ /*
+ if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+ {
+ solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+
+ if (solverConstraint.m_appliedImpulse)
+ {
+ if (multiBodyA)
+ {
+ btScalar impulse = solverConstraint.m_appliedImpulse;
+ btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+ multiBodyA->applyDeltaVee(deltaV,impulse);
+ applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
+ } else
+ {
+ if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
- }
- if (multiBodyB)
- {
- btScalar impulse = solverConstraint.m_appliedImpulse;
- btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
- multiBodyB->applyDeltaVee(deltaV,impulse);
- applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
- } else
- {
- if (rb1)
+ }
+ if (multiBodyB)
+ {
+ btScalar impulse = solverConstraint.m_appliedImpulse;
+ btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+ multiBodyB->applyDeltaVee(deltaV,impulse);
+ applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
+ } else
+ {
+ if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
- }
- }
- } else
- */
-
- solverConstraint.m_appliedImpulse = 0.f;
- solverConstraint.m_appliedPushImpulse = 0.f;
-
- {
-
- btScalar positionalError = 0.f;
- btScalar velocityError = desiredVelocity - rel_vel;// * damping;
-
-
- btScalar erp = infoGlobal.m_erp2;
- if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
- {
- erp = infoGlobal.m_erp;
- }
-
- positionalError = -penetration * erp/infoGlobal.m_timeStep;
-
- btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
- btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
-
- if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
- {
- //combine position and velocity into rhs
- solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
- solverConstraint.m_rhsPenetration = 0.f;
-
- } else
- {
- //split position and velocity into rhs and m_rhsPenetration
- solverConstraint.m_rhs = velocityImpulse;
- solverConstraint.m_rhsPenetration = penetrationImpulse;
- }
-
- solverConstraint.m_cfm = 0.f;
- solverConstraint.m_lowerLimit = lowerLimit;
- solverConstraint.m_upperLimit = upperLimit;
- }
-
- return rel_vel;
-
+ }
+ }
+ } else
+ */
+
+ solverConstraint.m_appliedImpulse = 0.f;
+ solverConstraint.m_appliedPushImpulse = 0.f;
+
+ {
+
+ btScalar positionalError = 0.f;
+ btScalar velocityError = desiredVelocity - rel_vel;// * damping;
+
+
+ btScalar erp = infoGlobal.m_erp2;
+ if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+ {
+ erp = infoGlobal.m_erp;
+ }
+
+ positionalError = -penetration * erp/infoGlobal.m_timeStep;
+
+ btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+ btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+
+ if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+ {
+ //combine position and velocity into rhs
+ solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+ solverConstraint.m_rhsPenetration = 0.f;
+
+ } else
+ {
+ //split position and velocity into rhs and m_rhsPenetration
+ solverConstraint.m_rhs = velocityImpulse;
+ solverConstraint.m_rhsPenetration = penetrationImpulse;
+ }
+
+ solverConstraint.m_cfm = 0.f;
+ solverConstraint.m_lowerLimit = lowerLimit;
+ solverConstraint.m_upperLimit = upperLimit;
+ }
+
+ return rel_vel;
+
}
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
index 3b32b46..74c6f5a 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
@@ -66,15 +66,19 @@ protected:
btAlignedObjectArray<btScalar> m_data;
void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
-
+
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
- btScalar* jacOrgA, btScalar* jacOrgB,
- const btVector3& contactNormalOnB,
+ btScalar* jacOrgA, btScalar* jacOrgB,
+ const btVector3& constraintNormalAng,
+
+ const btVector3& constraintNormalLin,
const btVector3& posAworld, const btVector3& posBworld,
btScalar posError,
const btContactSolverInfo& infoGlobal,
- btScalar lowerLimit, btScalar upperLimit,
+ btScalar lowerLimit, btScalar upperLimit,
+ bool angConstraint = false,
+
btScalar relaxation = 1.f,
bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
@@ -114,6 +118,7 @@ public:
btAssert(dof>=0);
btAssert(dof < getNumRows());
m_data[dof] = appliedImpulse;
+
}
btScalar getAppliedImpulse(int dof)
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index 8a034b3..29f8e46 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -31,9 +31,12 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
//solve featherstone non-contact constraints
//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
+
for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
{
- btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
+ int index = iteration&1? j : m_multiBodyNonContactConstraints.size()-1-j;
+
+ btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
resolveSingleConstraintRowGeneric(constraint);
if(constraint.m_multiBodyA)
@@ -221,9 +224,38 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
if (bodyB)
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
- relaxation = 1.f;
-
+ relaxation = infoGlobal.m_sor;
+ btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
+
+ //cfm = 1 / ( dt * kp + kd )
+ //erp = dt * kp / ( dt * kp + kd )
+
+ btScalar cfm = infoGlobal.m_globalCfm;
+ btScalar erp = infoGlobal.m_erp2;
+
+ if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
+ {
+ if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
+ cfm = cp.m_contactCFM;
+ if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
+ erp = cp.m_contactERP;
+ } else
+ {
+ if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
+ {
+ btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
+ if (denom < SIMD_EPSILON)
+ {
+ denom = SIMD_EPSILON;
+ }
+ cfm = btScalar(1) / denom;
+ erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
+ }
+ }
+
+ cfm *= invTimeStep;
+
if (multiBodyA)
@@ -366,7 +398,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
- btScalar d = denom0+denom1;
+ btScalar d = denom0+denom1+cfm;
if (d>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
@@ -402,7 +434,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
{
if (rb0)
{
- rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+ rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) +
+ (rb0->getTotalTorque()*rb0->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos1)+
+ rb0->getTotalForce()*rb0->getInvMass()*infoGlobal.m_timeStep).dot(solverConstraint.m_contactNormal1);
}
}
if (multiBodyB)
@@ -416,7 +450,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
{
if (rb1)
{
- rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+ rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2)+
+ (rb1->getTotalTorque()*rb1->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos2) +
+ rb1->getTotalForce()*rb1->getInvMass()*infoGlobal.m_timeStep).dot(solverConstraint.m_contactNormal2);
}
}
@@ -477,12 +513,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
- btScalar erp = infoGlobal.m_erp2;
- if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
- {
- erp = infoGlobal.m_erp;
- }
-
if (penetration>0)
{
positionalError = 0;
@@ -522,13 +552,283 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
solverConstraint.m_upperLimit = solverConstraint.m_friction;
}
- solverConstraint.m_cfm = 0.f; //why not use cfmSlip?
- }
+ solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
+
-}
+ }
+}
+void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
+ const btVector3& constraintNormal,
+ btManifoldPoint& cp,
+ btScalar combinedTorsionalFriction,
+ const btContactSolverInfo& infoGlobal,
+ btScalar& relaxation,
+ bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+{
+
+ BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
+ btVector3 rel_pos1;
+ btVector3 rel_pos2;
+
+ btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+ btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+
+ const btVector3& pos1 = cp.getPositionWorldOnA();
+ const btVector3& pos2 = cp.getPositionWorldOnB();
+
+ btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
+ btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
+
+ btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
+ btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
+
+ if (bodyA)
+ rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
+ if (bodyB)
+ rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
+
+ relaxation = infoGlobal.m_sor;
+
+ btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
+
+
+ if (multiBodyA)
+ {
+ if (solverConstraint.m_linkA<0)
+ {
+ rel_pos1 = pos1 - multiBodyA->getBasePos();
+ } else
+ {
+ rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
+ }
+ const int ndofA = multiBodyA->getNumDofs() + 6;
+
+ solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
+
+ if (solverConstraint.m_deltaVelAindex <0)
+ {
+ solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
+ multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
+ m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
+ } else
+ {
+ btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
+ }
+
+ solverConstraint.m_jacAindex = m_data.m_jacobians.size();
+ m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
+ m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
+ btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
+
+ btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
+ multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0,0,0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+ btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+ multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
+
+ btVector3 torqueAxis0 = constraintNormal;
+ solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+ solverConstraint.m_contactNormal1 = btVector3(0,0,0);
+ } else
+ {
+ btVector3 torqueAxis0 = constraintNormal;
+ solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+ solverConstraint.m_contactNormal1 = btVector3(0,0,0);
+ solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+ }
+
+
+
+ if (multiBodyB)
+ {
+ if (solverConstraint.m_linkB<0)
+ {
+ rel_pos2 = pos2 - multiBodyB->getBasePos();
+ } else
+ {
+ rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
+ }
+
+ const int ndofB = multiBodyB->getNumDofs() + 6;
+
+ solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
+ if (solverConstraint.m_deltaVelBindex <0)
+ {
+ solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
+ multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
+ m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
+ }
+
+ solverConstraint.m_jacBindex = m_data.m_jacobians.size();
+
+ m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
+ m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+ btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
+
+ multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0,0,0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+ multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
+
+ btVector3 torqueAxis1 = constraintNormal;
+ solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+ solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
+
+ } else
+ {
+ btVector3 torqueAxis1 = constraintNormal;
+ solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+ solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
+
+ solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+ }
+
+ {
+
+ btVector3 vec;
+ btScalar denom0 = 0.f;
+ btScalar denom1 = 0.f;
+ btScalar* jacB = 0;
+ btScalar* jacA = 0;
+ btScalar* lambdaA =0;
+ btScalar* lambdaB =0;
+ int ndofA = 0;
+ if (multiBodyA)
+ {
+ ndofA = multiBodyA->getNumDofs() + 6;
+ jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
+ lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+ for (int i = 0; i < ndofA; ++i)
+ {
+ btScalar j = jacA[i] ;
+ btScalar l =lambdaA[i];
+ denom0 += j*l;
+ }
+ } else
+ {
+ if (rb0)
+ {
+ vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+ denom0 = rb0->getInvMass() + constraintNormal.dot(vec);
+ }
+ }
+ if (multiBodyB)
+ {
+ const int ndofB = multiBodyB->getNumDofs() + 6;
+ jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
+ lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+ for (int i = 0; i < ndofB; ++i)
+ {
+ btScalar j = jacB[i] ;
+ btScalar l =lambdaB[i];
+ denom1 += j*l;
+ }
+
+ } else
+ {
+ if (rb1)
+ {
+ vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+ denom1 = rb1->getInvMass() + constraintNormal.dot(vec);
+ }
+ }
+
+
+
+ btScalar d = denom0+denom1+infoGlobal.m_globalCfm;
+ if (d>SIMD_EPSILON)
+ {
+ solverConstraint.m_jacDiagABInv = relaxation/(d);
+ } else
+ {
+ //disable the constraint row to handle singularity/redundant constraint
+ solverConstraint.m_jacDiagABInv = 0.f;
+ }
+
+ }
+
+
+ //compute rhs and remaining solverConstraint fields
+
+
+
+ btScalar restitution = 0.f;
+ btScalar penetration = isFriction? 0 : cp.getDistance();
+
+ btScalar rel_vel = 0.f;
+ int ndofA = 0;
+ int ndofB = 0;
+ {
+
+ btVector3 vel1,vel2;
+ if (multiBodyA)
+ {
+ ndofA = multiBodyA->getNumDofs() + 6;
+ btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
+ for (int i = 0; i < ndofA ; ++i)
+ rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+ } else
+ {
+ if (rb0)
+ {
+ rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+ }
+ }
+ if (multiBodyB)
+ {
+ ndofB = multiBodyB->getNumDofs() + 6;
+ btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
+ for (int i = 0; i < ndofB ; ++i)
+ rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+
+ } else
+ {
+ if (rb1)
+ {
+ rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+ }
+ }
+
+ solverConstraint.m_friction =combinedTorsionalFriction;
+
+ if(!isFriction)
+ {
+ restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
+ if (restitution <= btScalar(0.))
+ {
+ restitution = 0.f;
+ }
+ }
+ }
+
+
+ solverConstraint.m_appliedImpulse = 0.f;
+ solverConstraint.m_appliedPushImpulse = 0.f;
+
+ {
+
+ btScalar positionalError = 0.f;
+ btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
+
+ if (penetration>0)
+ {
+ velocityError -= penetration / infoGlobal.m_timeStep;
+ }
+
+ btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
+
+ solverConstraint.m_rhs = velocityImpulse;
+ solverConstraint.m_rhsPenetration = 0.f;
+ solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+ solverConstraint.m_upperLimit = solverConstraint.m_friction;
+
+ solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv;
+
+
+
+ }
+
+}
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
@@ -565,6 +865,43 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
return solverConstraint;
}
+btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,
+ btScalar combinedTorsionalFriction,
+ btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+{
+ BT_PROFILE("addMultiBodyRollingFrictionConstraint");
+ btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
+ solverConstraint.m_orgConstraint = 0;
+ solverConstraint.m_orgDofIndex = -1;
+
+ solverConstraint.m_frictionIndex = frictionIndex;
+ bool isFriction = true;
+
+ const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
+ const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
+
+ btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
+ btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
+
+ int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
+ int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
+
+ solverConstraint.m_solverBodyIdA = solverBodyIdA;
+ solverConstraint.m_solverBodyIdB = solverBodyIdB;
+ solverConstraint.m_multiBodyA = mbA;
+ if (mbA)
+ solverConstraint.m_linkA = fcA->m_link;
+
+ solverConstraint.m_multiBodyB = mbB;
+ if (mbB)
+ solverConstraint.m_linkB = fcB->m_link;
+
+ solverConstraint.m_originalContactPoint = &cp;
+
+ setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction,infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
+ return solverConstraint;
+}
+
void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
{
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
@@ -589,8 +926,9 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
// return;
-
-
+ //only a single rollingFriction per manifold
+ int rollingFriction=1;
+
for (int j=0;j<manifold->getNumContacts();j++)
{
@@ -631,49 +969,11 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
#define ENABLE_FRICTION
#ifdef ENABLE_FRICTION
solverConstraint.m_frictionIndex = frictionIndex;
-#if ROLLING_FRICTION
- int rollingFriction=1;
- btVector3 angVelA(0,0,0),angVelB(0,0,0);
- if (rb0)
- angVelA = rb0->getAngularVelocity();
- if (rb1)
- angVelB = rb1->getAngularVelocity();
- btVector3 relAngVel = angVelB-angVelA;
-
- if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
- {
- //only a single rollingFriction per manifold
- rollingFriction--;
- if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
- {
- relAngVel.normalize();
- applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
- applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
- if (relAngVel.length()>0.001)
- addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
- } else
- {
- addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
- btVector3 axis0,axis1;
- btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
- applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
- applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
- applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
- applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
- if (axis0.length()>0.001)
- addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
- if (axis1.length()>0.001)
- addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
- }
- }
-#endif //ROLLING_FRICTION
///Bullet has several options to set the friction directions
- ///By default, each contact has only a single friction direction that is recomputed automatically very frame
+ ///By default, each contact has only a single friction direction that is recomputed automatically every frame
///based on the relative linear velocity.
- ///If the relative velocity it zero, it will automatically compute a friction direction.
+ ///If the relative velocity is zero, it will automatically compute a friction direction.
///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
@@ -685,7 +985,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
- if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+ if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
{/*
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
@@ -714,6 +1014,15 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
+
+ if (rollingFriction > 0)
+ {
+ addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
+ addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
+ addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
+
+ rollingFriction--;
+ }
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
@@ -724,16 +1033,16 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
{
- cp.m_lateralFrictionInitialized = true;
+ cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
}
}
} else
{
- addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
+ addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_frictionCFM);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
- addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
+ addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_frictionCFM);
//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
//todo:
@@ -809,7 +1118,7 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS
}
#endif
-#include "Bullet3Common/b3Logging.h"
+
void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
{
#if 1
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
index 321ee42..3a89c63 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
@@ -47,8 +47,12 @@ protected:
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
- btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+
+ btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+ btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,
+ btScalar combinedTorsionalFriction,
+ btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
btScalar* jacA,btScalar* jacB,
@@ -60,6 +64,15 @@ protected:
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+
+ //either rolling or spinning friction
+ void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
+ const btVector3& contactNormal,
+ btManifoldPoint& cp,
+ btScalar combinedTorsionalFriction,
+ const btContactSolverInfo& infoGlobal,
+ btScalar& relaxation,
+ bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index a9c0b33..d94d1d4 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -396,22 +396,17 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
void btMultiBodyDynamicsWorld::forwardKinematics()
{
- btAlignedObjectArray<btQuaternion> world_to_local;
- btAlignedObjectArray<btVector3> local_origin;
for (int b=0;b<m_multiBodies.size();b++)
{
btMultiBody* bod = m_multiBodies[b];
- bod->forwardKinematics(world_to_local,local_origin);
+ bod->forwardKinematics(m_scratch_world_to_local,m_scratch_local_origin);
}
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
forwardKinematics();
- btAlignedObjectArray<btScalar> scratch_r;
- btAlignedObjectArray<btVector3> scratch_v;
- btAlignedObjectArray<btMatrix3x3> scratch_m;
BT_PROFILE("solveConstraints");
@@ -463,9 +458,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
- scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
- scratch_v.resize(bod->getNumLinks()+1);
- scratch_m.resize(bod->getNumLinks()+1);
+ m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
+ m_scratch_v.resize(bod->getNumLinks()+1);
+ m_scratch_m.resize(bod->getNumLinks()+1);
bod->addBaseForce(m_gravity * bod->getBaseMass());
@@ -500,15 +495,15 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
- scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
- scratch_v.resize(bod->getNumLinks()+1);
- scratch_m.resize(bod->getNumLinks()+1);
+ m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
+ m_scratch_v.resize(bod->getNumLinks()+1);
+ m_scratch_m.resize(bod->getNumLinks()+1);
bool doNotUpdatePos = false;
{
if(!bod->isUsingRK4Integration())
{
- bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m);
}
else
{
@@ -594,9 +589,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
btScalar h = solverInfo.m_timeStep;
- #define output &scratch_r[bod->getNumDofs()]
+ #define output &m_scratch_r[bod->getNumDofs()]
//calc qdd0 from: q0 & qd0
- bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd0, 0, numDofs);
//calc q1 = q0 + h/2 * qd0
pResetQx();
@@ -606,7 +601,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd1 from: q1 & qd1
pCopyToVelocityVector(bod, scratch_qd1);
- bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd1, 0, numDofs);
//calc q2 = q0 + h/2 * qd1
pResetQx();
@@ -616,7 +611,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd2 from: q2 & qd2
pCopyToVelocityVector(bod, scratch_qd2);
- bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd2, 0, numDofs);
//calc q3 = q0 + h * qd2
pResetQx();
@@ -626,7 +621,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd3 from: q3 & qd3
pCopyToVelocityVector(bod, scratch_qd3);
- bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd3, 0, numDofs);
//
@@ -661,7 +656,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
for(int link = 0; link < bod->getNumLinks(); ++link)
bod->getLink(link).updateCacheMultiDof();
- bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, scratch_r, scratch_v, scratch_m);
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m);
}
}
@@ -701,16 +696,16 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
- scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
- scratch_v.resize(bod->getNumLinks()+1);
- scratch_m.resize(bod->getNumLinks()+1);
+ m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
+ m_scratch_v.resize(bod->getNumLinks()+1);
+ m_scratch_m.resize(bod->getNumLinks()+1);
{
if(!bod->isUsingRK4Integration())
{
bool isConstraintPass = true;
- bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
+ bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass);
}
}
}
@@ -732,9 +727,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("btMultiBody stepPositions");
//integrate and update the Featherstone hierarchies
- btAlignedObjectArray<btQuaternion> world_to_local;
- btAlignedObjectArray<btVector3> local_origin;
-
+
for (int b=0;b<m_multiBodies.size();b++)
{
btMultiBody* bod = m_multiBodies[b];
@@ -770,10 +763,10 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
- world_to_local.resize(nLinks+1);
- local_origin.resize(nLinks+1);
+ m_scratch_world_to_local.resize(nLinks+1);
+ m_scratch_local_origin.resize(nLinks+1);
- bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
+ bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local,m_scratch_local_origin);
} else
{
@@ -818,8 +811,6 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("btMultiBody debugDrawWorld");
- btAlignedObjectArray<btQuaternion> world_to_local1;
- btAlignedObjectArray<btVector3> local_origin1;
for (int c=0;c<m_multiBodyConstraints.size();c++)
{
@@ -830,7 +821,7 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
for (int b = 0; b<m_multiBodies.size(); b++)
{
btMultiBody* bod = m_multiBodies[b];
- bod->forwardKinematics(world_to_local1,local_origin1);
+ bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1);
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
index 03ef333..2c912da 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
@@ -36,6 +36,16 @@ protected:
btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
+ //cached data to avoid memory allocations
+ btAlignedObjectArray<btQuaternion> m_scratch_world_to_local;
+ btAlignedObjectArray<btVector3> m_scratch_local_origin;
+ btAlignedObjectArray<btQuaternion> m_scratch_world_to_local1;
+ btAlignedObjectArray<btVector3> m_scratch_local_origin1;
+ btAlignedObjectArray<btScalar> m_scratch_r;
+ btAlignedObjectArray<btVector3> m_scratch_v;
+ btAlignedObjectArray<btMatrix3x3> m_scratch_m;
+
+
virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo& solverInfo);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
new file mode 100644
index 0000000..0f0d9f6
--- /dev/null
+++ b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
@@ -0,0 +1,211 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyFixedConstraint.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define BTMBFIXEDCONSTRAINT_DIM 6
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+ :btMultiBodyConstraint(body,0,link,-1,BTMBFIXEDCONSTRAINT_DIM,false),
+ m_rigidBodyA(0),
+ m_rigidBodyB(bodyB),
+ m_pivotInA(pivotInA),
+ m_pivotInB(pivotInB),
+ m_frameInA(frameInA),
+ m_frameInB(frameInB)
+{
+ m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses
+}
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+ :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBFIXEDCONSTRAINT_DIM,false),
+ m_rigidBodyA(0),
+ m_rigidBodyB(0),
+ m_pivotInA(pivotInA),
+ m_pivotInB(pivotInB),
+ m_frameInA(frameInA),
+ m_frameInB(frameInB)
+{
+ m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses
+}
+
+void btMultiBodyFixedConstraint::finalizeMultiDof()
+{
+ //not implemented yet
+ btAssert(0);
+}
+
+btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
+{
+}
+
+
+int btMultiBodyFixedConstraint::getIslandIdA() const
+{
+ if (m_rigidBodyA)
+ return m_rigidBodyA->getIslandTag();
+
+ if (m_bodyA)
+ {
+ btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+ if (col)
+ return col->getIslandTag();
+ for (int i=0;i<m_bodyA->getNumLinks();i++)
+ {
+ if (m_bodyA->getLink(i).m_collider)
+ return m_bodyA->getLink(i).m_collider->getIslandTag();
+ }
+ }
+ return -1;
+}
+
+int btMultiBodyFixedConstraint::getIslandIdB() const
+{
+ if (m_rigidBodyB)
+ return m_rigidBodyB->getIslandTag();
+ if (m_bodyB)
+ {
+ btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+ if (col)
+ return col->getIslandTag();
+
+ for (int i=0;i<m_bodyB->getNumLinks();i++)
+ {
+ col = m_bodyB->getLink(i).m_collider;
+ if (col)
+ return col->getIslandTag();
+ }
+ }
+ return -1;
+}
+
+void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
+{
+ int numDim = BTMBFIXEDCONSTRAINT_DIM;
+ for (int i=0;i<numDim;i++)
+ {
+ btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+ constraintRow.m_orgConstraint = this;
+ constraintRow.m_orgDofIndex = i;
+ constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
+ constraintRow.m_contactNormal1.setValue(0,0,0);
+ constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
+ constraintRow.m_contactNormal2.setValue(0,0,0);
+ constraintRow.m_angularComponentA.setValue(0,0,0);
+ constraintRow.m_angularComponentB.setValue(0,0,0);
+
+ constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
+ constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
+
+ // Convert local points back to world
+ btVector3 pivotAworld = m_pivotInA;
+ btMatrix3x3 frameAworld = m_frameInA;
+ if (m_rigidBodyA)
+ {
+
+ constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
+ pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
+ frameAworld = frameAworld.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation());
+
+ } else
+ {
+ if (m_bodyA) {
+ pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+ frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+ }
+ }
+ btVector3 pivotBworld = m_pivotInB;
+ btMatrix3x3 frameBworld = m_frameInB;
+ if (m_rigidBodyB)
+ {
+ constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+ pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
+ frameBworld = frameBworld.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation());
+
+ } else
+ {
+ if (m_bodyB) {
+ pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+ frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
+ }
+ }
+
+ btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
+ btVector3 angleDiff;
+ btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff);
+
+ btVector3 constraintNormalLin(0,0,0);
+ btVector3 constraintNormalAng(0,0,0);
+ btScalar posError = 0.0;
+ if (i < 3) {
+ constraintNormalLin[i] = -1;
+ posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
+ fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+ constraintNormalLin, pivotAworld, pivotBworld,
+ posError,
+ infoGlobal,
+ -m_maxAppliedImpulse, m_maxAppliedImpulse
+ );
+ }
+ else { //i>=3
+ constraintNormalAng = frameAworld.getColumn(i%3);
+ posError = angleDiff[i%3];
+ fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+ constraintNormalLin, pivotAworld, pivotBworld,
+ posError,
+ infoGlobal,
+ -m_maxAppliedImpulse, m_maxAppliedImpulse, true
+ );
+ }
+ }
+}
+
+void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
+{
+ btTransform tr;
+ tr.setIdentity();
+
+ if (m_rigidBodyA)
+ {
+ btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+ tr.setOrigin(pivot);
+ drawer->drawTransform(tr, 0.1);
+ }
+ if (m_bodyA)
+ {
+ btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+ tr.setOrigin(pivotAworld);
+ drawer->drawTransform(tr, 0.1);
+ }
+ if (m_rigidBodyB)
+ {
+ // that ideally should draw the same frame
+ btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+ tr.setOrigin(pivot);
+ drawer->drawTransform(tr, 0.1);
+ }
+ if (m_bodyB)
+ {
+ btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+ tr.setOrigin(pivotBworld);
+ drawer->drawTransform(tr, 0.1);
+ }
+}
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
new file mode 100644
index 0000000..26e28a7
--- /dev/null
+++ b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
@@ -0,0 +1,94 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H
+#define BT_MULTIBODY_FIXED_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodyFixedConstraint : public btMultiBodyConstraint
+{
+protected:
+
+ btRigidBody* m_rigidBodyA;
+ btRigidBody* m_rigidBodyB;
+ btVector3 m_pivotInA;
+ btVector3 m_pivotInB;
+ btMatrix3x3 m_frameInA;
+ btMatrix3x3 m_frameInB;
+
+public:
+
+ btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+ btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+
+ virtual ~btMultiBodyFixedConstraint();
+
+ virtual void finalizeMultiDof();
+
+ virtual int getIslandIdA() const;
+ virtual int getIslandIdB() const;
+
+ virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+ btMultiBodyJacobianData& data,
+ const btContactSolverInfo& infoGlobal);
+
+ const btVector3& getPivotInA() const
+ {
+ return m_pivotInA;
+ }
+
+ void setPivotInA(const btVector3& pivotInA)
+ {
+ m_pivotInA = pivotInA;
+ }
+
+ const btVector3& getPivotInB() const
+ {
+ return m_pivotInB;
+ }
+
+ void setPivotInB(const btVector3& pivotInB)
+ {
+ m_pivotInB = pivotInB;
+ }
+
+ const btMatrix3x3& getFrameInA() const
+ {
+ return m_frameInA;
+ }
+
+ void setFrameInA(const btMatrix3x3& frameInA)
+ {
+ m_frameInA = frameInA;
+ }
+
+ const btMatrix3x3& getFrameInB() const
+ {
+ return m_frameInB;
+ }
+
+ void setFrameInB(const btMatrix3x3& frameInB)
+ {
+ m_frameInB = frameInB;
+ }
+
+ virtual void debugDraw(class btIDebugDraw* drawer);
+
+};
+
+#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
index 3f05aa4..7078176 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
@@ -122,7 +122,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
const btScalar posError = 0; //why assume it's zero?
const btVector3 dummy(0, 0, 0);
- btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
+ btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
{
//expect either prismatic or revolute joint type for now
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
index 5026861..573c496 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
@@ -23,7 +23,12 @@ subject to the following restrictions:
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
- m_desiredVelocity(desiredVelocity)
+ m_desiredVelocity(desiredVelocity),
+ m_desiredPosition(0),
+ m_kd(1.),
+ m_kp(0),
+ m_erp(1),
+ m_rhsClamp(SIMD_INFINITY)
{
m_maxAppliedImpulse = maxMotorImpulse;
@@ -51,7 +56,12 @@ void btMultiBodyJointMotor::finalizeMultiDof()
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
- m_desiredVelocity(desiredVelocity)
+ m_desiredVelocity(desiredVelocity),
+ m_desiredPosition(0),
+ m_kd(1.),
+ m_kp(0),
+ m_erp(1),
+ m_rhsClamp(SIMD_INFINITY)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
@@ -114,8 +124,24 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-
- fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
+ int dof = 0;
+ btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+ btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+ btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
+
+ btScalar velocityError = (m_desiredVelocity - currentVelocity);
+ btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
+ if (rhs>m_rhsClamp)
+ {
+ rhs=m_rhsClamp;
+ }
+ if (rhs<-m_rhsClamp)
+ {
+ rhs=-m_rhsClamp;
+ }
+
+
+ fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
index d322f87..7cf5731 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
@@ -25,8 +25,13 @@ class btMultiBodyJointMotor : public btMultiBodyConstraint
{
protected:
-
btScalar m_desiredVelocity;
+ btScalar m_desiredPosition;
+ btScalar m_kd;
+ btScalar m_kp;
+ btScalar m_erp;
+ btScalar m_rhsClamp;//maximum error
+
public:
@@ -42,11 +47,30 @@ public:
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
- virtual void setVelocityTarget(btScalar velTarget)
+ virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
{
m_desiredVelocity = velTarget;
+ m_kd = kd;
}
+ virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
+ {
+ m_desiredPosition = posTarget;
+ m_kp = kp;
+ }
+
+ virtual void setErp(btScalar erp)
+ {
+ m_erp = erp;
+ }
+ virtual btScalar getErp() const
+ {
+ return m_erp;
+ }
+ virtual void setRhsClamp(btScalar rhsClamp)
+ {
+ m_rhsClamp = rhsClamp;
+ }
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
index 909e80e..8b90363 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -132,8 +132,12 @@ btVector3 m_appliedConstraintForce; // In WORLD frame
const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
+ const void* m_userPtr;//m_userPtr ptr needs to be managed by the developer/user!
+
+ btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
+ btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
- // ctor: set some sensible defaults
+ // ctor: set some sensible defaults
btMultibodyLink()
: m_mass(1),
m_parent(-1),
@@ -146,7 +150,10 @@ btVector3 m_appliedConstraintForce; // In WORLD frame
m_jointType(btMultibodyLink::eInvalid),
m_jointFeedback(0),
m_linkName(0),
- m_jointName(0)
+ m_jointName(0),
+ m_userPtr(0),
+ m_jointDamping(0),
+ m_jointFriction(0)
{
m_inertiaLocal.setValue(1, 1, 1);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
index 2ccb982..3e28f80 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
@@ -159,7 +159,7 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
- fillMultiBodyConstraint(constraintRow, data, 0, 0,
+ fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0,0,0),
contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
posError,
infoGlobal,
diff --git a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
new file mode 100644
index 0000000..67b106f
--- /dev/null
+++ b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
@@ -0,0 +1,230 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySliderConstraint.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define BTMBSLIDERCONSTRAINT_DIM 5
+#define EPSILON 0.000001
+
+btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
+ :btMultiBodyConstraint(body,0,link,-1,BTMBSLIDERCONSTRAINT_DIM,false),
+ m_rigidBodyA(0),
+ m_rigidBodyB(bodyB),
+ m_pivotInA(pivotInA),
+ m_pivotInB(pivotInB),
+ m_frameInA(frameInA),
+ m_frameInB(frameInB),
+ m_jointAxis(jointAxis)
+{
+ m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses
+}
+
+btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
+ :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBSLIDERCONSTRAINT_DIM,false),
+ m_rigidBodyA(0),
+ m_rigidBodyB(0),
+ m_pivotInA(pivotInA),
+ m_pivotInB(pivotInB),
+ m_frameInA(frameInA),
+ m_frameInB(frameInB),
+ m_jointAxis(jointAxis)
+{
+ m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses
+}
+
+void btMultiBodySliderConstraint::finalizeMultiDof()
+{
+ //not implemented yet
+ btAssert(0);
+}
+
+btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
+{
+}
+
+
+int btMultiBodySliderConstraint::getIslandIdA() const
+{
+ if (m_rigidBodyA)
+ return m_rigidBodyA->getIslandTag();
+
+ if (m_bodyA)
+ {
+ btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+ if (col)
+ return col->getIslandTag();
+ for (int i=0;i<m_bodyA->getNumLinks();i++)
+ {
+ if (m_bodyA->getLink(i).m_collider)
+ return m_bodyA->getLink(i).m_collider->getIslandTag();
+ }
+ }
+ return -1;
+}
+
+int btMultiBodySliderConstraint::getIslandIdB() const
+{
+ if (m_rigidBodyB)
+ return m_rigidBodyB->getIslandTag();
+ if (m_bodyB)
+ {
+ btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+ if (col)
+ return col->getIslandTag();
+
+ for (int i=0;i<m_bodyB->getNumLinks();i++)
+ {
+ col = m_bodyB->getLink(i).m_collider;
+ if (col)
+ return col->getIslandTag();
+ }
+ }
+ return -1;
+}
+
+void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
+{
+ // Convert local points back to world
+ btVector3 pivotAworld = m_pivotInA;
+ btMatrix3x3 frameAworld = m_frameInA;
+ btVector3 jointAxis = m_jointAxis;
+ if (m_rigidBodyA)
+ {
+ pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
+ frameAworld = m_frameInA.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation());
+ jointAxis = quatRotate(m_rigidBodyA->getOrientation(),m_jointAxis);
+
+ } else if (m_bodyA) {
+ pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+ frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
+ jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
+ }
+ btVector3 pivotBworld = m_pivotInB;
+ btMatrix3x3 frameBworld = m_frameInB;
+ if (m_rigidBodyB)
+ {
+ pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
+ frameBworld = m_frameInB.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation());
+
+ } else if (m_bodyB) {
+ pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+ frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
+ }
+
+ btVector3 constraintAxis[2];
+ for (int i = 0; i < 3; ++i)
+ {
+ constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
+ if (constraintAxis[0].safeNorm() > EPSILON)
+ {
+ constraintAxis[0] = constraintAxis[0].normalized();
+ constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
+ constraintAxis[1] = constraintAxis[1].normalized();
+ break;
+ }
+ }
+
+ btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
+ btVector3 angleDiff;
+ btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff);
+
+ int numDim = BTMBSLIDERCONSTRAINT_DIM;
+ for (int i=0;i<numDim;i++)
+ {
+ btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+ constraintRow.m_orgConstraint = this;
+ constraintRow.m_orgDofIndex = i;
+ constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
+ constraintRow.m_contactNormal1.setValue(0,0,0);
+ constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
+ constraintRow.m_contactNormal2.setValue(0,0,0);
+ constraintRow.m_angularComponentA.setValue(0,0,0);
+ constraintRow.m_angularComponentB.setValue(0,0,0);
+
+ constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
+ constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
+
+ if (m_rigidBodyA)
+ {
+ constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
+ }
+ if (m_rigidBodyB)
+ {
+ constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+ }
+
+ btVector3 constraintNormalLin(0,0,0);
+ btVector3 constraintNormalAng(0,0,0);
+ btScalar posError = 0.0;
+ if (i < 2) {
+ constraintNormalLin = constraintAxis[i];
+ posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
+ fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+ constraintNormalLin, pivotAworld, pivotBworld,
+ posError,
+ infoGlobal,
+ -m_maxAppliedImpulse, m_maxAppliedImpulse
+ );
+ }
+ else { //i>=2
+ constraintNormalAng = frameAworld.getColumn(i%3);
+ posError = angleDiff[i%3];
+ fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+ constraintNormalLin, pivotAworld, pivotBworld,
+ posError,
+ infoGlobal,
+ -m_maxAppliedImpulse, m_maxAppliedImpulse, true
+ );
+ }
+ }
+}
+
+void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
+{
+ btTransform tr;
+ tr.setIdentity();
+
+ if (m_rigidBodyA)
+ {
+ btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+ tr.setOrigin(pivot);
+ drawer->drawTransform(tr, 0.1);
+ }
+ if (m_bodyA)
+ {
+ btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+ tr.setOrigin(pivotAworld);
+ drawer->drawTransform(tr, 0.1);
+ }
+ if (m_rigidBodyB)
+ {
+ // that ideally should draw the same frame
+ btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+ tr.setOrigin(pivot);
+ drawer->drawTransform(tr, 0.1);
+ }
+ if (m_bodyB)
+ {
+ btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+ tr.setOrigin(pivotBworld);
+ drawer->drawTransform(tr, 0.1);
+ }
+}
diff --git a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h
new file mode 100644
index 0000000..571dcd5
--- /dev/null
+++ b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h
@@ -0,0 +1,105 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H
+#define BT_MULTIBODY_SLIDER_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodySliderConstraint : public btMultiBodyConstraint
+{
+protected:
+
+ btRigidBody* m_rigidBodyA;
+ btRigidBody* m_rigidBodyB;
+ btVector3 m_pivotInA;
+ btVector3 m_pivotInB;
+ btMatrix3x3 m_frameInA;
+ btMatrix3x3 m_frameInB;
+ btVector3 m_jointAxis;
+
+public:
+
+ btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
+ btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
+
+ virtual ~btMultiBodySliderConstraint();
+
+ virtual void finalizeMultiDof();
+
+ virtual int getIslandIdA() const;
+ virtual int getIslandIdB() const;
+
+ virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+ btMultiBodyJacobianData& data,
+ const btContactSolverInfo& infoGlobal);
+
+ const btVector3& getPivotInA() const
+ {
+ return m_pivotInA;
+ }
+
+ void setPivotInA(const btVector3& pivotInA)
+ {
+ m_pivotInA = pivotInA;
+ }
+
+ const btVector3& getPivotInB() const
+ {
+ return m_pivotInB;
+ }
+
+ void setPivotInB(const btVector3& pivotInB)
+ {
+ m_pivotInB = pivotInB;
+ }
+
+ const btMatrix3x3& getFrameInA() const
+ {
+ return m_frameInA;
+ }
+
+ void setFrameInA(const btMatrix3x3& frameInA)
+ {
+ m_frameInA = frameInA;
+ }
+
+ const btMatrix3x3& getFrameInB() const
+ {
+ return m_frameInB;
+ }
+
+ void setFrameInB(const btMatrix3x3& frameInB)
+ {
+ m_frameInB = frameInB;
+ }
+
+ const btVector3& getJointAxis() const
+ {
+ return m_jointAxis;
+ }
+
+ void setJointAxis(const btVector3& jointAxis)
+ {
+ m_jointAxis = jointAxis;
+ }
+
+ virtual void debugDraw(class btIDebugDraw* drawer);
+
+};
+
+#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H
diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
index 6688694..e73f4ac 100644
--- a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
+++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
@@ -22,8 +22,7 @@ subject to the following restrictions:
btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver)
:m_solver(solver),
-m_fallback(0),
-m_cfm(0.000001)//0.0000001
+m_fallback(0)
{
}
@@ -436,7 +435,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
- m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
+ m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm/ infoGlobal.m_timeStep);
}
}
@@ -564,7 +563,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
- m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
+ m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
}
}
diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
index 43e8544..88d587c 100644
--- a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
+++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
@@ -42,7 +42,6 @@ protected:
btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
btMLCPSolverInterface* m_solver;
int m_fallback;
- btScalar m_cfm;
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
@@ -73,15 +72,6 @@ public:
m_fallback = num;
}
- btScalar getCfm() const
- {
- return m_cfm;
- }
- void setCfm(btScalar cfm)
- {
- m_cfm = cfm;
- }
-
virtual btConstraintSolverType getSolverType() const
{
return BT_MLCP_SOLVER;
diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp
index dadbd82..c34f989 100644
--- a/src/BulletInverseDynamics/IDConfig.hpp
+++ b/src/BulletInverseDynamics/IDConfig.hpp
@@ -2,8 +2,17 @@
/// such as choice of linear algebra library and underlying scalar type
#ifndef IDCONFIG_HPP_
#define IDCONFIG_HPP_
+
+// If true, enable jacobian calculations.
+// This adds a 3xN matrix to every body, + 2 3-Vectors.
+// so it is not advised for large systems if it is not absolutely necessary.
+// Also, this is not required for standard inverse dynamics calculations.
+// Will only work with vector math libraries that support 3xN matrices.
+#define BT_ID_WITH_JACOBIANS
+
// If we have a custom configuration, compile without using other parts of bullet.
#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
+#include <cmath>
#define BT_ID_WO_BULLET
#define BT_ID_POW(a,b) std::pow(a,b)
#define BT_ID_SNPRINTF snprintf
@@ -22,10 +31,14 @@
#include "IDErrorMessages.hpp"
#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
+/*
+#include "IDConfigEigen.hpp"
+#include "IDConfigBuiltin.hpp"
+*/
#define INVDYN_INCLUDE_HELPER_2(x) #x
#define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x)
#include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H)
-#ifndef btInverseDynamics
+#ifndef btInverseDynamics
#error "custom inverse dynamics config, but no custom namespace defined"
#endif
diff --git a/src/BulletInverseDynamics/IDConfigEigen.hpp b/src/BulletInverseDynamics/IDConfigEigen.hpp
index a83ee8e..cbd7e8a 100644
--- a/src/BulletInverseDynamics/IDConfigEigen.hpp
+++ b/src/BulletInverseDynamics/IDConfigEigen.hpp
@@ -21,20 +21,7 @@ struct idArray {
typedef std::vector<int>::size_type idArrayIdx;
// default to standard malloc/free
#include <cstdlib>
-#define idMalloc ::malloc
-
-#define idFree ::free
-// currently not aligned at all...
-#define ID_DECLARE_ALIGNED_ALLOCATOR() \
- inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
- inline void operator delete(void* ptr) { idFree(ptr); } \
- inline void* operator new(std::size_t, void* ptr) { return ptr; } \
- inline void operator delete(void*, void*) {} \
- inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
- inline void operator delete[](void* ptr) { idFree(ptr); } \
- inline void* operator new[](std::size_t, void* ptr) { return ptr; } \
- inline void operator delete[](void*, void*) {}
-
+#define ID_DECLARE_ALIGNED_ALLOCATOR() EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// Note on interfaces:
// Eigen::Matrix has data(), to get c-array storage
// HOWEVER: default storage is column-major!
diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp
index 7ce055f..03452ca 100644
--- a/src/BulletInverseDynamics/IDMath.cpp
+++ b/src/BulletInverseDynamics/IDMath.cpp
@@ -55,6 +55,60 @@ idScalar maxAbs(const vec3 &v) {
return result;
}
+#if (defined BT_ID_HAVE_MAT3X)
+idScalar maxAbsMat3x(const mat3x &m) {
+ // only used for tests -- so just loop here for portability
+ idScalar result = 0.0;
+ for (idArrayIdx col = 0; col < m.cols(); col++) {
+ for (idArrayIdx row = 0; row < 3; row++) {
+ result = BT_ID_MAX(result, std::fabs(m(row, col)));
+ }
+ }
+ return result;
+}
+
+void mul(const mat33 &a, const mat3x &b, mat3x *result) {
+ if (b.cols() != result->cols()) {
+ error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
+ static_cast<int>(b.cols()), static_cast<int>(result->cols()));
+ abort();
+ }
+
+ for (idArrayIdx col = 0; col < b.cols(); col++) {
+ const idScalar x = a(0,0)*b(0,col)+a(0,1)*b(1,col)+a(0,2)*b(2,col);
+ const idScalar y = a(1,0)*b(0,col)+a(1,1)*b(1,col)+a(1,2)*b(2,col);
+ const idScalar z = a(2,0)*b(0,col)+a(2,1)*b(1,col)+a(2,2)*b(2,col);
+ setMat3xElem(0, col, x, result);
+ setMat3xElem(1, col, y, result);
+ setMat3xElem(2, col, z, result);
+ }
+}
+void add(const mat3x &a, const mat3x &b, mat3x *result) {
+ if (a.cols() != b.cols()) {
+ error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
+ static_cast<int>(a.cols()), static_cast<int>(b.cols()));
+ abort();
+ }
+ for (idArrayIdx col = 0; col < b.cols(); col++) {
+ for (idArrayIdx row = 0; row < 3; row++) {
+ setMat3xElem(row, col, a(row, col) + b(row, col), result);
+ }
+ }
+}
+void sub(const mat3x &a, const mat3x &b, mat3x *result) {
+ if (a.cols() != b.cols()) {
+ error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
+ static_cast<int>(a.cols()), static_cast<int>(b.cols()));
+ abort();
+ }
+ for (idArrayIdx col = 0; col < b.cols(); col++) {
+ for (idArrayIdx row = 0; row < 3; row++) {
+ setMat3xElem(row, col, a(row, col) - b(row, col), result);
+ }
+ }
+}
+#endif
+
mat33 transformX(const idScalar &alpha) {
mat33 T;
const idScalar cos_alpha = std::cos(alpha);
diff --git a/src/BulletInverseDynamics/IDMath.hpp b/src/BulletInverseDynamics/IDMath.hpp
index f7526b5..6369971 100644
--- a/src/BulletInverseDynamics/IDMath.hpp
+++ b/src/BulletInverseDynamics/IDMath.hpp
@@ -19,6 +19,16 @@ idScalar maxAbs(const vecx& v);
/// return maximum absolute value
idScalar maxAbs(const vec3& v);
#endif //ID_LINEAR_MATH_USE_EIGEN
+
+#if (defined BT_ID_HAVE_MAT3X)
+idScalar maxAbsMat3x(const mat3x& m);
+void setZero(mat3x&m);
+// define math functions on mat3x here to avoid allocations in operators.
+void mul(const mat33&a, const mat3x&b, mat3x* result);
+void add(const mat3x&a, const mat3x&b, mat3x* result);
+void sub(const mat3x&a, const mat3x&b, mat3x* result);
+#endif
+
/// get offset vector & transform matrix from DH parameters
/// TODO: add documentation
void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3* r, mat33* T);
diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp
index f5488b6..4235f13 100644
--- a/src/BulletInverseDynamics/MultiBodyTree.cpp
+++ b/src/BulletInverseDynamics/MultiBodyTree.cpp
@@ -61,6 +61,18 @@ int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_a
return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
}
+int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3* r) const {
+ return m_impl->getParentRParentBodyRef(body_index, r);
+}
+
+int MultiBodyTree::getBodyTParentRef(const int body_index, mat33* T) const {
+ return m_impl->getBodyTParentRef(body_index, T);
+}
+
+int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3* axis) const {
+ return m_impl->getBodyAxisOfMotion(body_index, axis);
+}
+
void MultiBodyTree::printTree() { m_impl->printTree(); }
void MultiBodyTree::printTreeData() { m_impl->printTreeData(); }
@@ -96,9 +108,106 @@ int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinemati
}
return 0;
}
+
int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) {
return calculateMassMatrix(q, true, true, true, mass_matrix);
}
+
+
+
+int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u) {
+ vec3 world_gravity(m_impl->m_world_gravity);
+ // temporarily set gravity to zero, to ensure we get the actual accelerations
+ setZero(m_impl->m_world_gravity);
+
+ if (false == m_is_finalized) {
+ error_message("system has not been initialized\n");
+ return -1;
+ }
+ if (-1 == m_impl->calculateKinematics(q, u, dot_u,
+ MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) {
+ error_message("error in kinematics calculation\n");
+ return -1;
+ }
+
+ m_impl->m_world_gravity=world_gravity;
+ return 0;
+}
+
+
+int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
+ if (false == m_is_finalized) {
+ error_message("system has not been initialized\n");
+ return -1;
+ }
+ if (-1 == m_impl->calculateKinematics(q, q, q,
+ MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
+ error_message("error in kinematics calculation\n");
+ return -1;
+ }
+ return 0;
+}
+
+int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) {
+ if (false == m_is_finalized) {
+ error_message("system has not been initialized\n");
+ return -1;
+ }
+ if (-1 == m_impl->calculateKinematics(q, u, u,
+ MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
+ error_message("error in kinematics calculation\n");
+ return -1;
+ }
+ return 0;
+}
+
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) {
+ if (false == m_is_finalized) {
+ error_message("system has not been initialized\n");
+ return -1;
+ }
+ if (-1 == m_impl->calculateJacobians(q, u,
+ MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
+ error_message("error in jacobian calculation\n");
+ return -1;
+ }
+ return 0;
+}
+
+int MultiBodyTree::calculateJacobians(const vecx& q){
+ if (false == m_is_finalized) {
+ error_message("system has not been initialized\n");
+ return -1;
+ }
+ if (-1 == m_impl->calculateJacobians(q, q,
+ MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) {
+ error_message("error in jacobian calculation\n");
+ return -1;
+ }
+ return 0;
+}
+
+int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const {
+ return m_impl->getBodyDotJacobianTransU(body_index,world_dot_jac_trans_u);
+}
+
+int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const {
+ return m_impl->getBodyDotJacobianRotU(body_index,world_dot_jac_rot_u);
+}
+
+int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const {
+ return m_impl->getBodyJacobianTrans(body_index,world_jac_trans);
+}
+
+int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const {
+ return m_impl->getBodyJacobianRot(body_index,world_jac_rot);
+}
+
+
+#endif
+
int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type,
const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
const vec3 &body_axis_of_motion_, idScalar mass,
@@ -186,6 +295,11 @@ int MultiBodyTree::finalize() {
const int &num_bodies = m_init_cache->numBodies();
const int &num_dofs = m_init_cache->numDoFs();
+ if(num_dofs<=0) {
+ error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
+ //return -1;
+ }
+
// 1 allocate internal MultiBody structure
m_impl = new MultiBodyImpl(num_bodies, num_dofs);
diff --git a/src/BulletInverseDynamics/MultiBodyTree.hpp b/src/BulletInverseDynamics/MultiBodyTree.hpp
index d33e60e..d235aa6 100644
--- a/src/BulletInverseDynamics/MultiBodyTree.hpp
+++ b/src/BulletInverseDynamics/MultiBodyTree.hpp
@@ -51,6 +51,7 @@ enum JointType {
/// - gears and motor inertia
class MultiBodyTree {
public:
+ ID_DECLARE_ALIGNED_ALLOCATOR();
/// The contructor.
/// Initialization & allocation is via addBody and buildSystem calls.
MultiBodyTree();
@@ -117,7 +118,10 @@ public:
void printTree();
/// print tree data to stdout
void printTreeData();
- /// calculate joint forces for given generalized state & derivatives
+ /// Calculate joint forces for given generalized state & derivatives.
+ /// This also updates kinematic terms computed in calculateKinematics.
+ /// If gravity is not set to zero, acceleration terms will contain
+ /// gravitational acceleration.
/// @param q generalized coordinates
/// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u)
/// @param dot_u time derivative of u
@@ -148,6 +152,31 @@ public:
/// @return -1 on error, 0 on success
int calculateMassMatrix(const vecx& q, matxx* mass_matrix);
+
+ /// Calculates kinematics also calculated in calculateInverseDynamics,
+ /// but not dynamics.
+ /// This function ensures that correct accelerations are computed that do not
+ /// contain gravitational acceleration terms.
+ /// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations)
+ int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u);
+ /// Calculate position kinematics
+ int calculatePositionKinematics(const vecx& q);
+ /// Calculate position and velocity kinematics
+ int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u);
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+ /// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components
+ /// d(Jacobian)/dt*u
+ /// This function assumes that calculateInverseDynamics was called, or calculateKinematics,
+ /// or calculatePositionAndVelocityKinematics
+ int calculateJacobians(const vecx& q, const vecx& u);
+ /// Calculate Jacobians (dvel/du)
+ /// This function assumes that calculateInverseDynamics was called, or
+ /// one of the calculateKineamtics functions
+ int calculateJacobians(const vecx& q);
+#endif // BT_ID_HAVE_MAT3X
+
+
/// set gravitational acceleration
/// the default is [0;0;-9.8] in the world frame
/// @param gravity the gravitational acceleration in world frame
@@ -200,9 +229,20 @@ public:
/// @param world_origin pointer for return data
/// @return 0 on success, -1 on error
int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+ // get translational jacobian, in world frame (dworld_velocity/du)
+ int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
+ // get rotational jacobian, in world frame (dworld_omega/du)
+ int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
+ // get product of translational jacobian derivative * generatlized velocities
+ int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
+ // get product of rotational jacobian derivative * generatlized velocities
+ int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
+#endif // BT_ID_HAVE_MAT3X
+
/// returns the (internal) index of body
- /// @param body_index is the index of a body (internal: TODO: fix/clarify
- /// indexing!)
+ /// @param body_index is the index of a body
/// @param parent_index pointer to where parent index will be stored
/// @return 0 on success, -1 on error
int getParentIndex(const int body_index, int* parent_index) const;
@@ -216,6 +256,21 @@ public:
/// @param joint_type string naming the corresponding joint type
/// @return 0 on success, -1 on failure
int getJointTypeStr(const int body_index, const char** joint_type) const;
+ /// get offset translation to parent body (see addBody)
+ /// @param body_index index of the body
+ /// @param r the offset translation (see above)
+ /// @return 0 on success, -1 on failure
+ int getParentRParentBodyRef(const int body_index, vec3* r) const;
+ /// get offset rotation to parent body (see addBody)
+ /// @param body_index index of the body
+ /// @param T the transform (see above)
+ /// @return 0 on success, -1 on failure
+ int getBodyTParentRef(const int body_index, mat33* T) const;
+ /// get axis of motion (see addBody)
+ /// @param body_index index of the body
+ /// @param axis the axis (see above)
+ /// @return 0 on success, -1 on failure
+ int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
/// get offset for degrees of freedom of this body into the q-vector
/// @param body_index index of the body
/// @param q_offset offset the q vector
diff --git a/src/BulletInverseDynamics/details/IDEigenInterface.hpp b/src/BulletInverseDynamics/details/IDEigenInterface.hpp
index ec6136a..836395c 100644
--- a/src/BulletInverseDynamics/details/IDEigenInterface.hpp
+++ b/src/BulletInverseDynamics/details/IDEigenInterface.hpp
@@ -2,16 +2,35 @@
#define INVDYNEIGENINTERFACE_HPP_
#include "../IDConfig.hpp"
namespace btInverseDynamics {
+
+#define BT_ID_HAVE_MAT3X
+
#ifdef BT_USE_DOUBLE_PRECISION
-typedef Eigen::VectorXd vecx;
-typedef Eigen::Vector3d vec3;
-typedef Eigen::Matrix3d mat33;
-typedef Eigen::MatrixXd matxx;
+typedef Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::DontAlign> vecx;
+typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> vec3;
+typedef Eigen::Matrix<double, 3, 3, Eigen::DontAlign> mat33;
+typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx;
+typedef Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x;
#else
-typedef Eigen::VectorXf vecx;
-typedef Eigen::Vector3f vec3;
-typedef Eigen::Matrix3f mat33;
-typedef Eigen::MatrixXf matxx;
-#endif //
+typedef Eigen::Matrix<float, Eigen::Dynamic, 1, Eigen::DontAlign> vecx;
+typedef Eigen::Matrix<float, 3, 1, Eigen::DontAlign> vec3;
+typedef Eigen::Matrix<float, 3, 3, Eigen::DontAlign> mat33;
+typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx;
+typedef Eigen::Matrix<float, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x;
+#endif
+
+inline void resize(mat3x &m, Eigen::Index size) {
+ m.resize(3, size);
+ m.setZero();
+}
+
+inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
+ (*m)(row, col) = val;
+}
+
+inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
+ (*m)(row, col) = val;
+}
+
}
#endif // INVDYNEIGENINTERFACE_HPP_
diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
index c77ba3c..5bb4a33 100644
--- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
+++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
@@ -8,6 +8,7 @@
#include "../../LinearMath/btMatrix3x3.h"
#include "../../LinearMath/btVector3.h"
#include "../../LinearMath/btMatrixX.h"
+#define BT_ID_HAVE_MAT3X
namespace btInverseDynamics {
class vec3;
@@ -21,7 +22,7 @@ public:
vec3(const btVector3& btv) { *this = btv; }
idScalar& operator()(int i) { return (*this)[i]; }
const idScalar& operator()(int i) const { return (*this)[i]; }
- const int size() const { return 3; }
+ int size() const { return 3; }
const vec3& operator=(const btVector3& rhs) {
*static_cast<btVector3*>(this) = rhs;
return *this;
@@ -107,6 +108,65 @@ inline vecx operator/(const vecx& a, const idScalar& s) {
return result;
}
+
+// use btMatrixX to implement 3xX matrix
+class mat3x : public matxx {
+public:
+ mat3x(){}
+ mat3x(const mat3x&rhs) {
+ matxx::resize(rhs.rows(), rhs.cols());
+ *this = rhs;
+ }
+ mat3x(int rows, int cols): matxx(3,cols) {
+ }
+ void operator=(const mat3x& rhs) {
+ if (m_cols != rhs.m_cols) {
+ error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
+ abort();
+ }
+ for(int i=0;i<rows();i++) {
+ for(int k=0;k<cols();k++) {
+ setElem(i,k,rhs(i,k));
+ }
+ }
+ }
+ void setZero() {
+ matxx::setZero();
+ }
+};
+
+
+inline vec3 operator*(const mat3x& a, const vecx& b) {
+ vec3 result;
+ if (a.cols() != b.size()) {
+ error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
+ abort();
+ }
+ result(0)=0.0;
+ result(1)=0.0;
+ result(2)=0.0;
+ for(int i=0;i<b.size();i++) {
+ for(int k=0;k<3;k++) {
+ result(k)+=a(k,i)*b(i);
+ }
+ }
+ return result;
+}
+
+
+inline void resize(mat3x &m, idArrayIdx size) {
+ m.resize(3, size);
+ m.setZero();
+}
+
+inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
+ m->setElem(row, col, val);
+}
+
+inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
+ m->setElem(row, col, val);
+}
+
}
#endif // IDLINEARMATHINTERFACE_HPP_
diff --git a/src/BulletInverseDynamics/details/IDMatVec.hpp b/src/BulletInverseDynamics/details/IDMatVec.hpp
index de15e9e..4d3f6c8 100644
--- a/src/BulletInverseDynamics/details/IDMatVec.hpp
+++ b/src/BulletInverseDynamics/details/IDMatVec.hpp
@@ -5,12 +5,14 @@
#include <cstdlib>
#include "../IDConfig.hpp"
+#define BT_ID_HAVE_MAT3X
namespace btInverseDynamics {
class vec3;
class vecx;
class mat33;
class matxx;
+class mat3x;
/// This is a very basic implementation to enable stand-alone use of the library.
/// The implementation is not really optimized and misses many features that you would
@@ -85,13 +87,17 @@ private:
class matxx {
public:
+ matxx() {
+ m_data = 0x0;
+ m_cols=0;
+ m_rows=0;
+ }
matxx(int rows, int cols) : m_rows(rows), m_cols(cols) {
m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols));
}
~matxx() { idFree(m_data); }
- const matxx& operator=(const matxx& rhs);
- idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; }
- const idScalar& operator()(int row, int col) const { return m_data[row * m_rows + col]; }
+ idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
+ const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
const int& rows() const { return m_rows; }
const int& cols() const { return m_cols; }
@@ -101,6 +107,61 @@ private:
idScalar* m_data;
};
+class mat3x {
+public:
+ mat3x() {
+ m_data = 0x0;
+ m_cols=0;
+ }
+ mat3x(const mat3x&rhs) {
+ m_cols=rhs.m_cols;
+ allocate();
+ *this = rhs;
+ }
+ mat3x(int rows, int cols): m_cols(cols) {
+ allocate();
+ };
+ void operator=(const mat3x& rhs) {
+ if (m_cols != rhs.m_cols) {
+ error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
+ abort();
+ }
+ for(int i=0;i<3*m_cols;i++) {
+ m_data[i] = rhs.m_data[i];
+ }
+ }
+
+ ~mat3x() {
+ free();
+ }
+ idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
+ const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
+ int rows() const { return m_rows; }
+ const int& cols() const { return m_cols; }
+ void resize(int rows, int cols) {
+ m_cols=cols;
+ free();
+ allocate();
+ }
+ void setZero() {
+ memset(m_data,0x0,sizeof(idScalar)*m_rows*m_cols);
+ }
+ // avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead
+private:
+ void allocate(){m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols));}
+ void free() { idFree(m_data);}
+ enum {m_rows=3};
+ int m_cols;
+ idScalar* m_data;
+};
+
+inline void resize(mat3x &m, idArrayIdx size) {
+ m.resize(3, size);
+ m.setZero();
+}
+
+//////////////////////////////////////////////////
+// Implementations
inline const vec3& vec3::operator=(const vec3& rhs) {
if (&rhs != this) {
memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
@@ -324,5 +385,31 @@ inline vecx operator/(const vecx& a, const idScalar& s) {
return result;
}
+
+inline vec3 operator*(const mat3x& a, const vecx& b) {
+ vec3 result;
+ if (a.cols() != b.size()) {
+ error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
+ abort();
+ }
+ result(0)=0.0;
+ result(1)=0.0;
+ result(2)=0.0;
+ for(int i=0;i<b.size();i++) {
+ for(int k=0;k<3;k++) {
+ result(k)+=a(k,i)*b(i);
+ }
+ }
+ return result;
+}
+
+inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
+ (*m)(row, col) = val;
}
+
+inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
+ (*m)(row, col) = val;
+}
+
+} // namespace btInverseDynamcis
#endif
diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
index 06a3595..847e5c6 100644
--- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
+++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
@@ -3,7 +3,15 @@
namespace btInverseDynamics {
MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
- : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) {
+ : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_)
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+ ,m_m3x(3,m_num_dofs)
+#endif
+{
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+ resize(m_m3x,m_num_dofs);
+#endif
m_body_list.resize(num_bodies_);
m_parent_index.resize(num_bodies_);
m_child_indices.resize(num_bodies_);
@@ -205,6 +213,20 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData() {
// no static data
break;
}
+
+ // resize & initialize jacobians to zero.
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+ body.m_body_dot_Jac_T_u(0) = 0.0;
+ body.m_body_dot_Jac_T_u(1) = 0.0;
+ body.m_body_dot_Jac_T_u(2) = 0.0;
+ body.m_body_dot_Jac_R_u(0) = 0.0;
+ body.m_body_dot_Jac_R_u(1) = 0.0;
+ body.m_body_dot_Jac_R_u(2) = 0.0;
+ resize(body.m_body_Jac_T,m_num_dofs);
+ resize(body.m_body_Jac_R,m_num_dofs);
+ body.m_body_Jac_T.setZero();
+ body.m_body_Jac_R.setZero();
+#endif //
}
}
@@ -218,6 +240,99 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
return -1;
}
+ // 1. relative kinematics
+ if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) {
+ error_message("error in calculateKinematics\n");
+ return -1;
+ }
+ // 2. update contributions to equations of motion for every body.
+ for (idArrayIdx i = 0; i < m_body_list.size(); i++) {
+ RigidBody &body = m_body_list[i];
+ // 3.4 update dynamic terms (rate of change of angular & linear momentum)
+ body.m_eom_lhs_rotational =
+ body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) +
+ body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) -
+ body.m_body_moment_user;
+ body.m_eom_lhs_translational =
+ body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc +
+ body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) -
+ body.m_body_force_user;
+ }
+
+ // 3. calculate full set of forces at parent joint
+ // (not directly calculating the joint force along the free direction
+ // simplifies inclusion of fixed joints.
+ // An alternative would be to fuse bodies in a pre-processing step,
+ // but that would make changing masses online harder (eg, payload masses
+ // added with fixed joints to a gripper)
+ // Also, this enables adding zero weight bodies as a way to calculate frame poses
+ // for force elements, etc.
+
+ for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) {
+ // sum of forces and moments acting on this body from its children
+ vec3 sum_f_children;
+ vec3 sum_m_children;
+ setZero(sum_f_children);
+ setZero(sum_m_children);
+ for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size();
+ child_list_idx++) {
+ const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]];
+ vec3 child_joint_force_in_this_frame =
+ child.m_body_T_parent.transpose() * child.m_force_at_joint;
+ sum_f_children -= child_joint_force_in_this_frame;
+ sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint +
+ child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame);
+ }
+ RigidBody &body = m_body_list[body_idx];
+
+ body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children;
+ body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children;
+ }
+
+ // 4. Calculate Joint forces.
+ // These are the components of force_at_joint/moment_at_joint
+ // in the free directions given by Jac_JT/Jac_JR
+ // 4.1 revolute joints
+ for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
+ RigidBody &body = m_body_list[m_body_revolute_list[i]];
+ // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint;
+ (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint);
+ }
+ // 4.2 for prismatic joints
+ for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
+ RigidBody &body = m_body_list[m_body_prismatic_list[i]];
+ // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint;
+ (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint);
+ }
+ // 4.3 floating bodies (6-DoF joints)
+ for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) {
+ RigidBody &body = m_body_list[m_body_floating_list[i]];
+ (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
+ (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
+ (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
+
+ (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0);
+ (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1);
+ (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2);
+ }
+
+ return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u,
+ const KinUpdateType type) {
+ if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) {
+ error_message("wrong vector dimension. system has %d DOFs,\n"
+ "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
+ m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
+ static_cast<int>(dot_u.size()));
+ return -1;
+ }
+ if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) {
+ error_message("invalid type %d\n", type);
+ return -1;
+ }
+
// 1. update relative kinematics
// 1.1 for revolute
for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
@@ -225,28 +340,25 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
mat33 T;
bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T);
body.m_body_T_parent = T * body.m_body_T_parent_ref;
-
- // body.m_parent_r_parent_body= fixed
- body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
- // body.m_parent_dot_r_rel = fixed;
- // NOTE: this assumes that Jac_JR is constant, which is true for revolute
- // joints, but not in the general case (eg, slider-crank type mechanisms)
- body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
- // body.m_parent_ddot_r_rel = fixed;
+ if(type >= POSITION_VELOCITY) {
+ body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
+ }
+ if(type >= POSITION_VELOCITY_ACCELERATION) {
+ body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
+ }
}
// 1.2 for prismatic
for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
RigidBody &body = m_body_list[m_body_prismatic_list[i]];
- // body.m_body_T_parent= fixed
body.m_parent_pos_parent_body =
body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
- // body.m_parent_omega_rel = 0;
- body.m_parent_vel_rel =
+ if(type >= POSITION_VELOCITY) {
+ body.m_parent_vel_rel =
body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
- // body.parent_dot_omega_rel = 0;
- // NOTE: this assumes that Jac_JT is constant, which is true for
- // prismatic joints, but not in the general case
- body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
+ }
+ if(type >= POSITION_VELOCITY_ACCELERATION) {
+ body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
+ }
}
// 1.3 fixed joints: nothing to do
// 1.4 6dof joints:
@@ -258,29 +370,33 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
+ body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
- body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
- body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
- body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
+ if(type >= POSITION_VELOCITY) {
+ body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
+ body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
+ body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
- body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
- body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
- body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
+ body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
+ body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
+ body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
- body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
- body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
- body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
+ body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
+ }
+ if(type >= POSITION_VELOCITY_ACCELERATION) {
+ body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
+ body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
+ body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
- body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
- body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
- body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
+ body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
+ body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
+ body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
- body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
- body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
- body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
+ body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
+ }
}
- // 3. absolute kinematic quantities & dynamic quantities
+ // 2. absolute kinematic quantities (vector valued)
// NOTE: this should be optimized by specializing for different body types
// (e.g., relative rotation is always zero for prismatic joints, etc.)
@@ -294,127 +410,147 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body;
body.m_body_T_world = body.m_body_T_parent;
- // 3.2 update absolute velocities
- body.m_body_ang_vel = body.m_body_ang_vel_rel;
- body.m_body_vel = body.m_parent_vel_rel;
-
- // 3.3 update absolute accelerations
- // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
- body.m_body_ang_acc = body.m_body_ang_acc_rel;
- body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
- // add gravitational acceleration to root body
- // this is an efficient way to add gravitational terms,
- // but it does mean that the kinematics are no longer
- // correct at the acceleration level
- // NOTE: To get correct acceleration kinematics, just set world_gravity to zero
- body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
+ if(type >= POSITION_VELOCITY) {
+ // 3.2 update absolute velocities
+ body.m_body_ang_vel = body.m_body_ang_vel_rel;
+ body.m_body_vel = body.m_parent_vel_rel;
+ }
+ if(type >= POSITION_VELOCITY_ACCELERATION) {
+ // 3.3 update absolute accelerations
+ // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
+ body.m_body_ang_acc = body.m_body_ang_acc_rel;
+ body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
+ // add gravitational acceleration to root body
+ // this is an efficient way to add gravitational terms,
+ // but it does mean that the kinematics are no longer
+ // correct at the acceleration level
+ // NOTE: To get correct acceleration kinematics, just set world_gravity to zero
+ body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
+ }
}
for (idArrayIdx i = 1; i < m_body_list.size(); i++) {
RigidBody &body = m_body_list[i];
RigidBody &parent = m_body_list[m_parent_index[i]];
- // 3.1 update absolute positions and orientations:
+ // 2.1 update absolute positions and orientations:
// will be required if we add force elements (eg springs between bodies,
// or contacts) not required right now added here for debugging purposes
body.m_body_pos =
body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body);
body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world;
- // 3.2 update absolute velocities
- body.m_body_ang_vel =
+ if(type >= POSITION_VELOCITY) {
+ // 2.2 update absolute velocities
+ body.m_body_ang_vel =
body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel;
- body.m_body_vel =
+ body.m_body_vel =
body.m_body_T_parent *
(parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) +
body.m_parent_vel_rel);
-
- // 3.3 update absolute accelerations
- // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
- body.m_body_ang_acc =
+ }
+ if(type >= POSITION_VELOCITY_ACCELERATION) {
+ // 2.3 update absolute accelerations
+ // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
+ body.m_body_ang_acc =
body.m_body_T_parent * parent.m_body_ang_acc -
body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) +
body.m_body_ang_acc_rel;
- body.m_body_acc =
+ body.m_body_acc =
body.m_body_T_parent *
(parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) +
- parent.m_body_ang_vel.cross(
- parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
+ parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel);
+ }
}
- for (idArrayIdx i = 0; i < m_body_list.size(); i++) {
- RigidBody &body = m_body_list[i];
- // 3.4 update dynamic terms (rate of change of angular & linear momentum)
- body.m_eom_lhs_rotational =
- body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) +
- body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) -
- body.m_body_moment_user;
- body.m_eom_lhs_translational =
- body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc +
- body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) -
- body.m_body_force_user;
- }
-
- // 4. calculate full set of forces at parent joint
- // (not directly calculating the joint force along the free direction
- // simplifies inclusion of fixed joints.
- // An alternative would be to fuse bodies in a pre-processing step,
- // but that would make changing masses online harder (eg, payload masses
- // added with fixed joints to a gripper)
- // Also, this enables adding zero weight bodies as a way to calculate frame poses
- // for force elements, etc.
-
- for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) {
- // sum of forces and moments acting on this body from its children
- vec3 sum_f_children;
- vec3 sum_m_children;
- setZero(sum_f_children);
- setZero(sum_m_children);
- for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size();
- child_list_idx++) {
- const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]];
- vec3 child_joint_force_in_this_frame =
- child.m_body_T_parent.transpose() * child.m_force_at_joint;
- sum_f_children -= child_joint_force_in_this_frame;
- sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint +
- child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame);
- }
- RigidBody &body = m_body_list[body_idx];
-
- body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children;
- body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children;
- }
-
- // 4. Calculate Joint forces.
- // These are the components of force_at_joint/moment_at_joint
- // in the free directions given by Jac_JT/Jac_JR
- // 4.1 revolute joints
- for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
- RigidBody &body = m_body_list[m_body_revolute_list[i]];
- // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint;
- (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint);
- }
- // 4.2 for prismatic joints
- for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
- RigidBody &body = m_body_list[m_body_prismatic_list[i]];
- // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint;
- (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint);
- }
- // 4.3 floating bodies (6-DoF joints)
- for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) {
- RigidBody &body = m_body_list[m_body_floating_list[i]];
- (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
- (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
- (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
+ return 0;
+}
- (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0);
- (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1);
- (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2);
- }
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+
+void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body) {
+ const int& idx=body.m_q_index;
+ switch(body.m_joint_type) {
+ case FIXED:
+ break;
+ case REVOLUTE:
+ setMat3xElem(0,idx, body.m_Jac_JR(0), &body.m_body_Jac_R);
+ setMat3xElem(1,idx, body.m_Jac_JR(1), &body.m_body_Jac_R);
+ setMat3xElem(2,idx, body.m_Jac_JR(2), &body.m_body_Jac_R);
+ break;
+ case PRISMATIC:
+ setMat3xElem(0,idx, body.m_body_T_parent_ref(0,0)*body.m_Jac_JT(0)
+ +body.m_body_T_parent_ref(1,0)*body.m_Jac_JT(1)
+ +body.m_body_T_parent_ref(2,0)*body.m_Jac_JT(2),
+ &body.m_body_Jac_T);
+ setMat3xElem(1,idx,body.m_body_T_parent_ref(0,1)*body.m_Jac_JT(0)
+ +body.m_body_T_parent_ref(1,1)*body.m_Jac_JT(1)
+ +body.m_body_T_parent_ref(2,1)*body.m_Jac_JT(2),
+ &body.m_body_Jac_T);
+ setMat3xElem(2,idx, body.m_body_T_parent_ref(0,2)*body.m_Jac_JT(0)
+ +body.m_body_T_parent_ref(1,2)*body.m_Jac_JT(1)
+ +body.m_body_T_parent_ref(2,2)*body.m_Jac_JT(2),
+ &body.m_body_Jac_T);
+ break;
+ case FLOATING:
+ setMat3xElem(0,idx+0, 1.0, &body.m_body_Jac_R);
+ setMat3xElem(1,idx+1, 1.0, &body.m_body_Jac_R);
+ setMat3xElem(2,idx+2, 1.0, &body.m_body_Jac_R);
+ // body_Jac_T = body_T_parent.transpose();
+ setMat3xElem(0,idx+3, body.m_body_T_parent(0,0), &body.m_body_Jac_T);
+ setMat3xElem(0,idx+4, body.m_body_T_parent(1,0), &body.m_body_Jac_T);
+ setMat3xElem(0,idx+5, body.m_body_T_parent(2,0), &body.m_body_Jac_T);
+
+ setMat3xElem(1,idx+3, body.m_body_T_parent(0,1), &body.m_body_Jac_T);
+ setMat3xElem(1,idx+4, body.m_body_T_parent(1,1), &body.m_body_Jac_T);
+ setMat3xElem(1,idx+5, body.m_body_T_parent(2,1), &body.m_body_Jac_T);
+
+ setMat3xElem(2,idx+3, body.m_body_T_parent(0,2), &body.m_body_Jac_T);
+ setMat3xElem(2,idx+4, body.m_body_T_parent(1,2), &body.m_body_Jac_T);
+ setMat3xElem(2,idx+5, body.m_body_T_parent(2,2), &body.m_body_Jac_T);
+
+ break;
+ }
+}
- return 0;
+int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) {
+ if (q.size() != m_num_dofs || u.size() != m_num_dofs) {
+ error_message("wrong vector dimension. system has %d DOFs,\n"
+ "but dim(q)= %d, dim(u)= %d\n",
+ m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
+ return -1;
+ }
+ if(type != POSITION_ONLY && type != POSITION_VELOCITY) {
+ error_message("invalid type %d\n", type);
+ return -1;
+ }
+
+ addRelativeJacobianComponent(m_body_list[0]);
+ for (idArrayIdx i = 1; i < m_body_list.size(); i++) {
+ RigidBody &body = m_body_list[i];
+ RigidBody &parent = m_body_list[m_parent_index[i]];
+
+ mul(body.m_body_T_parent, parent.m_body_Jac_R,& body.m_body_Jac_R);
+ body.m_body_Jac_T = parent.m_body_Jac_T;
+ mul(tildeOperator(body.m_parent_pos_parent_body),parent.m_body_Jac_R,&m_m3x);
+ sub(body.m_body_Jac_T,m_m3x, &body.m_body_Jac_T);
+
+ addRelativeJacobianComponent(body);
+ mul(body.m_body_T_parent, body.m_body_Jac_T,&body.m_body_Jac_T);
+
+ if(type >= POSITION_VELOCITY) {
+ body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u -
+ body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel);
+ body.m_body_dot_Jac_T_u = body.m_body_T_parent *
+ (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) +
+ parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
+ 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel));
+ }
+ }
+ return 0;
}
+#endif
static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) {
switch (dof) {
@@ -473,6 +609,7 @@ static inline int jointNumDoFs(const JointType &type) {
error_message("invalid joint type\n");
// TODO add configurable abort/crash function
abort();
+ return 0;
}
int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics,
@@ -487,13 +624,6 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
// This implementation, however, handles branched systems and uses a formulation centered
// on the origin of the body-fixed frame to avoid re-computing various quantities at the com.
-// Macro for setting matrix elements depending on underlying math library.
-#ifdef ID_LINEAR_MATH_USE_BULLET
-#define setMassMatrixElem(row, col, val) mass_matrix->setElem(row, col, val)
-#else
-#define setMassMatrixElem(row, col, val) (*mass_matrix)(row, col) = val
-#endif
-
if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
mass_matrix->cols() != m_num_dofs) {
error_message("Dimension error. System has %d DOFs,\n"
@@ -507,7 +637,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
if (initialize_matrix) {
for (int i = 0; i < m_num_dofs; i++) {
for (int j = 0; j < m_num_dofs; j++) {
- setMassMatrixElem(i, j, 0.0);
+ setMatxxElem(i, j, 0.0, mass_matrix);
}
}
}
@@ -597,7 +727,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
vec3 body_eom_trans =
body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR);
- setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans));
+ setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix);
// rest of the mass matrix column upwards
{
@@ -609,7 +739,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
}
setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
- setMassMatrixElem(col, row, Mrc);
+ setMatxxElem(col, row, Mrc, mass_matrix);
}
// 2. ancestor dofs
int child_idx = i;
@@ -634,7 +764,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
}
const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
- setMassMatrixElem(col, row, Mrc);
+ setMatxxElem(col, row, Mrc, mass_matrix);
}
child_idx = parent_idx;
@@ -647,12 +777,10 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
if (set_lower_triangular_matrix) {
for (int col = 0; col < m_num_dofs; col++) {
for (int row = 0; row < col; row++) {
- setMassMatrixElem(row, col, (*mass_matrix)(col, row));
+ setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix);
}
}
}
-
-#undef setMassMatrixElem
return 0;
}
@@ -779,6 +907,32 @@ int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index,
return 0;
}
+int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3* r) const{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ *r=m_body_list[body_index].m_parent_pos_parent_body_ref;
+ return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33* T) const{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ *T=m_body_list[body_index].m_body_T_parent_ref;
+ return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3* axis) const{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ if(m_body_list[body_index].m_joint_type == REVOLUTE) {
+ *axis = m_body_list[body_index].m_Jac_JR;
+ return 0;
+ }
+ if(m_body_list[body_index].m_joint_type == PRISMATIC) {
+ *axis = m_body_list[body_index].m_Jac_JT;
+ return 0;
+ }
+ setZero(*axis);
+ return 0;
+}
+
int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const {
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*q_index = m_body_list[body_index].m_q_index;
@@ -841,4 +995,34 @@ int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3
return 0;
}
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const {
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ const RigidBody &body = m_body_list[body_index];
+ *world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u;
+ return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ const RigidBody &body = m_body_list[body_index];
+ *world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u;
+ return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ const RigidBody &body = m_body_list[body_index];
+ mul(body.m_body_T_world.transpose(), body.m_body_Jac_T,world_jac_trans);
+ return 0;
+}
+
+int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ const RigidBody &body = m_body_list[body_index];
+ mul(body.m_body_T_world.transpose(), body.m_body_Jac_R,world_jac_rot);
+ return 0;
+}
+
+#endif
}
diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
index 7787a46..3efe9d0 100644
--- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
+++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
@@ -110,6 +110,19 @@ struct RigidBody {
vec3 m_body_subtree_mass_com;
/// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame
mat33 m_body_subtree_I_body;
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+ /// translational jacobian in body-fixed frame d(m_body_vel)/du
+ mat3x m_body_Jac_T;
+ /// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du
+ mat3x m_body_Jac_R;
+ /// components of linear acceleration depending on u
+ /// (same as is d(m_Jac_T)/dt*u)
+ vec3 m_body_dot_Jac_T_u;
+ /// components of angular acceleration depending on u
+ /// (same as is d(m_Jac_T)/dt*u)
+ vec3 m_body_dot_Jac_R_u;
+#endif
};
/// The MBS implements a tree structured multibody system
@@ -119,6 +132,12 @@ class MultiBodyTree::MultiBodyImpl {
public:
ID_DECLARE_ALIGNED_ALLOCATOR();
+ enum KinUpdateType {
+ POSITION_ONLY,
+ POSITION_VELOCITY,
+ POSITION_VELOCITY_ACCELERATION
+ };
+
/// constructor
/// @param num_bodies the number of bodies in the system
/// @param num_dofs number of degrees of freedom in the system
@@ -131,6 +150,25 @@ public:
int calculateMassMatrix(const vecx& q, const bool update_kinematics,
const bool initialize_matrix, const bool set_lower_triangular_matrix,
matxx* mass_matrix);
+ /// calculate kinematics (vector quantities)
+ /// Depending on type, update positions only, positions & velocities, or positions, velocities
+ /// and accelerations.
+ int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type);
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+ /// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms.
+ int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type);
+ /// \copydoc MultiBodyTree::getBodyDotJacobianTransU
+ int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const ;
+ /// \copydoc MultiBodyTree::getBodyDotJacobianRotU
+ int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
+ /// \copydoc MultiBodyTree::getBodyJacobianTrans
+ int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const ;
+ /// \copydoc MultiBodyTree::getBodyJacobianRot
+ int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
+ /// Add relative Jacobian component from motion relative to parent body
+ /// @param body the body to add the Jacobian component for
+ void addRelativeJacobianComponent(RigidBody&body);
+#endif
/// generate additional index sets from the parent_index array
/// @return -1 on error, 0 on success
int generateIndexSets();
@@ -152,6 +190,12 @@ public:
int getJointType(const int body_index, JointType* joint_type) const;
/// \copydoc MultiBodyTree::getJointTypeStr
int getJointTypeStr(const int body_index, const char** joint_type) const;
+ /// \copydoc MultiBodyTree::getParentRParentBodyRef
+ int getParentRParentBodyRef(const int body_index, vec3* r) const;
+ /// \copydoc MultiBodyTree::getBodyTParentRef
+ int getBodyTParentRef(const int body_index, mat33* T) const;
+ /// \copydoc MultiBodyTree::getBodyAxisOfMotion
+ int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
/// \copydoc MultiBodyTree:getDoFOffset
int getDoFOffset(const int body_index, int* q_index) const;
/// \copydoc MultiBodyTree::getBodyOrigin
@@ -231,6 +275,9 @@ private:
idArray<int>::type m_user_int;
// a user-provided pointer
idArray<void*>::type m_user_ptr;
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+ mat3x m_m3x;
+#endif
};
}
#endif
diff --git a/src/LinearMath/btAlignedAllocator.cpp b/src/LinearMath/btAlignedAllocator.cpp
index a65296c..e5f6040 100644
--- a/src/LinearMath/btAlignedAllocator.cpp
+++ b/src/LinearMath/btAlignedAllocator.cpp
@@ -105,30 +105,94 @@ void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc)
}
#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
+
+static int allocations_id[10241024];
+static int allocations_bytes[10241024];
+static int mynumallocs = 0;
+#include <stdio.h>
+
+int btDumpMemoryLeaks()
+{
+ int totalLeak = 0;
+
+ for (int i=0;i<mynumallocs;i++)
+ {
+ printf("Error: leaked memory of allocation #%d (%d bytes)\n", allocations_id[i], allocations_bytes[i]);
+ totalLeak+=allocations_bytes[i];
+ }
+ if (totalLeak)
+ {
+ printf("Error: memory leaks: %d allocations were not freed and leaked together %d bytes\n",mynumallocs,totalLeak);
+ }
+ return totalLeak;
+}
//this generic allocator provides the total allocated number of bytes
#include <stdio.h>
+struct btDebugPtrMagic
+{
+ union
+ {
+ void** vptrptr;
+ void* vptr;
+ int* iptr;
+ char* cptr;
+ };
+};
+
+
void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename)
{
+ if (size==0)
+ {
+ printf("Whaat? size==0");
+ return 0;
+ }
+ static int allocId = 0;
+
void *ret;
char *real;
+// to find some particular memory leak, you could do something like this:
+// if (allocId==172)
+// {
+// printf("catch me!\n");
+// }
+// if (size>1024*1024)
+// {
+// printf("big alloc!%d\n", size);
+// }
+
gTotalBytesAlignedAllocs += size;
gNumAlignedAllocs++;
- real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1));
+int sz4prt = 4*sizeof(void *);
+
+ real = (char *)sAllocFunc(size + sz4prt + (alignment-1));
if (real) {
- ret = (void*) btAlignPointer(real + 2*sizeof(void *), alignment);
- *((void **)(ret)-1) = (void *)(real);
- *((int*)(ret)-2) = size;
+
+ ret = (void*) btAlignPointer(real + sz4prt, alignment);
+ btDebugPtrMagic p;
+ p.vptr = ret;
+ p.cptr-=sizeof(void*);
+ *p.vptrptr = (void*)real;
+ p.cptr-=sizeof(void*);
+ *p.iptr = size;
+ p.cptr-=sizeof(void*);
+ *p.iptr = allocId;
+
+ allocations_id[mynumallocs] = allocId;
+ allocations_bytes[mynumallocs] = size;
+ mynumallocs++;
} else {
ret = (void *)(real);//??
}
- printf("allocation#%d at address %x, from %s,line %d, size %d\n",gNumAlignedAllocs,real, filename,line,size);
-
+ printf("allocation %d at address %x, from %s,line %d, size %d (total allocated = %d)\n",allocId,real, filename,line,size,gTotalBytesAlignedAllocs);
+ allocId++;
+
int* ptr = (int*)ret;
*ptr = 12;
return (ret);
@@ -138,19 +202,43 @@ void btAlignedFreeInternal (void* ptr,int line,char* filename)
{
void* real;
- gNumAlignedFree++;
if (ptr) {
- real = *((void **)(ptr)-1);
- int size = *((int*)(ptr)-2);
- gTotalBytesAlignedAllocs -= size;
-
- printf("free #%d at address %x, from %s,line %d, size %d\n",gNumAlignedFree,real, filename,line,size);
+ gNumAlignedFree++;
+
+ btDebugPtrMagic p;
+ p.vptr = ptr;
+ p.cptr-=sizeof(void*);
+ real = *p.vptrptr;
+ p.cptr-=sizeof(void*);
+ int size = *p.iptr;
+ p.cptr-=sizeof(void*);
+ int allocId = *p.iptr;
+
+ bool found = false;
+
+ for (int i=0;i<mynumallocs;i++)
+ {
+ if ( allocations_id[i] == allocId)
+ {
+ allocations_id[i] = allocations_id[mynumallocs-1];
+ allocations_bytes[i] = allocations_bytes[mynumallocs-1];
+ mynumallocs--;
+ found = true;
+ break;
+ }
+ }
+
+
+ gTotalBytesAlignedAllocs -= size;
+
+ int diff = gNumAlignedAllocs-gNumAlignedFree;
+ printf("free %d at address %x, from %s,line %d, size %d (total remain = %d in %d non-freed allocations)\n",allocId,real, filename,line,size, gTotalBytesAlignedAllocs, diff);
sFreeFunc(real);
} else
{
- printf("NULL ptr\n");
+ //printf("deleting a NULL ptr, no effect\n");
}
}
diff --git a/src/LinearMath/btAlignedAllocator.h b/src/LinearMath/btAlignedAllocator.h
index f168f3c..9873b33 100644
--- a/src/LinearMath/btAlignedAllocator.h
+++ b/src/LinearMath/btAlignedAllocator.h
@@ -21,9 +21,15 @@ subject to the following restrictions:
///that is better portable and more predictable
#include "btScalar.h"
-//#define BT_DEBUG_MEMORY_ALLOCATIONS 1
+
+
+///BT_DEBUG_MEMORY_ALLOCATIONS preprocessor can be set in build system
+///for regression tests to detect memory leaks
+///#define BT_DEBUG_MEMORY_ALLOCATIONS 1
#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
+int btDumpMemoryLeaks();
+
#define btAlignedAlloc(a,b) \
btAlignedAllocInternal(a,b,__LINE__,__FILE__)
diff --git a/src/LinearMath/btHashMap.h b/src/LinearMath/btHashMap.h
index af9727b..ca6f326 100644
--- a/src/LinearMath/btHashMap.h
+++ b/src/LinearMath/btHashMap.h
@@ -395,6 +395,19 @@ protected:
return &m_valueArray[index];
}
+ Key getKeyAtIndex(int index)
+ {
+ btAssert(index < m_keyArray.size());
+ return m_keyArray[index];
+ }
+
+ const Key getKeyAtIndex(int index) const
+ {
+ btAssert(index < m_keyArray.size());
+ return m_keyArray[index];
+ }
+
+
Value* operator[](const Key& key) {
return find(key);
}
diff --git a/src/LinearMath/btIDebugDraw.h b/src/LinearMath/btIDebugDraw.h
index 58c9838..a020c3f 100644
--- a/src/LinearMath/btIDebugDraw.h
+++ b/src/LinearMath/btIDebugDraw.h
@@ -166,9 +166,9 @@ class btIDebugDraw
virtual void drawTransform(const btTransform& transform, btScalar orthoLen)
{
btVector3 start = transform.getOrigin();
- drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0));
- drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0));
- drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f));
+ drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(1.f,0.3,0.3));
+ drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0.3,1.f, 0.3));
+ drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0.3, 0.3,1.f));
}
virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h
index 41dea69..40cd1e0 100644
--- a/src/LinearMath/btMatrix3x3.h
+++ b/src/LinearMath/btMatrix3x3.h
@@ -1047,7 +1047,8 @@ btMatrix3x3::inverse() const
{
btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
btScalar det = (*this)[0].dot(co);
- btFullAssert(det != btScalar(0.0));
+ //btFullAssert(det != btScalar(0.0));
+ btAssert(det != btScalar(0.0));
btScalar s = btScalar(1.0) / det;
return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
@@ -1329,7 +1330,9 @@ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
c0 = _mm_and_ps(c0, c1);
c0 = _mm_and_ps(c0, c2);
- return (0x7 == _mm_movemask_ps((__m128)c0));
+ int m = _mm_movemask_ps((__m128)c0);
+ return (0x7 == (m & 0x7));
+
#else
return
( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
diff --git a/src/LinearMath/btPolarDecomposition.cpp b/src/LinearMath/btPolarDecomposition.cpp
index a4dca7f..b3664fa 100644
--- a/src/LinearMath/btPolarDecomposition.cpp
+++ b/src/LinearMath/btPolarDecomposition.cpp
@@ -30,8 +30,7 @@ namespace
}
}
-const btScalar btPolarDecomposition::DEFAULT_TOLERANCE = btScalar(0.0001);
-const unsigned int btPolarDecomposition::DEFAULT_MAX_ITERATIONS = 16;
+
btPolarDecomposition::btPolarDecomposition(btScalar tolerance, unsigned int maxIterations)
: m_tolerance(tolerance)
diff --git a/src/LinearMath/btPolarDecomposition.h b/src/LinearMath/btPolarDecomposition.h
index 5615667..1feea0f 100644
--- a/src/LinearMath/btPolarDecomposition.h
+++ b/src/LinearMath/btPolarDecomposition.h
@@ -14,8 +14,7 @@
class btPolarDecomposition
{
public:
- static const btScalar DEFAULT_TOLERANCE;
- static const unsigned int DEFAULT_MAX_ITERATIONS;
+
/**
* Creates an instance with optional parameters.
@@ -25,8 +24,8 @@ class btPolarDecomposition
* @param maxIterations - the maximum number of iterations used to achieve
* convergence
*/
- btPolarDecomposition(btScalar tolerance = DEFAULT_TOLERANCE,
- unsigned int maxIterations = DEFAULT_MAX_ITERATIONS);
+ btPolarDecomposition(btScalar tolerance = btScalar(0.0001),
+ unsigned int maxIterations = 16);
/**
* Decomposes a matrix into orthogonal and symmetric, positive-definite
diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h
index ede7693..32f0f85 100644
--- a/src/LinearMath/btQuaternion.h
+++ b/src/LinearMath/btQuaternion.h
@@ -418,22 +418,21 @@ public:
return btAcos(dot(q) / s) * btScalar(2.0);
}
- /**@brief Return the angle of rotation represented by this quaternion */
+ /**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */
btScalar getAngle() const
{
btScalar s = btScalar(2.) * btAcos(m_floats[3]);
return s;
}
- /**@brief Return the angle of rotation represented by this quaternion along the shortest path*/
+ /**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */
btScalar getAngleShortestPath() const
{
btScalar s;
- if (dot(*this) < 0)
+ if (m_floats[3] >= 0)
s = btScalar(2.) * btAcos(m_floats[3]);
else
s = btScalar(2.) * btAcos(-m_floats[3]);
-
return s;
}
@@ -533,25 +532,29 @@ public:
* Slerp interpolates assuming constant velocity. */
btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
{
- btScalar magnitude = btSqrt(length2() * q.length2());
- btAssert(magnitude > btScalar(0));
- btScalar product = dot(q) / magnitude;
- if (btFabs(product) < btScalar(1))
+ const btScalar magnitude = btSqrt(length2() * q.length2());
+ btAssert(magnitude > btScalar(0));
+
+ const btScalar product = dot(q) / magnitude;
+ const btScalar absproduct = btFabs(product);
+
+ if(absproduct < btScalar(1.0 - SIMD_EPSILON))
{
- // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
- const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
-
- const btScalar theta = btAcos(sign * product);
- const btScalar s1 = btSin(sign * t * theta);
- const btScalar d = btScalar(1.0) / btSin(theta);
- const btScalar s0 = btSin((btScalar(1.0) - t) * theta);
-
- return btQuaternion(
- (m_floats[0] * s0 + q.x() * s1) * d,
- (m_floats[1] * s0 + q.y() * s1) * d,
- (m_floats[2] * s0 + q.z() * s1) * d,
- (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
+ // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+ const btScalar theta = btAcos(absproduct);
+ const btScalar d = btSin(theta);
+ btAssert(d > btScalar(0));
+
+ const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
+ const btScalar s0 = btSin((btScalar(1.0) - t) * theta) / d;
+ const btScalar s1 = btSin(sign * t * theta) / d;
+
+ return btQuaternion(
+ (m_floats[0] * s0 + q.x() * s1),
+ (m_floats[1] * s0 + q.y() * s1),
+ (m_floats[2] * s0 + q.z() * s1),
+ (m_floats[3] * s0 + q.w() * s1));
}
else
{
diff --git a/src/LinearMath/btQuickprof.cpp b/src/LinearMath/btQuickprof.cpp
index d88d965..cfbda36 100644
--- a/src/LinearMath/btQuickprof.cpp
+++ b/src/LinearMath/btQuickprof.cpp
@@ -15,10 +15,7 @@
#include "btQuickprof.h"
-#ifndef BT_NO_PROFILE
-
-static btClock gProfileClock;
#ifdef __CELLOS_LV2__
@@ -250,6 +247,10 @@ btScalar btClock::getTimeSeconds()
return btScalar(getTimeMicroseconds()) * microseconds_to_seconds;
}
+#ifndef BT_NO_PROFILE
+
+
+static btClock gProfileClock;
inline void Profile_Get_Ticks(unsigned long int * ticks)
@@ -265,7 +266,6 @@ inline float Profile_Get_Tick_Rate(void)
}
-
/***************************************************************************************************
**
** CProfileNode
diff --git a/src/LinearMath/btQuickprof.h b/src/LinearMath/btQuickprof.h
index 362f62d..4954571 100644
--- a/src/LinearMath/btQuickprof.h
+++ b/src/LinearMath/btQuickprof.h
@@ -15,18 +15,7 @@
#ifndef BT_QUICK_PROF_H
#define BT_QUICK_PROF_H
-//To disable built-in profiling, please comment out next line
-//#define BT_NO_PROFILE 1
-#ifndef BT_NO_PROFILE
-#include <stdio.h>//@todo remove this, backwards compatibility
#include "btScalar.h"
-#include "btAlignedAllocator.h"
-#include <new>
-
-
-
-
-
#define USE_BT_CLOCK 1
#ifdef USE_BT_CLOCK
@@ -64,6 +53,20 @@ private:
#endif //USE_BT_CLOCK
+//To disable built-in profiling, please comment out next line
+#define BT_NO_PROFILE 1
+#ifndef BT_NO_PROFILE
+#include <stdio.h>//@todo remove this, backwards compatibility
+
+#include "btAlignedAllocator.h"
+#include <new>
+
+
+
+
+
+
+
///A node in the Profile Hierarchy Tree
diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h
index 0bfd255..df907e1 100644
--- a/src/LinearMath/btScalar.h
+++ b/src/LinearMath/btScalar.h
@@ -17,6 +17,7 @@ subject to the following restrictions:
#ifndef BT_SCALAR_H
#define BT_SCALAR_H
+
#ifdef BT_MANAGED_CODE
//Aligned data types not supported in managed code
#pragma unmanaged
@@ -28,7 +29,7 @@ subject to the following restrictions:
#include <float.h>
/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
-#define BT_BULLET_VERSION 284
+#define BT_BULLET_VERSION 285
inline int btGetVersion()
{
@@ -57,7 +58,7 @@ inline int btGetVersion()
//#define BT_HAS_ALIGNED_ALLOCATOR
#pragma warning(disable : 4324) // disable padding warning
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
-// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
+ #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
// #pragma warning(disable:4786) // Disable the "debug name too long" warning
#define SIMD_FORCE_INLINE __forceinline
@@ -104,7 +105,7 @@ inline int btGetVersion()
#ifdef BT_DEBUG
#ifdef _MSC_VER
#include <stdio.h>
- #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }}
+ #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak(); }}
#else//_MSC_VER
#include <assert.h>
#define btAssert assert
diff --git a/src/LinearMath/btSerializer.cpp b/src/LinearMath/btSerializer.cpp
index 8fdcfb1..9838e6c 100644
--- a/src/LinearMath/btSerializer.cpp
+++ b/src/LinearMath/btSerializer.cpp
@@ -1,5 +1,5 @@
char sBulletDNAstr[]= {
-char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(123),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109),
+char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(126),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109),
char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95),
char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111),
char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),
@@ -100,123 +100,123 @@ char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),ch
char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),
char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),
char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),
-char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73),
-char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108),char(108),
-char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),
-char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),
-char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),
-char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),
-char(117),char(108),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),
-char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),
-char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),
-char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),
-char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),
-char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),
-char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),
-char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),
-char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),
-char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),
-char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),
-char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),
-char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),
-char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),
-char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),
-char(108),char(100),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),
-char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),
-char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),
-char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),
-char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),
-char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112),
-char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100),
-char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),
-char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),
-char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),
-char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),
-char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),
-char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98),
-char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104),
-char(111),char(108),char(100),char(0),char(109),char(95),char(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100),
-char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),
-char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),
-char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),
-char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),
-char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),
-char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),
-char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101),
-char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),
-char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),
-char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),
-char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),
-char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),
-char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119),
-char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),
-char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(100),char(97),char(109),char(112),
-char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),
-char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110),
-char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116),
-char(70),char(111),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114),char(97),char(109),char(101),char(0),char(109),
-char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110),
-char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),
-char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83),
-char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),
-char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(66),
-char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),
-char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),
-char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110),
-char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),
-char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(108),char(105),char(110),
-char(101),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(108),
-char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(108),
-char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),
-char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),
-char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),
-char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98),
-char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),
-char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),
-char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(108),
-char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),
-char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),
-char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),
-char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109),
-char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),
-char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),
-char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),
-char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(84),char(97),
-char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),
-char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(97),char(110),
-char(103),char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(97),
-char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),
-char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),
-char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),
-char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),
-char(69),char(110),char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),
-char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),
-char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),
-char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),
-char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),
-char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),
-char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),char(116),char(101),char(79),
-char(114),char(100),char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),char(97),char(120),char(105),
-char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109),
-char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114),
-char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114),
-char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109),
-char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116),
-char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112),
-char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95),
-char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97),
-char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114),
-char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108),
-char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),
-char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97),
-char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109),
-char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105),
-char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105),
-char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101),
-char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(105),
+char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(100),
+char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),
+char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114),char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),
+char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114),char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),
+char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),
+char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116),char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),
+char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),
+char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),
+char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97),char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),
+char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114),char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),
+char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108),char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),
+char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),
+char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),
+char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),
+char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),
+char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105),char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),
+char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),
+char(108),char(115),char(101),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(0),char(109),char(95),char(103),
+char(114),char(97),char(118),char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),
+char(106),char(101),char(99),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),
+char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),
+char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(86),
+char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(70),char(97),char(99),
+char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),
+char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),char(108),char(101),char(114),char(97),char(116),char(105),char(111),
+char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(76),char(111),char(99),char(97),char(108),char(0),
+char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),
+char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),char(114),char(115),char(101),char(77),char(97),char(115),char(115),
+char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),
+char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(100),char(105),
+char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),
+char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),char(105),char(110),char(101),char(97),char(114),char(68),char(97),
+char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),
+char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),
+char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),
+char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),
+char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),
+char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(110),
+char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),
+char(108),char(100),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),
+char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(82),char(111),
+char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),char(0),char(42),char(109),char(95),char(114),char(98),char(66),
+char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),
+char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),char(101),
+char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100),char(0),char(109),char(95),char(110),char(101),char(101),char(100),
+char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),char(112),char(112),char(108),char(105),char(101),char(100),char(73),
+char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),char(114),char(97),char(119),char(83),char(105),char(122),char(101),
+char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(115),
+char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),char(66),char(111),char(100),char(105),char(101),char(115),char(0),
+char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(73),
+char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98),char(114),char(101),char(97),char(107),char(105),char(110),char(103),
+char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(105),
+char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),
+char(109),char(95),char(116),char(121),char(112),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),
+char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),
+char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(114),char(98),char(66),char(70),
+char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),
+char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(79),char(110),char(108),char(121),char(0),
+char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),
+char(0),char(109),char(95),char(109),char(111),char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),
+char(116),char(121),char(0),char(109),char(95),char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),
+char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(112),char(112),char(101),
+char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),char(116),char(83),char(111),char(102),char(116),char(110),char(101),
+char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(114),char(101),
+char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(112),char(97),char(100),
+char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),
+char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(50),char(0),char(109),char(95),char(116),char(119),char(105),
+char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),
+char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),
+char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),
+char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),
+char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),
+char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),
+char(115),char(101),char(116),char(70),char(111),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114),char(97),char(109),
+char(101),char(0),char(109),char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),
+char(103),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),
+char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),
+char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),
+char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),
+char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),
+char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),
+char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),
+char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),
+char(101),char(97),char(114),char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),
+char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),
+char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),
+char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),
+char(101),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),
+char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),
+char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),
+char(110),char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),
+char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),
+char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),
+char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),
+char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),
+char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),
+char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),
+char(101),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),
+char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),
+char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),
+char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),
+char(114),char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),
+char(103),char(117),char(108),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),
+char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),
+char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),
+char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),
+char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),
+char(105),char(108),char(105),char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),
+char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),
+char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),
+char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),
+char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),
+char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),
+char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),
+char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),
+char(116),char(101),char(79),char(114),char(100),char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),
+char(97),char(120),char(105),char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(108),char(105),
char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),
char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),
char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),
@@ -297,298 +297,301 @@ char(111),char(109),char(84),char(111),char(84),char(104),char(105),char(115),ch
char(116),char(104),char(105),char(115),char(80),char(105),char(118),char(111),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),
char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91),
char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109),
-char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106),
+char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),
+char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110),char(100),
+char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115),char(86),
+char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91),char(55),
+char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111),
+char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),
+char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(70),char(114),char(105),char(99),char(116),
+char(105),char(111),char(110),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106),
char(111),char(105),char(110),char(116),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108),
-char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),
-char(95),char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110),
-char(100),char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115),
-char(86),char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91),
-char(55),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106),
-char(111),char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(98),char(97),char(115),
-char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101),
-char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),
-char(114),char(109),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98),
-char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(84),char(89),char(80),char(69),char(95),char(0),char(0),char(0),char(99),char(104),char(97),char(114),
-char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),
-char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),
-char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),
-char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),
-char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),
-char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),
-char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),
-char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),
-char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),
-char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),
-char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),
-char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),
-char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),
-char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),
-char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),
-char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),
-char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),
-char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),
-char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),
-char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),
-char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),
-char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),
-char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),
-char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),
-char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),
-char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),
-char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),
-char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83),
-char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104),
-char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),
-char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),
-char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),
-char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),
-char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),
-char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),
-char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),
-char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),
-char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),
-char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108),
-char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),
-char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),
-char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),
-char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116),
-char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),
-char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),
-char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),
-char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),
-char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),
-char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),
-char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),
+char(105),char(100),char(101),char(114),char(0),char(42),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(80),char(116),char(114),char(0),char(109),
+char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),
+char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98),char(97),char(115),char(101),
+char(77),char(97),char(115),char(115),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),
+char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(0),char(0),char(0),char(0),char(84),char(89),char(80),char(69),
+char(95),char(0),char(0),char(0),char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),
+char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),
+char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),
+char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),
+char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),
+char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),
+char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),
+char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),
+char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),
+char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),
+char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),
+char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),
+char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),
+char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),
+char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),
+char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),
+char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
+char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),
+char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),
+char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),
+char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),
+char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),
+char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),
+char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),
+char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),
+char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),
+char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),
+char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),
+char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),
+char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),
+char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),
+char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),
+char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),
+char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),
+char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),
+char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),
+char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),
+char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),
+char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),
+char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),
+char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108),char(111),char(97),char(116),
+char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),
+char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),
+char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),
+char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),
+char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),
+char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),
+char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),
+char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),
-char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),
+char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),
+char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),
+char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),
char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),
-char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),
-char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67),
-char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),
-char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),
-char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),
-char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),
+char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),
+char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),
+char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),
+char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),
+char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),
char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),
+char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),
+char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),
+char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),
+char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),
+char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),
+char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),
+char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),
char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),
-char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),
-char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),
-char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
-char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),
-char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),
-char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),
-char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),
-char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),
-char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),
-char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),
-char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),
-char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),
-char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),
-char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),
-char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),
-char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),
-char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),
-char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),
-char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),
-char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98),
-char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),
-char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),
-char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),
-char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),
-char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),
-char(12),char(0),char(36),char(0),char(8),char(0),char(16),char(0),char(32),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),
-char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(84),char(0),char(-124),char(0),char(12),char(0),char(52),char(0),char(52),char(0),
-char(20),char(0),char(64),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(32),char(0),char(28),char(0),char(60),char(0),char(56),char(0),
-char(76),char(0),char(76),char(0),char(24),char(0),char(60),char(0),char(60),char(0),char(60),char(0),char(16),char(0),char(64),char(0),char(68),char(0),char(-48),char(1),
-char(0),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-24),char(1),char(-96),char(3),char(8),char(0),char(52),char(0),char(52),char(0),
-char(0),char(0),char(68),char(0),char(84),char(0),char(-124),char(0),char(116),char(0),char(92),char(1),char(-36),char(0),char(-116),char(1),char(124),char(1),char(-44),char(0),
-char(-4),char(0),char(-52),char(1),char(92),char(1),char(116),char(2),char(-124),char(2),char(-76),char(4),char(-52),char(0),char(108),char(1),char(92),char(0),char(-116),char(0),
-char(16),char(0),char(100),char(0),char(20),char(0),char(36),char(0),char(100),char(0),char(92),char(0),char(104),char(0),char(-64),char(0),char(92),char(1),char(104),char(0),
-char(-84),char(1),char(-68),char(2),char(108),char(1),char(-68),char(0),char(100),char(0),char(0),char(0),char(83),char(84),char(82),char(67),char(84),char(0),char(0),char(0),
-char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),
-char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),
-char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),
-char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),
-char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),
-char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0),
-char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0),
-char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),
-char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),
-char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0),
-char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),
-char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),
-char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),
-char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),
-char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),
-char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0),
-char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),
-char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0),
-char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0),
-char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),
-char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),
-char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),
-char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),
-char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),
-char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0),
-char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),
-char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),
-char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),
-char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),
-char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0),
-char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),
-char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),
-char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),
-char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0),
-char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),
-char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),
-char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),
-char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0),char(29),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0),
-char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(49),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),
-char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0),
-char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),
-char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),
-char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),
-char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(50),char(0),char(25),char(0),char(9),char(0),char(101),char(0),
-char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0),
-char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),
-char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),
-char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),
-char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(2),char(0),
-char(52),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(53),char(0),char(2),char(0),char(54),char(0),char(124),char(0),char(13),char(0),char(125),char(0),
-char(55),char(0),char(21),char(0),char(50),char(0),char(126),char(0),char(17),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0),
-char(13),char(0),char(-126),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0),
-char(13),char(0),char(-122),char(0),char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0),
-char(7),char(0),char(-117),char(0),char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0),
-char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(56),char(0),char(22),char(0),char(49),char(0),char(126),char(0),char(18),char(0),char(127),char(0),
-char(14),char(0),char(-128),char(0),char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0),
-char(14),char(0),char(-124),char(0),char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),
-char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0),
-char(8),char(0),char(-114),char(0),char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0),
-char(57),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-108),char(0),
-char(55),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),
-char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),
-char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(59),char(0),char(13),char(0),char(60),char(0),char(-108),char(0),char(60),char(0),char(-107),char(0),
-char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),
-char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),
-char(4),char(0),char(-97),char(0),char(61),char(0),char(14),char(0),char(56),char(0),char(-108),char(0),char(56),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),
-char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0),
-char(8),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),
-char(0),char(0),char(-96),char(0),char(62),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0),
-char(63),char(0),char(3),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(64),char(0),char(3),char(0),
-char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),
-char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),
-char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),
-char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(66),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),
-char(19),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),
-char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0),
-char(7),char(0),char(-81),char(0),char(67),char(0),char(14),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),
-char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0),
-char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),
-char(0),char(0),char(-80),char(0),char(68),char(0),char(10),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),
-char(8),char(0),char(-79),char(0),char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),
-char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(69),char(0),char(11),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),
-char(19),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0),
-char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0),
-char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),
-char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(71),char(0),char(9),char(0),
-char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),
-char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(72),char(0),char(5),char(0),
-char(70),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),
-char(73),char(0),char(5),char(0),char(71),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),
-char(8),char(0),char(-65),char(0),char(74),char(0),char(41),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),
-char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),char(13),char(0),char(-62),char(0),
-char(13),char(0),char(-61),char(0),char(13),char(0),char(-60),char(0),char(13),char(0),char(-59),char(0),char(13),char(0),char(-58),char(0),char(13),char(0),char(-57),char(0),
-char(13),char(0),char(-56),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),
-char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0),char(0),char(0),char(-80),char(0),char(13),char(0),char(-73),char(0),
-char(13),char(0),char(-72),char(0),char(13),char(0),char(-48),char(0),char(13),char(0),char(-47),char(0),char(13),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0),
-char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0),
-char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),
-char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-95),char(0),
-char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),char(14),char(0),char(-64),char(0),
-char(14),char(0),char(-63),char(0),char(14),char(0),char(-62),char(0),char(14),char(0),char(-61),char(0),char(14),char(0),char(-60),char(0),char(14),char(0),char(-59),char(0),
-char(14),char(0),char(-58),char(0),char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),
-char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0),
-char(0),char(0),char(-80),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(14),char(0),char(-48),char(0),char(14),char(0),char(-47),char(0),
-char(14),char(0),char(-46),char(0),char(14),char(0),char(-45),char(0),char(14),char(0),char(-44),char(0),char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0),
-char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0),char(14),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),
-char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),
-char(76),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(7),char(0),char(-75),char(0),
-char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),
-char(77),char(0),char(9),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(8),char(0),char(-75),char(0),
-char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),
-char(78),char(0),char(5),char(0),char(58),char(0),char(-95),char(0),char(13),char(0),char(-31),char(0),char(13),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0),
-char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-31),char(0),char(14),char(0),char(-30),char(0),
-char(8),char(0),char(-29),char(0),char(52),char(0),char(22),char(0),char(8),char(0),char(-28),char(0),char(8),char(0),char(-76),char(0),char(8),char(0),char(111),char(0),
-char(8),char(0),char(-27),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-26),char(0),char(8),char(0),char(-25),char(0),char(8),char(0),char(-24),char(0),
-char(8),char(0),char(-23),char(0),char(8),char(0),char(-22),char(0),char(8),char(0),char(-21),char(0),char(8),char(0),char(-20),char(0),char(8),char(0),char(-19),char(0),
-char(8),char(0),char(-18),char(0),char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),
-char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0),
-char(7),char(0),char(-28),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(113),char(0),
-char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0),
-char(7),char(0),char(-21),char(0),char(7),char(0),char(-20),char(0),char(7),char(0),char(-19),char(0),char(7),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),
-char(7),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),
-char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(80),char(0),char(4),char(0),char(7),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),
-char(7),char(0),char(-8),char(0),char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),char(80),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0),
-char(13),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(7),char(0),char(-120),char(0),
-char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(4),char(0),char(53),char(0),char(82),char(0),char(4),char(0),char(80),char(0),char(-7),char(0),
-char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(83),char(0),char(4),char(0),char(13),char(0),char(-2),char(0),
-char(80),char(0),char(-7),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),char(84),char(0),char(7),char(0),char(13),char(0),char(6),char(1),
-char(80),char(0),char(-7),char(0),char(4),char(0),char(7),char(1),char(7),char(0),char(8),char(1),char(7),char(0),char(9),char(1),char(7),char(0),char(10),char(1),
-char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(11),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(12),char(1),
-char(60),char(0),char(13),char(1),char(4),char(0),char(14),char(1),char(7),char(0),char(10),char(1),char(86),char(0),char(26),char(0),char(4),char(0),char(15),char(1),
-char(7),char(0),char(16),char(1),char(7),char(0),char(-76),char(0),char(7),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(19),char(1),
-char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),
-char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1),
-char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),
-char(4),char(0),char(35),char(1),char(4),char(0),char(36),char(1),char(4),char(0),char(37),char(1),char(4),char(0),char(38),char(1),char(4),char(0),char(118),char(0),
-char(87),char(0),char(12),char(0),char(17),char(0),char(39),char(1),char(17),char(0),char(40),char(1),char(17),char(0),char(41),char(1),char(13),char(0),char(42),char(1),
-char(13),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),char(4),char(0),char(46),char(1),char(4),char(0),char(47),char(1),
-char(4),char(0),char(48),char(1),char(7),char(0),char(8),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(27),char(0),char(19),char(0),char(49),char(1),
-char(17),char(0),char(50),char(1),char(17),char(0),char(51),char(1),char(13),char(0),char(42),char(1),char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1),
-char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1),
-char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1),
-char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(7),char(0),char(66),char(1),char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1),
-char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(4),char(0),char(72),char(1),char(4),char(0),char(73),char(1),
-char(4),char(0),char(74),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(75),char(1),char(9),char(0),char(76),char(1),char(13),char(0),char(77),char(1),
-char(7),char(0),char(78),char(1),char(7),char(0),char(-24),char(0),char(7),char(0),char(79),char(1),char(4),char(0),char(80),char(1),char(13),char(0),char(81),char(1),
-char(4),char(0),char(82),char(1),char(4),char(0),char(83),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0),
-char(50),char(0),char(126),char(0),char(87),char(0),char(85),char(1),char(80),char(0),char(86),char(1),char(81),char(0),char(87),char(1),char(82),char(0),char(88),char(1),
-char(83),char(0),char(89),char(1),char(84),char(0),char(90),char(1),char(85),char(0),char(91),char(1),char(88),char(0),char(92),char(1),char(89),char(0),char(93),char(1),
-char(4),char(0),char(94),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(97),char(1),
-char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(86),char(0),char(101),char(1),char(91),char(0),char(17),char(0),
-char(16),char(0),char(102),char(1),char(14),char(0),char(103),char(1),char(14),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1),
-char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),char(49),char(0),char(109),char(1),char(14),char(0),char(110),char(1),char(8),char(0),char(111),char(1),
-char(4),char(0),char(112),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(113),char(1),char(4),char(0),char(114),char(1),char(8),char(0),char(115),char(1),
-char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(92),char(0),char(17),char(0),char(15),char(0),char(102),char(1),char(13),char(0),char(103),char(1),
-char(13),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),
-char(50),char(0),char(109),char(1),char(13),char(0),char(110),char(1),char(4),char(0),char(113),char(1),char(7),char(0),char(111),char(1),char(4),char(0),char(112),char(1),
-char(4),char(0),char(84),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(7),char(0),char(117),char(1),char(4),char(0),char(114),char(1),
-char(93),char(0),char(8),char(0),char(0),char(0),char(118),char(1),char(91),char(0),char(88),char(1),char(49),char(0),char(119),char(1),char(20),char(0),char(120),char(1),
-char(14),char(0),char(121),char(1),char(4),char(0),char(95),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),
-char(0),char(0),char(118),char(1),char(92),char(0),char(88),char(1),char(50),char(0),char(119),char(1),char(19),char(0),char(120),char(1),char(13),char(0),char(121),char(1),
-char(7),char(0),char(122),char(1),char(4),char(0),char(95),char(1),};
+char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),
+char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),
+char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),
+char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),
+char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),
+char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),
+char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),
+char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),
+char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),
+char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),
+char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),
+char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),
+char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),
+char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),
+char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),
+char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),
+char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),
+char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),
+char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(12),char(0),char(36),char(0),char(8),char(0),char(16),char(0),char(32),char(0),char(16),char(0),
+char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(84),char(0),
+char(-124),char(0),char(12),char(0),char(52),char(0),char(52),char(0),char(20),char(0),char(64),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),
+char(32),char(0),char(28),char(0),char(60),char(0),char(56),char(0),char(76),char(0),char(76),char(0),char(24),char(0),char(60),char(0),char(60),char(0),char(60),char(0),
+char(16),char(0),char(64),char(0),char(68),char(0),char(-48),char(1),char(0),char(1),char(-104),char(0),char(88),char(0),char(-72),char(0),char(104),char(0),char(-24),char(1),
+char(-96),char(3),char(8),char(0),char(52),char(0),char(52),char(0),char(0),char(0),char(68),char(0),char(84),char(0),char(-124),char(0),char(116),char(0),char(92),char(1),
+char(-36),char(0),char(-116),char(1),char(124),char(1),char(-44),char(0),char(-4),char(0),char(-52),char(1),char(92),char(1),char(116),char(2),char(-124),char(2),char(-76),char(4),
+char(-52),char(0),char(108),char(1),char(92),char(0),char(-116),char(0),char(16),char(0),char(100),char(0),char(20),char(0),char(36),char(0),char(100),char(0),char(92),char(0),
+char(104),char(0),char(-64),char(0),char(92),char(1),char(104),char(0),char(-84),char(1),char(-48),char(2),char(120),char(1),char(-64),char(0),char(100),char(0),char(0),char(0),
+char(83),char(84),char(82),char(67),char(84),char(0),char(0),char(0),char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),
+char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),
+char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),
+char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),
+char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),
+char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),
+char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0),char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),
+char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0),char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),
+char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),
+char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),
+char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),
+char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),
+char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),
+char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),
+char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),
+char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),
+char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),
+char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),
+char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),
+char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0),char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),
+char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),
+char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),
+char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),
+char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),
+char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),
+char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0),char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),
+char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),
+char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),
+char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),
+char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),
+char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),
+char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),
+char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),
+char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),
+char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0),char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),
+char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),
+char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),
+char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0),char(29),char(0),char(47),char(0),
+char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0),char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(49),char(0),char(25),char(0),
+char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),
+char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0),char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),
+char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),
+char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),
+char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),
+char(50),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),
+char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0),char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),
+char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),
+char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),
+char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),
+char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(8),char(0),char(124),char(0),char(8),char(0),char(125),char(0),char(8),char(0),char(111),char(0),
+char(8),char(0),char(126),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(127),char(0),char(8),char(0),char(-128),char(0),char(8),char(0),char(-127),char(0),
+char(8),char(0),char(-126),char(0),char(8),char(0),char(-125),char(0),char(8),char(0),char(-124),char(0),char(8),char(0),char(-123),char(0),char(8),char(0),char(-122),char(0),
+char(8),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),char(8),char(0),char(-119),char(0),char(4),char(0),char(-118),char(0),char(4),char(0),char(-117),char(0),
+char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0),char(4),char(0),char(-114),char(0),char(0),char(0),char(37),char(0),char(52),char(0),char(22),char(0),
+char(7),char(0),char(124),char(0),char(7),char(0),char(125),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(126),char(0),char(7),char(0),char(113),char(0),
+char(7),char(0),char(127),char(0),char(7),char(0),char(-128),char(0),char(7),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(7),char(0),char(-125),char(0),
+char(7),char(0),char(-124),char(0),char(7),char(0),char(-123),char(0),char(7),char(0),char(-122),char(0),char(7),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),
+char(7),char(0),char(-119),char(0),char(4),char(0),char(-118),char(0),char(4),char(0),char(-117),char(0),char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0),
+char(4),char(0),char(-114),char(0),char(0),char(0),char(37),char(0),char(53),char(0),char(2),char(0),char(51),char(0),char(-113),char(0),char(14),char(0),char(-112),char(0),
+char(54),char(0),char(2),char(0),char(52),char(0),char(-113),char(0),char(13),char(0),char(-112),char(0),char(55),char(0),char(21),char(0),char(50),char(0),char(-111),char(0),
+char(17),char(0),char(-110),char(0),char(13),char(0),char(-109),char(0),char(13),char(0),char(-108),char(0),char(13),char(0),char(-107),char(0),char(13),char(0),char(-106),char(0),
+char(13),char(0),char(-112),char(0),char(13),char(0),char(-105),char(0),char(13),char(0),char(-104),char(0),char(13),char(0),char(-103),char(0),char(13),char(0),char(-102),char(0),
+char(7),char(0),char(-101),char(0),char(7),char(0),char(-100),char(0),char(7),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),char(7),char(0),char(-97),char(0),
+char(7),char(0),char(-96),char(0),char(7),char(0),char(-95),char(0),char(7),char(0),char(-94),char(0),char(7),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),
+char(56),char(0),char(22),char(0),char(49),char(0),char(-111),char(0),char(18),char(0),char(-110),char(0),char(14),char(0),char(-109),char(0),char(14),char(0),char(-108),char(0),
+char(14),char(0),char(-107),char(0),char(14),char(0),char(-106),char(0),char(14),char(0),char(-112),char(0),char(14),char(0),char(-105),char(0),char(14),char(0),char(-104),char(0),
+char(14),char(0),char(-103),char(0),char(14),char(0),char(-102),char(0),char(8),char(0),char(-101),char(0),char(8),char(0),char(-100),char(0),char(8),char(0),char(-99),char(0),
+char(8),char(0),char(-98),char(0),char(8),char(0),char(-97),char(0),char(8),char(0),char(-96),char(0),char(8),char(0),char(-95),char(0),char(8),char(0),char(-94),char(0),
+char(8),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(0),char(0),char(37),char(0),char(57),char(0),char(2),char(0),char(4),char(0),char(-91),char(0),
+char(4),char(0),char(-90),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-89),char(0),char(55),char(0),char(-88),char(0),char(0),char(0),char(35),char(0),
+char(4),char(0),char(-87),char(0),char(4),char(0),char(-86),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),
+char(7),char(0),char(-82),char(0),char(4),char(0),char(-81),char(0),char(4),char(0),char(-80),char(0),char(7),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),
+char(59),char(0),char(13),char(0),char(60),char(0),char(-89),char(0),char(60),char(0),char(-88),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-87),char(0),
+char(4),char(0),char(-86),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0),
+char(4),char(0),char(-81),char(0),char(4),char(0),char(-80),char(0),char(7),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(61),char(0),char(14),char(0),
+char(56),char(0),char(-89),char(0),char(56),char(0),char(-88),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-87),char(0),char(4),char(0),char(-86),char(0),
+char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(4),char(0),char(-81),char(0),
+char(4),char(0),char(-80),char(0),char(8),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(0),char(0),char(-77),char(0),char(62),char(0),char(3),char(0),
+char(59),char(0),char(-76),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),char(63),char(0),char(3),char(0),char(61),char(0),char(-76),char(0),
+char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),char(64),char(0),char(3),char(0),char(59),char(0),char(-76),char(0),char(14),char(0),char(-75),char(0),
+char(14),char(0),char(-74),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-76),char(0),char(20),char(0),char(-73),char(0),char(20),char(0),char(-72),char(0),
+char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(7),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),
+char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),
+char(66),char(0),char(13),char(0),char(59),char(0),char(-76),char(0),char(19),char(0),char(-73),char(0),char(19),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),
+char(4),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(7),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),
+char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(67),char(0),char(14),char(0),
+char(61),char(0),char(-76),char(0),char(20),char(0),char(-73),char(0),char(20),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),
+char(4),char(0),char(-69),char(0),char(8),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(-65),char(0),
+char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(0),char(0),char(-61),char(0),char(68),char(0),char(10),char(0),
+char(61),char(0),char(-76),char(0),char(20),char(0),char(-73),char(0),char(20),char(0),char(-72),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(-59),char(0),
+char(8),char(0),char(-58),char(0),char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(125),char(0),
+char(69),char(0),char(11),char(0),char(59),char(0),char(-76),char(0),char(19),char(0),char(-73),char(0),char(19),char(0),char(-72),char(0),char(7),char(0),char(-60),char(0),
+char(7),char(0),char(-59),char(0),char(7),char(0),char(-58),char(0),char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),
+char(7),char(0),char(125),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0),char(59),char(0),char(-76),char(0),char(19),char(0),char(-73),char(0),
+char(19),char(0),char(-72),char(0),char(13),char(0),char(-57),char(0),char(13),char(0),char(-56),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),
+char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(71),char(0),char(9),char(0),char(61),char(0),char(-76),char(0),char(20),char(0),char(-73),char(0),
+char(20),char(0),char(-72),char(0),char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),
+char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(72),char(0),char(5),char(0),char(70),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),
+char(7),char(0),char(-49),char(0),char(7),char(0),char(-48),char(0),char(7),char(0),char(-47),char(0),char(73),char(0),char(5),char(0),char(71),char(0),char(-51),char(0),
+char(4),char(0),char(-50),char(0),char(8),char(0),char(-49),char(0),char(8),char(0),char(-48),char(0),char(8),char(0),char(-47),char(0),char(74),char(0),char(41),char(0),
+char(59),char(0),char(-76),char(0),char(19),char(0),char(-73),char(0),char(19),char(0),char(-72),char(0),char(13),char(0),char(-57),char(0),char(13),char(0),char(-56),char(0),
+char(13),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0),char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),
+char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0),char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(13),char(0),char(-37),char(0),
+char(13),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0),
+char(0),char(0),char(-31),char(0),char(0),char(0),char(-61),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(13),char(0),char(-30),char(0),
+char(13),char(0),char(-29),char(0),char(13),char(0),char(-28),char(0),char(13),char(0),char(-27),char(0),char(13),char(0),char(-26),char(0),char(13),char(0),char(-25),char(0),
+char(13),char(0),char(-24),char(0),char(13),char(0),char(-23),char(0),char(13),char(0),char(-22),char(0),char(13),char(0),char(-21),char(0),char(13),char(0),char(-20),char(0),
+char(0),char(0),char(-19),char(0),char(0),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0),
+char(4),char(0),char(-14),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-76),char(0),char(20),char(0),char(-73),char(0),char(20),char(0),char(-72),char(0),
+char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),char(14),char(0),char(-46),char(0),char(14),char(0),char(-45),char(0),char(14),char(0),char(-44),char(0),
+char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0),char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0),
+char(14),char(0),char(-38),char(0),char(14),char(0),char(-37),char(0),char(14),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),
+char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0),char(0),char(0),char(-31),char(0),char(0),char(0),char(-61),char(0),char(14),char(0),char(-55),char(0),
+char(14),char(0),char(-54),char(0),char(14),char(0),char(-30),char(0),char(14),char(0),char(-29),char(0),char(14),char(0),char(-28),char(0),char(14),char(0),char(-27),char(0),
+char(14),char(0),char(-26),char(0),char(14),char(0),char(-25),char(0),char(14),char(0),char(-24),char(0),char(14),char(0),char(-23),char(0),char(14),char(0),char(-22),char(0),
+char(14),char(0),char(-21),char(0),char(14),char(0),char(-20),char(0),char(0),char(0),char(-19),char(0),char(0),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0),
+char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(76),char(0),char(9),char(0),char(59),char(0),char(-76),char(0),
+char(19),char(0),char(-73),char(0),char(19),char(0),char(-72),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),char(7),char(0),char(-55),char(0),
+char(7),char(0),char(-54),char(0),char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(77),char(0),char(9),char(0),char(61),char(0),char(-76),char(0),
+char(20),char(0),char(-73),char(0),char(20),char(0),char(-72),char(0),char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0),char(8),char(0),char(-55),char(0),
+char(8),char(0),char(-54),char(0),char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(78),char(0),char(5),char(0),char(58),char(0),char(-76),char(0),
+char(13),char(0),char(-13),char(0),char(13),char(0),char(-12),char(0),char(7),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),
+char(61),char(0),char(-76),char(0),char(14),char(0),char(-13),char(0),char(14),char(0),char(-12),char(0),char(8),char(0),char(-11),char(0),char(80),char(0),char(4),char(0),
+char(7),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),char(7),char(0),char(-8),char(0),char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),
+char(80),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0),char(13),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),
+char(13),char(0),char(-2),char(0),char(7),char(0),char(-101),char(0),char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(4),char(0),char(53),char(0),
+char(82),char(0),char(4),char(0),char(80),char(0),char(-7),char(0),char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(4),char(0),char(3),char(1),
+char(83),char(0),char(4),char(0),char(13),char(0),char(-2),char(0),char(80),char(0),char(-7),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),
+char(84),char(0),char(7),char(0),char(13),char(0),char(6),char(1),char(80),char(0),char(-7),char(0),char(4),char(0),char(7),char(1),char(7),char(0),char(8),char(1),
+char(7),char(0),char(9),char(1),char(7),char(0),char(10),char(1),char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(11),char(1),
+char(13),char(0),char(9),char(1),char(13),char(0),char(12),char(1),char(60),char(0),char(13),char(1),char(4),char(0),char(14),char(1),char(7),char(0),char(10),char(1),
+char(86),char(0),char(26),char(0),char(4),char(0),char(15),char(1),char(7),char(0),char(16),char(1),char(7),char(0),char(125),char(0),char(7),char(0),char(17),char(1),
+char(7),char(0),char(18),char(1),char(7),char(0),char(19),char(1),char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1),
+char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1),
+char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1),char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1),
+char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),char(4),char(0),char(35),char(1),char(4),char(0),char(36),char(1),char(4),char(0),char(37),char(1),
+char(4),char(0),char(38),char(1),char(4),char(0),char(118),char(0),char(87),char(0),char(12),char(0),char(17),char(0),char(39),char(1),char(17),char(0),char(40),char(1),
+char(17),char(0),char(41),char(1),char(13),char(0),char(42),char(1),char(13),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),
+char(4),char(0),char(46),char(1),char(4),char(0),char(47),char(1),char(4),char(0),char(48),char(1),char(7),char(0),char(8),char(1),char(4),char(0),char(53),char(0),
+char(88),char(0),char(27),char(0),char(19),char(0),char(49),char(1),char(17),char(0),char(50),char(1),char(17),char(0),char(51),char(1),char(13),char(0),char(42),char(1),
+char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1),char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),
+char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1),char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(61),char(1),
+char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1),char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(7),char(0),char(66),char(1),
+char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1),char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),
+char(4),char(0),char(72),char(1),char(4),char(0),char(73),char(1),char(4),char(0),char(74),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(75),char(1),
+char(9),char(0),char(76),char(1),char(13),char(0),char(77),char(1),char(7),char(0),char(78),char(1),char(7),char(0),char(-127),char(0),char(7),char(0),char(79),char(1),
+char(4),char(0),char(80),char(1),char(13),char(0),char(81),char(1),char(4),char(0),char(82),char(1),char(4),char(0),char(83),char(1),char(4),char(0),char(84),char(1),
+char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0),char(50),char(0),char(-111),char(0),char(87),char(0),char(85),char(1),char(80),char(0),char(86),char(1),
+char(81),char(0),char(87),char(1),char(82),char(0),char(88),char(1),char(83),char(0),char(89),char(1),char(84),char(0),char(90),char(1),char(85),char(0),char(91),char(1),
+char(88),char(0),char(92),char(1),char(89),char(0),char(93),char(1),char(4),char(0),char(94),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(95),char(1),
+char(4),char(0),char(96),char(1),char(4),char(0),char(97),char(1),char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),
+char(86),char(0),char(101),char(1),char(91),char(0),char(20),char(0),char(16),char(0),char(102),char(1),char(14),char(0),char(103),char(1),char(14),char(0),char(104),char(1),
+char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1),char(14),char(0),char(107),char(1),char(8),char(0),char(108),char(1),char(4),char(0),char(109),char(1),
+char(4),char(0),char(84),char(1),char(4),char(0),char(110),char(1),char(4),char(0),char(111),char(1),char(8),char(0),char(112),char(1),char(8),char(0),char(113),char(1),
+char(8),char(0),char(114),char(1),char(8),char(0),char(115),char(1),char(8),char(0),char(116),char(1),char(0),char(0),char(117),char(1),char(0),char(0),char(118),char(1),
+char(49),char(0),char(119),char(1),char(0),char(0),char(120),char(1),char(92),char(0),char(20),char(0),char(15),char(0),char(102),char(1),char(13),char(0),char(103),char(1),
+char(13),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(13),char(0),char(107),char(1),char(4),char(0),char(110),char(1),
+char(7),char(0),char(108),char(1),char(4),char(0),char(109),char(1),char(4),char(0),char(84),char(1),char(7),char(0),char(112),char(1),char(7),char(0),char(113),char(1),
+char(7),char(0),char(114),char(1),char(4),char(0),char(111),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(0),char(0),char(117),char(1),
+char(0),char(0),char(118),char(1),char(50),char(0),char(119),char(1),char(0),char(0),char(120),char(1),char(93),char(0),char(9),char(0),char(20),char(0),char(121),char(1),
+char(14),char(0),char(122),char(1),char(8),char(0),char(123),char(1),char(0),char(0),char(124),char(1),char(91),char(0),char(88),char(1),char(49),char(0),char(125),char(1),
+char(0),char(0),char(120),char(1),char(4),char(0),char(95),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),char(0),char(0),char(124),char(1),
+char(92),char(0),char(88),char(1),char(50),char(0),char(125),char(1),char(19),char(0),char(121),char(1),char(13),char(0),char(122),char(1),char(7),char(0),char(123),char(1),
+char(4),char(0),char(95),char(1),};
int sBulletDNAlen= sizeof(sBulletDNAstr);
-
char sBulletDNAstr64[]= {
-char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(123),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109),
+char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(126),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109),
char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95),
char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111),
char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),
@@ -689,123 +692,123 @@ char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),ch
char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),
char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),
char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),
-char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73),
-char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108),char(108),
-char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),
-char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),
-char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),
-char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),
-char(117),char(108),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),
-char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),
-char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),
-char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),
-char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),
-char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),
-char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),
-char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),
-char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),
-char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),
-char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),
-char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),
-char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),
-char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),
-char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),
-char(108),char(100),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),
-char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),
-char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),
-char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),
-char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),
-char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112),
-char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100),
-char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),
-char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),
-char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),
-char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),
-char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),
-char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98),
-char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104),
-char(111),char(108),char(100),char(0),char(109),char(95),char(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100),
-char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),
-char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),
-char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),
-char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),
-char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),
-char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),
-char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101),
-char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),
-char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),
-char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),
-char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),
-char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),
-char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119),
-char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),
-char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(100),char(97),char(109),char(112),
-char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),
-char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),
-char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110),
-char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116),
-char(70),char(111),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114),char(97),char(109),char(101),char(0),char(109),
-char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110),
-char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),
-char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83),
-char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),
-char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(66),
-char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),
-char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),
-char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110),
-char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),
-char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(108),char(105),char(110),
-char(101),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(108),
-char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(108),
-char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),
-char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),
-char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),
-char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98),
-char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),
-char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),
-char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(108),
-char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),
-char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),
-char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),
-char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109),
-char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),
-char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),
-char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),
-char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(84),char(97),
-char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),
-char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(97),char(110),
-char(103),char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(97),
-char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),
-char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),
-char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),
-char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),
-char(69),char(110),char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),
-char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),
-char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),
-char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),
-char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),
-char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),
-char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),char(116),char(101),char(79),
-char(114),char(100),char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),char(97),char(120),char(105),
-char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109),
-char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114),
-char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114),
-char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109),
-char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116),
-char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112),
-char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95),
-char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97),
-char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114),
-char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108),
-char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),
-char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97),
-char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109),
-char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105),
-char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105),
-char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101),
-char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(105),
+char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(100),
+char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),
+char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114),char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),
+char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114),char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),
+char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),
+char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116),char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),
+char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),
+char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),
+char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97),char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),
+char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114),char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),
+char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108),char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),
+char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),
+char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),
+char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),
+char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),
+char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105),char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),
+char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),
+char(108),char(115),char(101),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(0),char(109),char(95),char(103),
+char(114),char(97),char(118),char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),
+char(106),char(101),char(99),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),
+char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),
+char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(86),
+char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(70),char(97),char(99),
+char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),
+char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),char(108),char(101),char(114),char(97),char(116),char(105),char(111),
+char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(76),char(111),char(99),char(97),char(108),char(0),
+char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),
+char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),char(114),char(115),char(101),char(77),char(97),char(115),char(115),
+char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),
+char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(100),char(105),
+char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),
+char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),char(105),char(110),char(101),char(97),char(114),char(68),char(97),
+char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),
+char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),
+char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),
+char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),
+char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),
+char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(110),
+char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),
+char(108),char(100),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),
+char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(82),char(111),
+char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),char(0),char(42),char(109),char(95),char(114),char(98),char(66),
+char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),
+char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),char(101),
+char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100),char(0),char(109),char(95),char(110),char(101),char(101),char(100),
+char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),char(112),char(112),char(108),char(105),char(101),char(100),char(73),
+char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),char(114),char(97),char(119),char(83),char(105),char(122),char(101),
+char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(115),
+char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),char(66),char(111),char(100),char(105),char(101),char(115),char(0),
+char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(73),
+char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98),char(114),char(101),char(97),char(107),char(105),char(110),char(103),
+char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(105),
+char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),
+char(109),char(95),char(116),char(121),char(112),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),
+char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),
+char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(114),char(98),char(66),char(70),
+char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),
+char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(79),char(110),char(108),char(121),char(0),
+char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),
+char(0),char(109),char(95),char(109),char(111),char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),
+char(116),char(121),char(0),char(109),char(95),char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),
+char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(112),char(112),char(101),
+char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),char(116),char(83),char(111),char(102),char(116),char(110),char(101),
+char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(114),char(101),
+char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(112),char(97),char(100),
+char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),
+char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(50),char(0),char(109),char(95),char(116),char(119),char(105),
+char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),
+char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),
+char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),
+char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),
+char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),
+char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),
+char(115),char(101),char(116),char(70),char(111),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114),char(97),char(109),
+char(101),char(0),char(109),char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),
+char(103),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),
+char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),
+char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),
+char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),
+char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),
+char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),
+char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),
+char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),
+char(101),char(97),char(114),char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),
+char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),
+char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),
+char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),
+char(101),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),
+char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),
+char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),
+char(110),char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),
+char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),
+char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),
+char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),
+char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),
+char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),
+char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),
+char(101),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),
+char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),
+char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),
+char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),
+char(114),char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),
+char(103),char(117),char(108),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),
+char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),
+char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),
+char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),
+char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),
+char(105),char(108),char(105),char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),
+char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),
+char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),
+char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),
+char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),
+char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),
+char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),
+char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),
+char(116),char(101),char(79),char(114),char(100),char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),
+char(97),char(120),char(105),char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(108),char(105),
char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),
char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),
char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),
@@ -886,292 +889,296 @@ char(111),char(109),char(84),char(111),char(84),char(104),char(105),char(115),ch
char(116),char(104),char(105),char(115),char(80),char(105),char(118),char(111),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),
char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91),
char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109),
-char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106),
+char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),
+char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110),char(100),
+char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115),char(86),
+char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91),char(55),
+char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111),
+char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),
+char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(70),char(114),char(105),char(99),char(116),
+char(105),char(111),char(110),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106),
char(111),char(105),char(110),char(116),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108),
-char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),
-char(95),char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110),
-char(100),char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115),
-char(86),char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91),
-char(55),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106),
-char(111),char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(98),char(97),char(115),
-char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101),
-char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),
-char(114),char(109),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98),
-char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(84),char(89),char(80),char(69),char(95),char(0),char(0),char(0),char(99),char(104),char(97),char(114),
-char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),
-char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),
-char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),
-char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),
-char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),
-char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),
-char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),
-char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),
-char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),
-char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),
-char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),
-char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),
-char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),
-char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),
-char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),
-char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),
-char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),
-char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),
-char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),
-char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),
-char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),
-char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),
-char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),
-char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),
-char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),
-char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),
-char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),
-char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),
-char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83),
-char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104),
-char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),
-char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),
-char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),
-char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),
-char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),
-char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),
-char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),
-char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),
-char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),
-char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108),
-char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),
-char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),
-char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
-char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),
-char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116),
-char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),
-char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),
-char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),
-char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),
-char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),
-char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),
-char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),
-char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),
+char(105),char(100),char(101),char(114),char(0),char(42),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(80),char(116),char(114),char(0),char(109),
+char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),
+char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98),char(97),char(115),char(101),
+char(77),char(97),char(115),char(115),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),
+char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(0),char(0),char(0),char(0),char(84),char(89),char(80),char(69),
+char(95),char(0),char(0),char(0),char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),
+char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),
+char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),
+char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),
+char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),
+char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),
+char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),
+char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),
+char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),
+char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),
+char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),
+char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),
+char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),
+char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),
+char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),
+char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),
+char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
+char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),
+char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),
+char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),
+char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),
+char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),
+char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),
+char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),
+char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),
+char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),
+char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),
+char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),
+char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),
+char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),
+char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),
+char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),
+char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),
+char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),
+char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),
+char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),
+char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),
+char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),
+char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),
+char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),
+char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),
+char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108),char(111),char(97),char(116),
+char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),
+char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),
+char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),
+char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),
+char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),
+char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),
+char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),
+char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),
char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),
-char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),
+char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),
+char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),
+char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),
char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),
-char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),
-char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67),
-char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),
-char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),
-char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),
-char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),
+char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),
+char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),
+char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),
+char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),
+char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),
char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),
+char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),
+char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),
+char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),
+char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),
+char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),
+char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),
+char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),
char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),
-char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),
-char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),
-char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),
-char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),
-char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),
-char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),
-char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),
-char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),
-char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),
-char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),
-char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),
-char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),
-char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),
-char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),
-char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),
-char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),
-char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),
-char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),
-char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),
-char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),
-char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),
-char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98),
-char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),
-char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),
-char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),
-char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),
-char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),
-char(16),char(0),char(48),char(0),char(16),char(0),char(16),char(0),char(32),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),
-char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(96),char(0),char(-112),char(0),char(16),char(0),char(56),char(0),char(56),char(0),
-char(20),char(0),char(72),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(56),char(0),char(32),char(0),char(80),char(0),char(72),char(0),
-char(96),char(0),char(80),char(0),char(32),char(0),char(64),char(0),char(64),char(0),char(64),char(0),char(16),char(0),char(72),char(0),char(80),char(0),char(-32),char(1),
-char(16),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-8),char(1),char(-80),char(3),char(8),char(0),char(64),char(0),char(64),char(0),
-char(0),char(0),char(80),char(0),char(96),char(0),char(-112),char(0),char(-128),char(0),char(104),char(1),char(-24),char(0),char(-104),char(1),char(-120),char(1),char(-32),char(0),
-char(8),char(1),char(-40),char(1),char(104),char(1),char(-128),char(2),char(-112),char(2),char(-64),char(4),char(-40),char(0),char(120),char(1),char(104),char(0),char(-104),char(0),
-char(16),char(0),char(104),char(0),char(24),char(0),char(40),char(0),char(104),char(0),char(96),char(0),char(104),char(0),char(-56),char(0),char(104),char(1),char(112),char(0),
-char(-32),char(1),char(-56),char(2),char(120),char(1),char(-56),char(0),char(112),char(0),char(0),char(0),char(83),char(84),char(82),char(67),char(84),char(0),char(0),char(0),
-char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),
-char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),
-char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),
-char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),
-char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),
-char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0),
-char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0),
-char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),
-char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),
-char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0),
-char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),
-char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),
-char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),
-char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),
-char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),
-char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0),
-char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),
-char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0),
-char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0),
-char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),
-char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),
-char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),
-char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),
-char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),
-char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0),
-char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),
-char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),
-char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),
-char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),
-char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0),
-char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),
-char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),
-char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),
-char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0),
-char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),
-char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),
-char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),
-char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0),char(29),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0),
-char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(49),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),
-char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0),
-char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),
-char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),
-char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),
-char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(50),char(0),char(25),char(0),char(9),char(0),char(101),char(0),
-char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0),
-char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),
-char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),
-char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),
-char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(2),char(0),
-char(52),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(53),char(0),char(2),char(0),char(54),char(0),char(124),char(0),char(13),char(0),char(125),char(0),
-char(55),char(0),char(21),char(0),char(50),char(0),char(126),char(0),char(17),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0),
-char(13),char(0),char(-126),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0),
-char(13),char(0),char(-122),char(0),char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0),
-char(7),char(0),char(-117),char(0),char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0),
-char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(56),char(0),char(22),char(0),char(49),char(0),char(126),char(0),char(18),char(0),char(127),char(0),
-char(14),char(0),char(-128),char(0),char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0),
-char(14),char(0),char(-124),char(0),char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),
-char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0),
-char(8),char(0),char(-114),char(0),char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0),
-char(57),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-108),char(0),
-char(55),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),
-char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),
-char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(59),char(0),char(13),char(0),char(60),char(0),char(-108),char(0),char(60),char(0),char(-107),char(0),
-char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),
-char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),
-char(4),char(0),char(-97),char(0),char(61),char(0),char(14),char(0),char(56),char(0),char(-108),char(0),char(56),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),
-char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0),
-char(8),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),
-char(0),char(0),char(-96),char(0),char(62),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0),
-char(63),char(0),char(3),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(64),char(0),char(3),char(0),
-char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),
-char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),
-char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),
-char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(66),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),
-char(19),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),
-char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0),
-char(7),char(0),char(-81),char(0),char(67),char(0),char(14),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),
-char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0),
-char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),
-char(0),char(0),char(-80),char(0),char(68),char(0),char(10),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),
-char(8),char(0),char(-79),char(0),char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),
-char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(69),char(0),char(11),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),
-char(19),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0),
-char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0),
-char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),
-char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(71),char(0),char(9),char(0),
-char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),
-char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(72),char(0),char(5),char(0),
-char(70),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),
-char(73),char(0),char(5),char(0),char(71),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),
-char(8),char(0),char(-65),char(0),char(74),char(0),char(41),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),
-char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),char(13),char(0),char(-62),char(0),
-char(13),char(0),char(-61),char(0),char(13),char(0),char(-60),char(0),char(13),char(0),char(-59),char(0),char(13),char(0),char(-58),char(0),char(13),char(0),char(-57),char(0),
-char(13),char(0),char(-56),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),
-char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0),char(0),char(0),char(-80),char(0),char(13),char(0),char(-73),char(0),
-char(13),char(0),char(-72),char(0),char(13),char(0),char(-48),char(0),char(13),char(0),char(-47),char(0),char(13),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0),
-char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0),
-char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),
-char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-95),char(0),
-char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),char(14),char(0),char(-64),char(0),
-char(14),char(0),char(-63),char(0),char(14),char(0),char(-62),char(0),char(14),char(0),char(-61),char(0),char(14),char(0),char(-60),char(0),char(14),char(0),char(-59),char(0),
-char(14),char(0),char(-58),char(0),char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),
-char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0),
-char(0),char(0),char(-80),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(14),char(0),char(-48),char(0),char(14),char(0),char(-47),char(0),
-char(14),char(0),char(-46),char(0),char(14),char(0),char(-45),char(0),char(14),char(0),char(-44),char(0),char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0),
-char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0),char(14),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),
-char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),
-char(76),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(7),char(0),char(-75),char(0),
-char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),
-char(77),char(0),char(9),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(8),char(0),char(-75),char(0),
-char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),
-char(78),char(0),char(5),char(0),char(58),char(0),char(-95),char(0),char(13),char(0),char(-31),char(0),char(13),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0),
-char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-31),char(0),char(14),char(0),char(-30),char(0),
-char(8),char(0),char(-29),char(0),char(52),char(0),char(22),char(0),char(8),char(0),char(-28),char(0),char(8),char(0),char(-76),char(0),char(8),char(0),char(111),char(0),
-char(8),char(0),char(-27),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-26),char(0),char(8),char(0),char(-25),char(0),char(8),char(0),char(-24),char(0),
-char(8),char(0),char(-23),char(0),char(8),char(0),char(-22),char(0),char(8),char(0),char(-21),char(0),char(8),char(0),char(-20),char(0),char(8),char(0),char(-19),char(0),
-char(8),char(0),char(-18),char(0),char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),
-char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0),
-char(7),char(0),char(-28),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(113),char(0),
-char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0),
-char(7),char(0),char(-21),char(0),char(7),char(0),char(-20),char(0),char(7),char(0),char(-19),char(0),char(7),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),
-char(7),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),
-char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(80),char(0),char(4),char(0),char(7),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),
-char(7),char(0),char(-8),char(0),char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),char(80),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0),
-char(13),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(7),char(0),char(-120),char(0),
-char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(4),char(0),char(53),char(0),char(82),char(0),char(4),char(0),char(80),char(0),char(-7),char(0),
-char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(83),char(0),char(4),char(0),char(13),char(0),char(-2),char(0),
-char(80),char(0),char(-7),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),char(84),char(0),char(7),char(0),char(13),char(0),char(6),char(1),
-char(80),char(0),char(-7),char(0),char(4),char(0),char(7),char(1),char(7),char(0),char(8),char(1),char(7),char(0),char(9),char(1),char(7),char(0),char(10),char(1),
-char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(11),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(12),char(1),
-char(60),char(0),char(13),char(1),char(4),char(0),char(14),char(1),char(7),char(0),char(10),char(1),char(86),char(0),char(26),char(0),char(4),char(0),char(15),char(1),
-char(7),char(0),char(16),char(1),char(7),char(0),char(-76),char(0),char(7),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(19),char(1),
-char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),
-char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1),
-char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),
-char(4),char(0),char(35),char(1),char(4),char(0),char(36),char(1),char(4),char(0),char(37),char(1),char(4),char(0),char(38),char(1),char(4),char(0),char(118),char(0),
-char(87),char(0),char(12),char(0),char(17),char(0),char(39),char(1),char(17),char(0),char(40),char(1),char(17),char(0),char(41),char(1),char(13),char(0),char(42),char(1),
-char(13),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),char(4),char(0),char(46),char(1),char(4),char(0),char(47),char(1),
-char(4),char(0),char(48),char(1),char(7),char(0),char(8),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(27),char(0),char(19),char(0),char(49),char(1),
-char(17),char(0),char(50),char(1),char(17),char(0),char(51),char(1),char(13),char(0),char(42),char(1),char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1),
-char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1),
-char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1),
-char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(7),char(0),char(66),char(1),char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1),
-char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(4),char(0),char(72),char(1),char(4),char(0),char(73),char(1),
-char(4),char(0),char(74),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(75),char(1),char(9),char(0),char(76),char(1),char(13),char(0),char(77),char(1),
-char(7),char(0),char(78),char(1),char(7),char(0),char(-24),char(0),char(7),char(0),char(79),char(1),char(4),char(0),char(80),char(1),char(13),char(0),char(81),char(1),
-char(4),char(0),char(82),char(1),char(4),char(0),char(83),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0),
-char(50),char(0),char(126),char(0),char(87),char(0),char(85),char(1),char(80),char(0),char(86),char(1),char(81),char(0),char(87),char(1),char(82),char(0),char(88),char(1),
-char(83),char(0),char(89),char(1),char(84),char(0),char(90),char(1),char(85),char(0),char(91),char(1),char(88),char(0),char(92),char(1),char(89),char(0),char(93),char(1),
-char(4),char(0),char(94),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(97),char(1),
-char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(86),char(0),char(101),char(1),char(91),char(0),char(17),char(0),
-char(16),char(0),char(102),char(1),char(14),char(0),char(103),char(1),char(14),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1),
-char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),char(49),char(0),char(109),char(1),char(14),char(0),char(110),char(1),char(8),char(0),char(111),char(1),
-char(4),char(0),char(112),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(113),char(1),char(4),char(0),char(114),char(1),char(8),char(0),char(115),char(1),
-char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(92),char(0),char(17),char(0),char(15),char(0),char(102),char(1),char(13),char(0),char(103),char(1),
-char(13),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),
-char(50),char(0),char(109),char(1),char(13),char(0),char(110),char(1),char(4),char(0),char(113),char(1),char(7),char(0),char(111),char(1),char(4),char(0),char(112),char(1),
-char(4),char(0),char(84),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(7),char(0),char(117),char(1),char(4),char(0),char(114),char(1),
-char(93),char(0),char(8),char(0),char(0),char(0),char(118),char(1),char(91),char(0),char(88),char(1),char(49),char(0),char(119),char(1),char(20),char(0),char(120),char(1),
-char(14),char(0),char(121),char(1),char(4),char(0),char(95),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),
-char(0),char(0),char(118),char(1),char(92),char(0),char(88),char(1),char(50),char(0),char(119),char(1),char(19),char(0),char(120),char(1),char(13),char(0),char(121),char(1),
-char(7),char(0),char(122),char(1),char(4),char(0),char(95),char(1),};
+char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),
+char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),
+char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),
+char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),
+char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),
+char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),
+char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),
+char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),
+char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),
+char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),
+char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),
+char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),
+char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),
+char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),
+char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),
+char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),
+char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),
+char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),
+char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),
+char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),
+char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),
+char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(16),char(0),char(48),char(0),char(16),char(0),char(16),char(0),char(32),char(0),char(16),char(0),
+char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(96),char(0),
+char(-112),char(0),char(16),char(0),char(56),char(0),char(56),char(0),char(20),char(0),char(72),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),
+char(56),char(0),char(32),char(0),char(80),char(0),char(72),char(0),char(96),char(0),char(80),char(0),char(32),char(0),char(64),char(0),char(64),char(0),char(64),char(0),
+char(16),char(0),char(72),char(0),char(80),char(0),char(-32),char(1),char(16),char(1),char(-104),char(0),char(88),char(0),char(-72),char(0),char(104),char(0),char(-8),char(1),
+char(-80),char(3),char(8),char(0),char(64),char(0),char(64),char(0),char(0),char(0),char(80),char(0),char(96),char(0),char(-112),char(0),char(-128),char(0),char(104),char(1),
+char(-24),char(0),char(-104),char(1),char(-120),char(1),char(-32),char(0),char(8),char(1),char(-40),char(1),char(104),char(1),char(-128),char(2),char(-112),char(2),char(-64),char(4),
+char(-40),char(0),char(120),char(1),char(104),char(0),char(-104),char(0),char(16),char(0),char(104),char(0),char(24),char(0),char(40),char(0),char(104),char(0),char(96),char(0),
+char(104),char(0),char(-56),char(0),char(104),char(1),char(112),char(0),char(-32),char(1),char(-32),char(2),char(-120),char(1),char(-48),char(0),char(112),char(0),char(0),char(0),
+char(83),char(84),char(82),char(67),char(84),char(0),char(0),char(0),char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),
+char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),
+char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),
+char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),
+char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),
+char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),
+char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0),char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),
+char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0),char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),
+char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),
+char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),
+char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),
+char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),
+char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),
+char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),
+char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),
+char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),
+char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),
+char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),
+char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),
+char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0),char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),
+char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),
+char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),
+char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),
+char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),
+char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),
+char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0),char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),
+char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),
+char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),
+char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),
+char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),
+char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),
+char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),
+char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),
+char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),
+char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0),char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),
+char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),
+char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),
+char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0),char(29),char(0),char(47),char(0),
+char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0),char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(49),char(0),char(25),char(0),
+char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),
+char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0),char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),
+char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),
+char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),
+char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),
+char(50),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),
+char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0),char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),
+char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),
+char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),
+char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),
+char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(8),char(0),char(124),char(0),char(8),char(0),char(125),char(0),char(8),char(0),char(111),char(0),
+char(8),char(0),char(126),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(127),char(0),char(8),char(0),char(-128),char(0),char(8),char(0),char(-127),char(0),
+char(8),char(0),char(-126),char(0),char(8),char(0),char(-125),char(0),char(8),char(0),char(-124),char(0),char(8),char(0),char(-123),char(0),char(8),char(0),char(-122),char(0),
+char(8),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),char(8),char(0),char(-119),char(0),char(4),char(0),char(-118),char(0),char(4),char(0),char(-117),char(0),
+char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0),char(4),char(0),char(-114),char(0),char(0),char(0),char(37),char(0),char(52),char(0),char(22),char(0),
+char(7),char(0),char(124),char(0),char(7),char(0),char(125),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(126),char(0),char(7),char(0),char(113),char(0),
+char(7),char(0),char(127),char(0),char(7),char(0),char(-128),char(0),char(7),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(7),char(0),char(-125),char(0),
+char(7),char(0),char(-124),char(0),char(7),char(0),char(-123),char(0),char(7),char(0),char(-122),char(0),char(7),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),
+char(7),char(0),char(-119),char(0),char(4),char(0),char(-118),char(0),char(4),char(0),char(-117),char(0),char(4),char(0),char(-116),char(0),char(4),char(0),char(-115),char(0),
+char(4),char(0),char(-114),char(0),char(0),char(0),char(37),char(0),char(53),char(0),char(2),char(0),char(51),char(0),char(-113),char(0),char(14),char(0),char(-112),char(0),
+char(54),char(0),char(2),char(0),char(52),char(0),char(-113),char(0),char(13),char(0),char(-112),char(0),char(55),char(0),char(21),char(0),char(50),char(0),char(-111),char(0),
+char(17),char(0),char(-110),char(0),char(13),char(0),char(-109),char(0),char(13),char(0),char(-108),char(0),char(13),char(0),char(-107),char(0),char(13),char(0),char(-106),char(0),
+char(13),char(0),char(-112),char(0),char(13),char(0),char(-105),char(0),char(13),char(0),char(-104),char(0),char(13),char(0),char(-103),char(0),char(13),char(0),char(-102),char(0),
+char(7),char(0),char(-101),char(0),char(7),char(0),char(-100),char(0),char(7),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0),char(7),char(0),char(-97),char(0),
+char(7),char(0),char(-96),char(0),char(7),char(0),char(-95),char(0),char(7),char(0),char(-94),char(0),char(7),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),
+char(56),char(0),char(22),char(0),char(49),char(0),char(-111),char(0),char(18),char(0),char(-110),char(0),char(14),char(0),char(-109),char(0),char(14),char(0),char(-108),char(0),
+char(14),char(0),char(-107),char(0),char(14),char(0),char(-106),char(0),char(14),char(0),char(-112),char(0),char(14),char(0),char(-105),char(0),char(14),char(0),char(-104),char(0),
+char(14),char(0),char(-103),char(0),char(14),char(0),char(-102),char(0),char(8),char(0),char(-101),char(0),char(8),char(0),char(-100),char(0),char(8),char(0),char(-99),char(0),
+char(8),char(0),char(-98),char(0),char(8),char(0),char(-97),char(0),char(8),char(0),char(-96),char(0),char(8),char(0),char(-95),char(0),char(8),char(0),char(-94),char(0),
+char(8),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(0),char(0),char(37),char(0),char(57),char(0),char(2),char(0),char(4),char(0),char(-91),char(0),
+char(4),char(0),char(-90),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-89),char(0),char(55),char(0),char(-88),char(0),char(0),char(0),char(35),char(0),
+char(4),char(0),char(-87),char(0),char(4),char(0),char(-86),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),
+char(7),char(0),char(-82),char(0),char(4),char(0),char(-81),char(0),char(4),char(0),char(-80),char(0),char(7),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),
+char(59),char(0),char(13),char(0),char(60),char(0),char(-89),char(0),char(60),char(0),char(-88),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-87),char(0),
+char(4),char(0),char(-86),char(0),char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0),
+char(4),char(0),char(-81),char(0),char(4),char(0),char(-80),char(0),char(7),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(61),char(0),char(14),char(0),
+char(56),char(0),char(-89),char(0),char(56),char(0),char(-88),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-87),char(0),char(4),char(0),char(-86),char(0),
+char(4),char(0),char(-85),char(0),char(4),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(4),char(0),char(-81),char(0),
+char(4),char(0),char(-80),char(0),char(8),char(0),char(-79),char(0),char(4),char(0),char(-78),char(0),char(0),char(0),char(-77),char(0),char(62),char(0),char(3),char(0),
+char(59),char(0),char(-76),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),char(63),char(0),char(3),char(0),char(61),char(0),char(-76),char(0),
+char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),char(64),char(0),char(3),char(0),char(59),char(0),char(-76),char(0),char(14),char(0),char(-75),char(0),
+char(14),char(0),char(-74),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-76),char(0),char(20),char(0),char(-73),char(0),char(20),char(0),char(-72),char(0),
+char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(7),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),
+char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),
+char(66),char(0),char(13),char(0),char(59),char(0),char(-76),char(0),char(19),char(0),char(-73),char(0),char(19),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),
+char(4),char(0),char(-70),char(0),char(4),char(0),char(-69),char(0),char(7),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),
+char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(67),char(0),char(14),char(0),
+char(61),char(0),char(-76),char(0),char(20),char(0),char(-73),char(0),char(20),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),
+char(4),char(0),char(-69),char(0),char(8),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(-65),char(0),
+char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(0),char(0),char(-61),char(0),char(68),char(0),char(10),char(0),
+char(61),char(0),char(-76),char(0),char(20),char(0),char(-73),char(0),char(20),char(0),char(-72),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(-59),char(0),
+char(8),char(0),char(-58),char(0),char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(125),char(0),
+char(69),char(0),char(11),char(0),char(59),char(0),char(-76),char(0),char(19),char(0),char(-73),char(0),char(19),char(0),char(-72),char(0),char(7),char(0),char(-60),char(0),
+char(7),char(0),char(-59),char(0),char(7),char(0),char(-58),char(0),char(7),char(0),char(-64),char(0),char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),
+char(7),char(0),char(125),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0),char(59),char(0),char(-76),char(0),char(19),char(0),char(-73),char(0),
+char(19),char(0),char(-72),char(0),char(13),char(0),char(-57),char(0),char(13),char(0),char(-56),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),
+char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(71),char(0),char(9),char(0),char(61),char(0),char(-76),char(0),char(20),char(0),char(-73),char(0),
+char(20),char(0),char(-72),char(0),char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0),
+char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(72),char(0),char(5),char(0),char(70),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),
+char(7),char(0),char(-49),char(0),char(7),char(0),char(-48),char(0),char(7),char(0),char(-47),char(0),char(73),char(0),char(5),char(0),char(71),char(0),char(-51),char(0),
+char(4),char(0),char(-50),char(0),char(8),char(0),char(-49),char(0),char(8),char(0),char(-48),char(0),char(8),char(0),char(-47),char(0),char(74),char(0),char(41),char(0),
+char(59),char(0),char(-76),char(0),char(19),char(0),char(-73),char(0),char(19),char(0),char(-72),char(0),char(13),char(0),char(-57),char(0),char(13),char(0),char(-56),char(0),
+char(13),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0),char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),
+char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0),char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(13),char(0),char(-37),char(0),
+char(13),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0),
+char(0),char(0),char(-31),char(0),char(0),char(0),char(-61),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(13),char(0),char(-30),char(0),
+char(13),char(0),char(-29),char(0),char(13),char(0),char(-28),char(0),char(13),char(0),char(-27),char(0),char(13),char(0),char(-26),char(0),char(13),char(0),char(-25),char(0),
+char(13),char(0),char(-24),char(0),char(13),char(0),char(-23),char(0),char(13),char(0),char(-22),char(0),char(13),char(0),char(-21),char(0),char(13),char(0),char(-20),char(0),
+char(0),char(0),char(-19),char(0),char(0),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0),char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0),
+char(4),char(0),char(-14),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-76),char(0),char(20),char(0),char(-73),char(0),char(20),char(0),char(-72),char(0),
+char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),char(14),char(0),char(-46),char(0),char(14),char(0),char(-45),char(0),char(14),char(0),char(-44),char(0),
+char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0),char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0),
+char(14),char(0),char(-38),char(0),char(14),char(0),char(-37),char(0),char(14),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),
+char(0),char(0),char(-33),char(0),char(0),char(0),char(-32),char(0),char(0),char(0),char(-31),char(0),char(0),char(0),char(-61),char(0),char(14),char(0),char(-55),char(0),
+char(14),char(0),char(-54),char(0),char(14),char(0),char(-30),char(0),char(14),char(0),char(-29),char(0),char(14),char(0),char(-28),char(0),char(14),char(0),char(-27),char(0),
+char(14),char(0),char(-26),char(0),char(14),char(0),char(-25),char(0),char(14),char(0),char(-24),char(0),char(14),char(0),char(-23),char(0),char(14),char(0),char(-22),char(0),
+char(14),char(0),char(-21),char(0),char(14),char(0),char(-20),char(0),char(0),char(0),char(-19),char(0),char(0),char(0),char(-18),char(0),char(0),char(0),char(-17),char(0),
+char(0),char(0),char(-16),char(0),char(0),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(76),char(0),char(9),char(0),char(59),char(0),char(-76),char(0),
+char(19),char(0),char(-73),char(0),char(19),char(0),char(-72),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),char(7),char(0),char(-55),char(0),
+char(7),char(0),char(-54),char(0),char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(77),char(0),char(9),char(0),char(61),char(0),char(-76),char(0),
+char(20),char(0),char(-73),char(0),char(20),char(0),char(-72),char(0),char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0),char(8),char(0),char(-55),char(0),
+char(8),char(0),char(-54),char(0),char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(78),char(0),char(5),char(0),char(58),char(0),char(-76),char(0),
+char(13),char(0),char(-13),char(0),char(13),char(0),char(-12),char(0),char(7),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),
+char(61),char(0),char(-76),char(0),char(14),char(0),char(-13),char(0),char(14),char(0),char(-12),char(0),char(8),char(0),char(-11),char(0),char(80),char(0),char(4),char(0),
+char(7),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),char(7),char(0),char(-8),char(0),char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),
+char(80),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0),char(13),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),
+char(13),char(0),char(-2),char(0),char(7),char(0),char(-101),char(0),char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(4),char(0),char(53),char(0),
+char(82),char(0),char(4),char(0),char(80),char(0),char(-7),char(0),char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(4),char(0),char(3),char(1),
+char(83),char(0),char(4),char(0),char(13),char(0),char(-2),char(0),char(80),char(0),char(-7),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),
+char(84),char(0),char(7),char(0),char(13),char(0),char(6),char(1),char(80),char(0),char(-7),char(0),char(4),char(0),char(7),char(1),char(7),char(0),char(8),char(1),
+char(7),char(0),char(9),char(1),char(7),char(0),char(10),char(1),char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(11),char(1),
+char(13),char(0),char(9),char(1),char(13),char(0),char(12),char(1),char(60),char(0),char(13),char(1),char(4),char(0),char(14),char(1),char(7),char(0),char(10),char(1),
+char(86),char(0),char(26),char(0),char(4),char(0),char(15),char(1),char(7),char(0),char(16),char(1),char(7),char(0),char(125),char(0),char(7),char(0),char(17),char(1),
+char(7),char(0),char(18),char(1),char(7),char(0),char(19),char(1),char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1),
+char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1),
+char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1),char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1),
+char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1),char(4),char(0),char(35),char(1),char(4),char(0),char(36),char(1),char(4),char(0),char(37),char(1),
+char(4),char(0),char(38),char(1),char(4),char(0),char(118),char(0),char(87),char(0),char(12),char(0),char(17),char(0),char(39),char(1),char(17),char(0),char(40),char(1),
+char(17),char(0),char(41),char(1),char(13),char(0),char(42),char(1),char(13),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),
+char(4),char(0),char(46),char(1),char(4),char(0),char(47),char(1),char(4),char(0),char(48),char(1),char(7),char(0),char(8),char(1),char(4),char(0),char(53),char(0),
+char(88),char(0),char(27),char(0),char(19),char(0),char(49),char(1),char(17),char(0),char(50),char(1),char(17),char(0),char(51),char(1),char(13),char(0),char(42),char(1),
+char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1),char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),
+char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1),char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(61),char(1),
+char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1),char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(7),char(0),char(66),char(1),
+char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1),char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),
+char(4),char(0),char(72),char(1),char(4),char(0),char(73),char(1),char(4),char(0),char(74),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(75),char(1),
+char(9),char(0),char(76),char(1),char(13),char(0),char(77),char(1),char(7),char(0),char(78),char(1),char(7),char(0),char(-127),char(0),char(7),char(0),char(79),char(1),
+char(4),char(0),char(80),char(1),char(13),char(0),char(81),char(1),char(4),char(0),char(82),char(1),char(4),char(0),char(83),char(1),char(4),char(0),char(84),char(1),
+char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0),char(50),char(0),char(-111),char(0),char(87),char(0),char(85),char(1),char(80),char(0),char(86),char(1),
+char(81),char(0),char(87),char(1),char(82),char(0),char(88),char(1),char(83),char(0),char(89),char(1),char(84),char(0),char(90),char(1),char(85),char(0),char(91),char(1),
+char(88),char(0),char(92),char(1),char(89),char(0),char(93),char(1),char(4),char(0),char(94),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(95),char(1),
+char(4),char(0),char(96),char(1),char(4),char(0),char(97),char(1),char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),
+char(86),char(0),char(101),char(1),char(91),char(0),char(20),char(0),char(16),char(0),char(102),char(1),char(14),char(0),char(103),char(1),char(14),char(0),char(104),char(1),
+char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1),char(14),char(0),char(107),char(1),char(8),char(0),char(108),char(1),char(4),char(0),char(109),char(1),
+char(4),char(0),char(84),char(1),char(4),char(0),char(110),char(1),char(4),char(0),char(111),char(1),char(8),char(0),char(112),char(1),char(8),char(0),char(113),char(1),
+char(8),char(0),char(114),char(1),char(8),char(0),char(115),char(1),char(8),char(0),char(116),char(1),char(0),char(0),char(117),char(1),char(0),char(0),char(118),char(1),
+char(49),char(0),char(119),char(1),char(0),char(0),char(120),char(1),char(92),char(0),char(20),char(0),char(15),char(0),char(102),char(1),char(13),char(0),char(103),char(1),
+char(13),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(13),char(0),char(107),char(1),char(4),char(0),char(110),char(1),
+char(7),char(0),char(108),char(1),char(4),char(0),char(109),char(1),char(4),char(0),char(84),char(1),char(7),char(0),char(112),char(1),char(7),char(0),char(113),char(1),
+char(7),char(0),char(114),char(1),char(4),char(0),char(111),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(0),char(0),char(117),char(1),
+char(0),char(0),char(118),char(1),char(50),char(0),char(119),char(1),char(0),char(0),char(120),char(1),char(93),char(0),char(9),char(0),char(20),char(0),char(121),char(1),
+char(14),char(0),char(122),char(1),char(8),char(0),char(123),char(1),char(0),char(0),char(124),char(1),char(91),char(0),char(88),char(1),char(49),char(0),char(125),char(1),
+char(0),char(0),char(120),char(1),char(4),char(0),char(95),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0),char(0),char(0),char(124),char(1),
+char(92),char(0),char(88),char(1),char(50),char(0),char(125),char(1),char(19),char(0),char(121),char(1),char(13),char(0),char(122),char(1),char(7),char(0),char(123),char(1),
+char(4),char(0),char(95),char(1),};
int sBulletDNAlen64= sizeof(sBulletDNAstr64);
diff --git a/src/LinearMath/btSerializer.h b/src/LinearMath/btSerializer.h
index 033895b..454068d 100644
--- a/src/LinearMath/btSerializer.h
+++ b/src/LinearMath/btSerializer.h
@@ -484,7 +484,7 @@ public:
buffer[9] = '2';
buffer[10] = '8';
- buffer[11] = '4';
+ buffer[11] = '5';
}
diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h
index 839b19c..4876700 100644
--- a/src/LinearMath/btVector3.h
+++ b/src/LinearMath/btVector3.h
@@ -267,10 +267,20 @@ public:
/**@brief Return the norm (length) of the vector */
SIMD_FORCE_INLINE btScalar norm() const
- {
+ {
return length();
}
+ /**@brief Return the norm (length) of the vector */
+ SIMD_FORCE_INLINE btScalar safeNorm() const
+ {
+ btScalar d = length2();
+ //workaround for some clang/gcc issue of sqrtf(tiny number) = -INF
+ if (d>SIMD_EPSILON)
+ return btSqrt(d);
+ return btScalar(0);
+ }
+
/**@brief Return the distance squared between the ends of this and another vector
* This is symantically treating the vector like a point */
SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index e8b0842..2b08544 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,7 +1,7 @@
IF(BUILD_BULLET3)
#SUBDIRS( TestBullet3OpenCL )
- SUBDIRS( InverseDynamics )
+ SUBDIRS( InverseDynamics SharedMemory )
ENDIF(BUILD_BULLET3)
-SUBDIRS( gtest-1.7.0 collision BulletDynamics/pendulum )
+SUBDIRS( gtest-1.7.0 collision BulletDynamics/pendulum )
diff --git a/test/InverseDynamics/CMakeLists.txt b/test/InverseDynamics/CMakeLists.txt
index 89d70c0..e31ea6e 100644
--- a/test/InverseDynamics/CMakeLists.txt
+++ b/test/InverseDynamics/CMakeLists.txt
@@ -20,14 +20,112 @@ IF (NOT WIN32)
ENDIF()
- ADD_EXECUTABLE(Test_BulletInverseDynamics
- test_invdyn_kinematics.cpp
+ ADD_EXECUTABLE(Test_BulletInverseDynamicsJacobian
+ test_invdyn_jacobian.cpp
)
+ADD_TEST(Test_BulletInverseDynamicsJacobian_PASS Test_BulletInverseDynamicsJacobian)
+
+IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+ SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES DEBUG_POSTFIX "_Debug")
+ SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
+ SET_TARGET_PROPERTIES(Test_BulletInverseDynamicsJacobian PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
+ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+
+INCLUDE_DIRECTORIES(
+ .
+ ../../src
+ ../gtest-1.7.0/include
+ ../../Extras/InverseDynamics
+)
+
+
+#ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1)
+ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
+
+LINK_LIBRARIES(
+BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest
+)
+IF (NOT WIN32)
+ LINK_LIBRARIES( pthread )
+ENDIF()
+
+
+ ADD_EXECUTABLE(Test_BulletInverseForwardDynamics
+ test_invdyn_bullet.cpp
+ ../../examples/Utils/b3ResourcePath.cpp
+ ../../examples/Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h
+ ../../examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h
+ ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
+ ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
+ ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+ ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
+ ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
+ ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
+ ../../examples/Importers/ImportURDFDemo/UrdfParser.h
+ ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+ ../../examples/Importers/ImportURDFDemo/URDF2Bullet.h
+ ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
+ ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
+ ../../examples/Utils/b3Clock.cpp
+ ../../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp
+ ../../Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp
+ ../../Extras/Serialize/BulletFileLoader/bChunk.cpp
+ ../../Extras/Serialize/BulletFileLoader/bFile.cpp
+ ../../Extras/Serialize/BulletFileLoader/bDNA.cpp
+ ../../Extras/Serialize/BulletFileLoader/btBulletFile.cpp
+ ../../examples/Importers/ImportURDFDemo/URDFImporterInterface.h
+ ../../examples/Importers/ImportURDFDemo/URDFJointTypes.h
+ ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
+ ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
+ ../../examples/Importers/ImportSTLDemo/ImportSTLSetup.h
+ ../../examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
+ ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
+ ../../examples/Importers/ImportColladaDemo/ColladaGraphicsInstance.h
+ ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
+
+ )
+
+ADD_TEST(Test_BulletInverseForwardDynamics_PASS Test_BulletInverseForwardDynamics)
+
+IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+ SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES DEBUG_POSTFIX "_Debug")
+ SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
+ SET_TARGET_PROPERTIES(Test_BulletInverseForwardDynamics PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
+ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+
+INCLUDE_DIRECTORIES(
+ .
+ ../../src
+ ../gtest-1.7.0/include
+ ../../Extras/InverseDynamics
+)
+
+
+#ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1)
+ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
+
+LINK_LIBRARIES(
+ BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest
+)
+IF (NOT WIN32)
+ LINK_LIBRARIES( pthread )
+ENDIF()
+
+
+ ADD_EXECUTABLE(Test_BulletInverseDynamics
+ test_invdyn_kinematics.cpp
+ )
+
ADD_TEST(Test_BulletInverseDynamics_PASS Test_BulletInverseDynamics)
IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
- SET_TARGET_PROPERTIES(Test_Collision PROPERTIES DEBUG_POSTFIX "_Debug")
- SET_TARGET_PROPERTIES(Test_Collision PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
- SET_TARGET_PROPERTIES(Test_Collision PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
+ SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES DEBUG_POSTFIX "_Debug")
+ SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
+ SET_TARGET_PROPERTIES(Test_BulletInverseDynamics PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+
diff --git a/test/InverseDynamics/premake4.lua b/test/InverseDynamics/premake4.lua
index b16efba..1892a02 100644
--- a/test/InverseDynamics/premake4.lua
+++ b/test/InverseDynamics/premake4.lua
@@ -37,7 +37,8 @@
- project "Test_InverseForwardDynamics"
+ project "Test_InverseDynamicsJacobian"
+
kind "ConsoleApp"
@@ -50,6 +51,38 @@
".",
"../../src",
"../../examples/InverseDynamics",
+ "../../Extras/InverseDynamics",
+ "../gtest-1.7.0/include"
+
+ }
+
+
+ if os.is("Windows") then
+ --see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012
+ defines {"_VARIADIC_MAX=10"}
+ end
+
+ links {"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","LinearMath", "gtest"}
+
+ files {
+ "test_invdyn_jacobian.cpp",
+ }
+
+ if os.is("Linux") then
+ links {"pthread"}
+ end
+
+ project "Test_InverseForwardDynamics"
+ kind "ConsoleApp"
+-- defines { }
+
+
+
+ includedirs
+ {
+ ".",
+ "../../src",
+ "../../examples/InverseDynamics",
"../../examples/ThirdPartyLibs",
"../../Extras/InverseDynamics",
"../gtest-1.7.0/include"
@@ -73,14 +106,13 @@
"../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h",
"../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
"../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h",
- "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
- "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
- "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h",
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
"../../examples/Importers/ImportURDFDemo/UrdfParser.h",
"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../../examples/Importers/ImportURDFDemo/URDF2Bullet.h",
+ "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
"../../examples/Utils/b3Clock.cpp",
"../../Extras/Serialize/BulletWorldImporter/*",
"../../Extras/Serialize/BulletFileLoader/*",
diff --git a/test/InverseDynamics/test_invdyn_bullet.cpp b/test/InverseDynamics/test_invdyn_bullet.cpp
index 1ac0956..63597a5 100644
--- a/test/InverseDynamics/test_invdyn_bullet.cpp
+++ b/test/InverseDynamics/test_invdyn_bullet.cpp
@@ -16,12 +16,12 @@
#include <BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h>
#include <BulletDynamics/Featherstone/btMultiBodyPoint2Point.h>
#include <BulletDynamics/Featherstone/btMultiBodyLinkCollider.h>
-#include <../CommonInterfaces/CommonGUIHelperInterface.h>
#include <gtest/gtest.h>
-#include <../Importers/ImportURDFDemo/BulletUrdfImporter.h>
-#include <../Importers/ImportURDFDemo/URDF2Bullet.h>
-#include <../Importers/ImportURDFDemo/MyMultiBodyCreator.h>
-#include <../Importers/ImportURDFDemo/URDF2Bullet.h>
+#include "../../examples/CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h"
+#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h"
+#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../../examples/Utils/b3ResourcePath.h"
#include <invdyn_bullet_comparison.hpp>
#include <btMultiBodyFromURDF.hpp>
diff --git a/test/InverseDynamics/test_invdyn_jacobian.cpp b/test/InverseDynamics/test_invdyn_jacobian.cpp
new file mode 100644
index 0000000..01907b2
--- /dev/null
+++ b/test/InverseDynamics/test_invdyn_jacobian.cpp
@@ -0,0 +1,326 @@
+// Test of kinematic consistency: check if finite differences of velocities, accelerations
+// match positions
+
+#include <cmath>
+#include <cstdio>
+#include <cstdlib>
+#include <iostream>
+
+#include <gtest/gtest.h>
+
+#include "Bullet3Common/b3Random.h"
+
+#include "CloneTreeCreator.hpp"
+#include "CoilCreator.hpp"
+#include "DillCreator.hpp"
+#include "RandomTreeCreator.hpp"
+#include "BulletInverseDynamics/MultiBodyTree.hpp"
+#include "MultiBodyTreeDebugGraph.hpp"
+
+using namespace btInverseDynamics;
+
+#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
+// minimal smart pointer to make this work for c++2003
+template <typename T>
+class ptr {
+ ptr();
+ ptr(const ptr&);
+public:
+ ptr(T* p) : m_p(p) {};
+ ~ptr() { delete m_p; }
+ T& operator*() { return *m_p; }
+ T* operator->() { return m_p; }
+ T*get() {return m_p;}
+ const T*get() const {return m_p;}
+ friend bool operator==(const ptr<T>& lhs, const ptr<T>& rhs) { return rhs.m_p == lhs.m_p; }
+ friend bool operator!=(const ptr<T>& lhs, const ptr<T>& rhs) { return !(rhs.m_p == lhs.m_p);
+}
+
+private:
+ T* m_p;
+};
+
+void calculateDotJacUError(const MultiBodyTreeCreator& creator, const int nloops,
+ double* max_error) {
+ // tree1 is used as reference to compute dot(Jacobian)*u from acceleration(dot(u)=0)
+ ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
+ ASSERT_TRUE(0x0 != tree1);
+ CloneTreeCreator clone(tree1.get());
+ // tree2 is used to compute dot(Jacobian)*u using the calculateJacobian function
+ ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
+ ASSERT_TRUE(0x0 != tree2);
+
+ const int ndofs = tree1->numDoFs();
+ const int nbodies = tree1->numBodies();
+ if (ndofs <= 0) {
+ *max_error = 0;
+ return;
+ }
+
+ vecx q(ndofs);
+ vecx u(ndofs);
+ vecx dot_u(ndofs);
+ vecx zero(ndofs);
+ setZero(zero);
+
+ double max_lin_error = 0;
+ double max_ang_error = 0;
+
+ for (int loop = 0; loop < nloops; loop++) {
+ for (int i = 0; i < q.size(); i++) {
+ q(i) = b3RandRange(-B3_PI, B3_PI);
+ u(i) = b3RandRange(-B3_PI, B3_PI);
+ }
+
+ EXPECT_EQ(0, tree1->calculateKinematics(q, u, zero));
+ EXPECT_EQ(0, tree2->calculatePositionAndVelocityKinematics(q, u));
+ EXPECT_EQ(0, tree2->calculateJacobians(q, u));
+
+ for (size_t idx = 0; idx < nbodies; idx++) {
+ vec3 tmp1, tmp2;
+ vec3 diff;
+ EXPECT_EQ(0, tree1->getBodyLinearAcceleration(idx, &tmp1));
+ EXPECT_EQ(0, tree2->getBodyDotJacobianTransU(idx, &tmp2));
+ diff = tmp1 - tmp2;
+ double lin_error = maxAbs(diff);
+
+ if (lin_error > max_lin_error) {
+ max_lin_error = lin_error;
+ }
+
+ EXPECT_EQ(0, tree1->getBodyAngularAcceleration(idx, &tmp1));
+ EXPECT_EQ(0, tree2->getBodyDotJacobianRotU(idx, &tmp2));
+ diff = tmp1 - tmp2;
+ double ang_error = maxAbs(diff);
+ if (ang_error > max_ang_error) {
+ max_ang_error = ang_error;
+ }
+ }
+ }
+ *max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
+}
+
+void calculateJacobianError(const MultiBodyTreeCreator& creator, const int nloops,
+ double* max_error) {
+ // tree1 is used as reference to compute the Jacobian from velocities with unit u vectors.
+ ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
+ ASSERT_TRUE(0x0 != tree1);
+ // tree2 is used to compute the Jacobians using the calculateJacobian function
+ CloneTreeCreator clone(tree1.get());
+ ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
+ ASSERT_TRUE(0x0 != tree2);
+
+ const int ndofs = tree1->numDoFs();
+ const int nbodies = tree1->numBodies();
+
+ if (ndofs <= 0) {
+ *max_error = 0;
+ return;
+ }
+
+ vecx q(ndofs);
+ vecx zero(ndofs);
+ setZero(zero);
+ vecx one(ndofs);
+
+ double max_lin_error = 0;
+ double max_ang_error = 0;
+
+ for (int loop = 0; loop < nloops; loop++) {
+ for (int i = 0; i < q.size(); i++) {
+ q(i) = b3RandRange(-B3_PI, B3_PI);
+ }
+
+ EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
+ EXPECT_EQ(0, tree2->calculateJacobians(q));
+
+ for (size_t idx = 0; idx < nbodies; idx++) {
+ mat3x ref_jac_r(3, ndofs);
+ mat3x ref_jac_t(3, ndofs);
+ ref_jac_r.setZero();
+ ref_jac_t.setZero();
+ // this re-computes all jacobians for every body ...
+ // but avoids std::vector<eigen matrix> issues
+ for (int col = 0; col < ndofs; col++) {
+ setZero(one);
+ one(col) = 1.0;
+ EXPECT_EQ(0, tree1->calculatePositionAndVelocityKinematics(q, one));
+ vec3 vel, omg;
+ EXPECT_EQ(0, tree1->getBodyLinearVelocity(idx, &vel));
+ EXPECT_EQ(0, tree1->getBodyAngularVelocity(idx, &omg));
+ setMat3xElem(0, col, omg(0), &ref_jac_r);
+ setMat3xElem(1, col, omg(1), &ref_jac_r);
+ setMat3xElem(2, col, omg(2), &ref_jac_r);
+ setMat3xElem(0, col, vel(0), &ref_jac_t);
+ setMat3xElem(1, col, vel(1), &ref_jac_t);
+ setMat3xElem(2, col, vel(2), &ref_jac_t);
+ }
+
+ mat3x jac_r(3, ndofs);
+ mat3x jac_t(3, ndofs);
+ mat3x diff(3, ndofs);
+
+ EXPECT_EQ(0, tree2->getBodyJacobianTrans(idx, &jac_t));
+ EXPECT_EQ(0, tree2->getBodyJacobianRot(idx, &jac_r));
+ sub(ref_jac_t,jac_t,&diff);
+ double lin_error = maxAbsMat3x(diff);
+ if (lin_error > max_lin_error) {
+ max_lin_error = lin_error;
+ }
+ sub(ref_jac_r, jac_r,&diff);
+ double ang_error = maxAbsMat3x(diff);
+ if (ang_error > max_ang_error) {
+ max_ang_error = ang_error;
+ }
+ }
+ }
+ *max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
+}
+
+void calculateVelocityJacobianError(const MultiBodyTreeCreator& creator, const int nloops,
+ double* max_error) {
+ // tree1 is used as reference to compute the velocities directly
+ ptr<MultiBodyTree> tree1(CreateMultiBodyTree(creator));
+ ASSERT_TRUE(0x0 != tree1);
+ // tree2 is used to compute the velocities via jacobians
+ CloneTreeCreator clone(tree1.get());
+ ptr<MultiBodyTree> tree2(CreateMultiBodyTree(clone));
+ ASSERT_TRUE(0x0 != tree2);
+
+ const int ndofs = tree1->numDoFs();
+ const int nbodies = tree1->numBodies();
+
+ if (ndofs <= 0) {
+ *max_error = 0;
+ return;
+ }
+
+ vecx q(ndofs);
+ vecx u(ndofs);
+
+ double max_lin_error = 0;
+ double max_ang_error = 0;
+
+ for (int loop = 0; loop < nloops; loop++) {
+ for (int i = 0; i < q.size(); i++) {
+ q(i) = b3RandRange(-B3_PI, B3_PI);
+ u(i) = b3RandRange(-B3_PI, B3_PI);
+ }
+
+ EXPECT_EQ(0, tree1->calculatePositionAndVelocityKinematics(q, u));
+ EXPECT_EQ(0, tree2->calculatePositionKinematics(q));
+ EXPECT_EQ(0, tree2->calculateJacobians(q));
+
+ for (size_t idx = 0; idx < nbodies; idx++) {
+ vec3 vel1;
+ vec3 omg1;
+ vec3 vel2;
+ vec3 omg2;
+ mat3x jac_r2(3, ndofs);
+ mat3x jac_t2(3, ndofs);
+
+ EXPECT_EQ(0, tree1->getBodyLinearVelocity(idx, &vel1));
+ EXPECT_EQ(0, tree1->getBodyAngularVelocity(idx, &omg1));
+ EXPECT_EQ(0, tree2->getBodyJacobianTrans(idx, &jac_t2));
+ EXPECT_EQ(0, tree2->getBodyJacobianRot(idx, &jac_r2));
+ omg2 = jac_r2 * u;
+ vel2 = jac_t2 * u;
+
+ double lin_error = maxAbs(vel1 - vel2);
+ if (lin_error > max_lin_error) {
+ max_lin_error = lin_error;
+ }
+ double ang_error = maxAbs(omg1 - omg2);
+ if (ang_error > max_ang_error) {
+ max_ang_error = ang_error;
+ }
+ }
+ }
+ *max_error = max_ang_error > max_lin_error ? max_ang_error : max_lin_error;
+}
+
+// test nonlinear terms: dot(Jacobian)*u (linear and angular acceleration for dot_u ==0)
+// from Jacobian calculation method and pseudo-numerically using via the kinematics method.
+TEST(InvDynJacobians, JacDotJacU) {
+ const int kNumLevels = 5;
+#ifdef B3_USE_DOUBLE_PRECISION
+ const double kMaxError = 1e-12;
+#else
+ const double kMaxError = 5e-5;
+#endif
+ const int kNumLoops = 20;
+ for (int level = 0; level < kNumLevels; level++) {
+ const int nbodies = BT_ID_POW(2, level);
+ CoilCreator coil(nbodies);
+ double error;
+ calculateDotJacUError(coil, kNumLoops, &error);
+ EXPECT_GT(kMaxError, error);
+ DillCreator dill(level);
+ calculateDotJacUError(dill, kNumLoops, &error);
+ EXPECT_GT(kMaxError, error);
+ }
+
+ const int kRandomLoops = 100;
+ const int kMaxRandomBodies = 128;
+ for (int loop = 0; loop < kRandomLoops; loop++) {
+ RandomTreeCreator random(kMaxRandomBodies);
+ double error;
+ calculateDotJacUError(random, kNumLoops, &error);
+ EXPECT_GT(kMaxError, error);
+ }
+}
+
+// Jacobians: linear and angular acceleration for dot_u ==0
+// from Jacobian calculation method and pseudo-numerically using via the kinematics method.
+TEST(InvDynJacobians, Jacobians) {
+ const int kNumLevels = 5;
+#ifdef B3_USE_DOUBLE_PRECISION
+ const double kMaxError = 1e-12;
+#else
+ const double kMaxError = 5e-5;
+#endif
+ const int kNumLoops = 20;
+ for (int level = 0; level < kNumLevels; level++) {
+ const int nbodies = BT_ID_POW(2, level);
+ CoilCreator coil(nbodies);
+ double error;
+ calculateJacobianError(coil, kNumLoops, &error);
+ EXPECT_GT(kMaxError, error);
+ DillCreator dill(level);
+ calculateDotJacUError(dill, kNumLoops, &error);
+ EXPECT_GT(kMaxError, error);
+ }
+ const int kRandomLoops = 20;
+ const int kMaxRandomBodies = 16;
+ for (int loop = 0; loop < kRandomLoops; loop++) {
+ RandomTreeCreator random(kMaxRandomBodies);
+ double error;
+ calculateJacobianError(random, kNumLoops, &error);
+ EXPECT_GT(kMaxError, error);
+ }
+}
+
+// test for jacobian*u == velocity
+TEST(InvDynJacobians, VelocitiesFromJacobians) {
+ const int kRandomLoops = 20;
+ const int kMaxRandomBodies = 16;
+ const int kNumLoops = 20;
+#ifdef B3_USE_DOUBLE_PRECISION
+ const double kMaxError = 1e-12;
+#else
+ const double kMaxError = 5e-5;
+#endif
+ for (int loop = 0; loop < kRandomLoops; loop++) {
+ RandomTreeCreator random(kMaxRandomBodies);
+ double error;
+ calculateVelocityJacobianError(random, kNumLoops, &error);
+ EXPECT_GT(kMaxError, error);
+ }
+}
+#endif
+
+int main(int argc, char** argv) {
+ b3Srand(1234);
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt
new file mode 100644
index 0000000..496ca50
--- /dev/null
+++ b/test/SharedMemory/CMakeLists.txt
@@ -0,0 +1,87 @@
+
+INCLUDE_DIRECTORIES(
+ .
+ ../../src
+ ../gtest-1.7.0/include
+ ../../examples
+ ../../examples/ThirdPartyLibs
+)
+
+
+#ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1)
+ADD_DEFINITIONS(-DPHYSICS_LOOP_BACK -DPHYSICS_SERVER_DIRECT -DENABLE_GTEST -D_VARIADIC_MAX=10)
+
+
+LINK_LIBRARIES(
+ BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BussIK
+)
+
+IF (NOT WIN32)
+ LINK_LIBRARIES( pthread )
+ENDIF()
+
+ ADD_EXECUTABLE(Test_PhysicsClientServer
+ gtestwrap.cpp
+ ../../examples/SharedMemory/PhysicsClient.cpp
+ ../../examples/SharedMemory/IKTrajectoryHelper.cpp
+ ../../examples/SharedMemory/IKTrajectoryHelper.h
+ ../../examples/SharedMemory/PhysicsClient.h
+ ../../examples/SharedMemory/PhysicsServer.cpp
+ ../../examples/SharedMemory/PhysicsServer.h
+ ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
+ ../../examples/SharedMemory/PhysicsServerSharedMemory.h
+ ../../examples/SharedMemory/PhysicsDirect.cpp
+ ../../examples/SharedMemory/PhysicsDirect.h
+ ../../examples/SharedMemory/PhysicsDirectC_API.cpp
+ ../../examples/SharedMemory/PhysicsDirectC_API.h
+ ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+ ../../examples/SharedMemory/PhysicsServerCommandProcessor.h
+ ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
+ ../../examples/SharedMemory/PhysicsClientSharedMemory.h
+ ../../examples/SharedMemory/PhysicsClientC_API.cpp
+ ../../examples/SharedMemory/PhysicsClientC_API.h
+ ../../examples/SharedMemory/PhysicsLoopBack.cpp
+ ../../examples/SharedMemory/PhysicsLoopBack.h
+ ../../examples/SharedMemory/PhysicsLoopBackC_API.cpp
+ ../../examples/SharedMemory/PhysicsLoopBackC_API.h
+ ../../examples/SharedMemory/Win32SharedMemory.cpp
+ ../../examples/SharedMemory/Win32SharedMemory.h
+ ../../examples/SharedMemory/PosixSharedMemory.cpp
+ ../../examples/SharedMemory/PosixSharedMemory.h
+ ../../examples/Utils/b3ResourcePath.cpp
+ ../../examples/Utils/b3ResourcePath.h
+ ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+ ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
+ ../../examples/OpenGLWindow/SimpleCamera.cpp
+ ../../examples/OpenGLWindow/SimpleCamera.h
+ ../../examples/TinyRenderer/geometry.cpp
+ ../../examples/TinyRenderer/model.cpp
+ ../../examples/TinyRenderer/tgaimage.cpp
+ ../../examples/TinyRenderer/our_gl.cpp
+ ../../examples/TinyRenderer/TinyRenderer.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
+ ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
+ ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
+ ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
+ ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
+ ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
+ ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
+ ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+ ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
+ ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+ ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
+ ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
+ ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
+ ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
+
+ )
+
+ADD_TEST(Test_PhysicsClientServer_PASS Test_PhysicsClientServer)
+
+IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+ SET_TARGET_PROPERTIES(Test_PhysicsClientServer PROPERTIES DEBUG_POSTFIX "_Debug")
+ SET_TARGET_PROPERTIES(Test_PhysicsClientServer PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel")
+ SET_TARGET_PROPERTIES(Test_PhysicsClientServer PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
+ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
diff --git a/test/SharedMemory/gtestwrap.cpp b/test/SharedMemory/gtestwrap.cpp
new file mode 100644
index 0000000..c260734
--- /dev/null
+++ b/test/SharedMemory/gtestwrap.cpp
@@ -0,0 +1,23 @@
+#include <gtest/gtest.h>
+#include "test.c"
+#include "Bullet3Common/b3Logging.h"
+
+void myprintf(const char* bla)
+{
+}
+
+
+int main(int argc, char **argv) {
+
+b3SetCustomPrintfFunc(myprintf);
+b3SetCustomWarningMessageFunc(myprintf);
+
+
+#if _MSC_VER
+ _CrtSetDbgFlag ( _CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF );
+ //void *testWhetherMemoryLeakDetectionWorks = malloc(1);
+#endif
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua
index b964545..1974ba4 100644
--- a/test/SharedMemory/premake4.lua
+++ b/test/SharedMemory/premake4.lua
@@ -9,6 +9,7 @@ project ("Test_SharedMemoryPhysicsClient")
"Bullet3Common",
"LinearMath"
}
+ defines {"PHYSICS_SHARED_MEMORY"}
files {
"test.c",
@@ -35,16 +36,21 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/ThirdPartyLibs"}
defines {"PHYSICS_LOOP_BACK"}
links {
+ "BulletInverseDynamicsUtils",
+ "BulletInverseDynamics",
"BulletFileLoader",
"BulletWorldImporter",
"Bullet3Common",
"BulletDynamics",
"BulletCollision",
+ "BussIK",
"LinearMath"
}
files {
"test.c",
+ "../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+ "../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp",
@@ -65,6 +71,15 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/SharedMemory/Win32SharedMemory.h",
"../../examples/SharedMemory/PosixSharedMemory.cpp",
"../../examples/SharedMemory/PosixSharedMemory.h",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
+ "../../examples/OpenGLWindow/SimpleCamera.cpp",
+ "../../examples/OpenGLWindow/SimpleCamera.h",
+ "../../examples/TinyRenderer/geometry.cpp",
+ "../../examples/TinyRenderer/model.cpp",
+ "../../examples/TinyRenderer/tgaimage.cpp",
+ "../../examples/TinyRenderer/our_gl.cpp",
+ "../../examples/TinyRenderer/TinyRenderer.cpp",
"../../examples/Utils/b3ResourcePath.cpp",
"../../examples/Utils/b3ResourcePath.h",
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
@@ -81,7 +96,9 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
- }
+ "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
+ }
project ("Test_PhysicsServerDirect")
@@ -92,16 +109,21 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/ThirdPartyLibs"}
defines {"PHYSICS_SERVER_DIRECT"}
links {
+ "BulletInverseDynamicsUtils",
+ "BulletInverseDynamics",
"BulletFileLoader",
"BulletWorldImporter",
"Bullet3Common",
"BulletDynamics",
- "BulletCollision",
+ "BulletCollision",
+ "BussIK",
"LinearMath"
}
files {
"test.c",
+ "../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+ "../../examples/SharedMemory/IKTrajectoryHelper.h",
"../../examples/SharedMemory/PhysicsClient.cpp",
"../../examples/SharedMemory/PhysicsClient.h",
"../../examples/SharedMemory/PhysicsServer.cpp",
@@ -122,6 +144,15 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/SharedMemory/Win32SharedMemory.h",
"../../examples/SharedMemory/PosixSharedMemory.cpp",
"../../examples/SharedMemory/PosixSharedMemory.h",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
+ "../../examples/TinyRenderer/geometry.cpp",
+ "../../examples/TinyRenderer/model.cpp",
+ "../../examples/TinyRenderer/tgaimage.cpp",
+ "../../examples/TinyRenderer/our_gl.cpp",
+ "../../examples/TinyRenderer/TinyRenderer.cpp",
+ "../../examples/OpenGLWindow/SimpleCamera.cpp",
+ "../../examples/OpenGLWindow/SimpleCamera.h",
"../../examples/Utils/b3ResourcePath.cpp",
"../../examples/Utils/b3ResourcePath.h",
"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
@@ -138,7 +169,114 @@ project ("Test_PhysicsServerLoopBack")
"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
}
+project ("Test_PhysicsServerInProcessExampleBrowser")
+
+ language "C++"
+ kind "ConsoleApp"
+
+ includedirs {"../../src", "../../examples",
+ "../../examples/ThirdPartyLibs"}
+ defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
+-- links {
+-- "BulletExampleBrowserLib",
+-- "BulletFileLoader",
+-- "BulletWorldImporter",
+-- "Bullet3Common",
+-- "BulletDynamics",
+-- "BulletCollision",
+-- "LinearMath"
+-- }
+ hasCL = findOpenCL("clew")
+
+ links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletFileLoader","BulletWorldImporter","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK","Bullet3Common"}
+ initOpenGL()
+ initGlew()
+
+ includedirs {
+ ".",
+ "../../src",
+ "../ThirdPartyLibs",
+ }
+
+
+ if os.is("MacOSX") then
+ links{"Cocoa.framework"}
+ end
+
+ if (hasCL) then
+ links {
+ "Bullet3OpenCL_clew",
+ "Bullet3Dynamics",
+ "Bullet3Collision",
+ "Bullet3Geometry",
+ "Bullet3Common",
+ }
+ end
+
+ files {
+ "test.c",
+ "../../examples/SharedMemory/IKTrajectoryHelper.cpp",
+ "../../examples/SharedMemory/IKTrajectoryHelper.h",
+ "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",
+ "../../examples/SharedMemory/InProcessMemory.cpp",
+ "../../examples/SharedMemory/PhysicsClient.cpp",
+ "../../examples/SharedMemory/PhysicsClient.h",
+ "../../examples/SharedMemory/PhysicsServer.cpp",
+ "../../examples/SharedMemory/PhysicsServer.h",
+ "../../examples/SharedMemory/PhysicsServerExample.cpp",
+ "../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
+ "../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
+ "../../examples/SharedMemory/PhysicsServerSharedMemory.h",
+ "../../examples/SharedMemory/PhysicsDirect.cpp",
+ "../../examples/SharedMemory/PhysicsDirect.h",
+ "../../examples/SharedMemory/PhysicsDirectC_API.cpp",
+ "../../examples/SharedMemory/PhysicsDirectC_API.h",
+ "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
+ "../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
+ "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
+ "../../examples/SharedMemory/PhysicsClientSharedMemory.h",
+ "../../examples/SharedMemory/PhysicsClientC_API.cpp",
+ "../../examples/SharedMemory/PhysicsClientC_API.h",
+ "../../examples/SharedMemory/Win32SharedMemory.cpp",
+ "../../examples/SharedMemory/Win32SharedMemory.h",
+ "../../examples/SharedMemory/PosixSharedMemory.cpp",
+ "../../examples/SharedMemory/PosixSharedMemory.h",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp",
+ "../../examples/SharedMemory/TinyRendererVisualShapeConverter.h",
+ "../../examples/TinyRenderer/geometry.cpp",
+ "../../examples/TinyRenderer/model.cpp",
+ "../../examples/TinyRenderer/tgaimage.cpp",
+ "../../examples/TinyRenderer/our_gl.cpp",
+ "../../examples/TinyRenderer/TinyRenderer.cpp",
+ "../../examples/Utils/b3ResourcePath.cpp",
+ "../../examples/Utils/b3ResourcePath.h",
+ "../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+ "../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+ "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+ "../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+ "../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+ "../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+ "../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+ "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+ "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+ "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
+ "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
+ "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
+ "../../examples/MultiThreading/b3PosixThreadSupport.cpp",
+ "../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
+ "../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
+ "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+ "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
+ }
+ if os.is("Linux") then
+ initX11()
+ end
+
diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c
index f300346..0a8b603 100644
--- a/test/SharedMemory/test.c
+++ b/test/SharedMemory/test.c
@@ -1,5 +1,7 @@
//#include "SharedMemoryCommands.h"
+#ifdef PHYSICS_SHARED_MEMORY
#include "SharedMemory/PhysicsClientC_API.h"
+#endif //PHYSICS_SHARED_MEMORY
#ifdef PHYSICS_LOOP_BACK
#include "SharedMemory/PhysicsLoopBackC_API.h"
@@ -9,6 +11,9 @@
#include "SharedMemory/PhysicsDirectC_API.h"
#endif //PHYSICS_SERVER_DIRECT
+#ifdef PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
+#include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
+#endif//PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
#include "SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Logging.h"
@@ -17,55 +22,98 @@
#include <stdio.h>
-int main(int argc, char* argv[])
+#ifndef ENABLE_GTEST
+#include <assert.h>
+#define ASSERT_EQ(a,b) assert((a)==(b));
+#else
+#define printf
+#endif
+
+void testSharedMemory(b3PhysicsClientHandle sm)
{
int i, dofCount , posVarCount, ret ,numJoints ;
int sensorJointIndexLeft=-1;
int sensorJointIndexRight=-1;
const char* urdfFileName = "r2d2.urdf";
+ const char* sdfFileName = "kuka_iiwa/model.sdf";
double gravx=0, gravy=0, gravz=-9.8;
double timeStep = 1./60.;
double startPosX, startPosY,startPosZ;
int imuLinkIndex = -1;
-
-
- b3PhysicsClientHandle sm=0;
int bodyIndex = -1;
-
- printf("hello world\n");
-#ifdef PHYSICS_LOOP_BACK
- sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
-#endif
-
-#ifdef PHYSICS_SERVER_DIRECT
- sm = b3ConnectPhysicsDirect();
-#else//PHYSICS_SERVER_DIRECT
- sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
-#endif //PHYSICS_SERVER_DIRECT
-
-
-
if (b3CanSubmitCommand(sm))
{
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+ b3SharedMemoryStatusHandle statusHandle;
ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz);
ret = b3PhysicsParamSetTimeStep(command, timeStep);
- b3SubmitClientCommandAndWaitStatus(sm, command);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
-
{
b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ int bodyIndicesOut[10];//MAX_SDF_BODIES = 10
+ int numJoints, numBodies;
+ int bodyUniqueId;
+ b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ statusType = b3GetStatusType(statusHandle);
+ ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED);
+
+ numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
+ ASSERT_EQ(numBodies,1);
+ bodyUniqueId = bodyIndicesOut[0];
+
+ numJoints = b3GetNumJoints(sm,bodyUniqueId);
+ ASSERT_EQ(numJoints,7);
+
+#if 0
+ b3Printf("numJoints: %d\n", numJoints);
+ for (i=0;i<numJoints;i++)
+ {
+ struct b3JointInfo jointInfo;
+ if (b3GetJointInfo(sm,bodyUniqueId, i,&jointInfo))
+ {
+ b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
+ }
+ }
+#endif
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ b3SharedMemoryCommandHandle commandHandle;
+ double jointAngle = 0.f;
+ int jointIndex;
+ commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId);
+ for (jointIndex=0;jointIndex<numJoints;jointIndex++)
+ {
+ b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle);
+ }
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+
+ ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
+ }
+
+ }
+
+
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
//setting the initial position, orientation and other arguments are optional
startPosX =2;
- startPosY =3;
+ startPosY =0;
startPosZ = 1;
ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ statusType = b3GetStatusType(statusHandle);
+ ASSERT_EQ(statusType, CMD_URDF_LOADING_COMPLETED);
bodyIndex = b3GetStatusBodyIndex(statusHandle);
}
@@ -77,7 +125,7 @@ int main(int argc, char* argv[])
struct b3JointInfo jointInfo;
b3GetJointInfo(sm,bodyIndex, i,&jointInfo);
- printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
+ // printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
//pick the IMU link index based on torso name
if (strstr(jointInfo.m_linkName,"base_link"))
{
@@ -98,7 +146,7 @@ int main(int argc, char* argv[])
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{
- b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
+ b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle statusHandle;
if (imuLinkIndex>=0)
{
@@ -114,7 +162,7 @@ int main(int argc, char* argv[])
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
-
+ ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
}
@@ -125,6 +173,7 @@ int main(int argc, char* argv[])
ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1);
ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+ ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RIGID_BODY_CREATION_COMPLETED);
}
@@ -134,15 +183,15 @@ int main(int argc, char* argv[])
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
-
+ ASSERT_EQ(statusType, CMD_ACTUAL_STATE_UPDATE_COMPLETED);
+
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
b3GetStatusActualState(statusHandle,
0, &posVarCount, &dofCount,
0, 0, 0, 0);
-
- b3Printf("posVarCount = %d\n",posVarCount);
- printf("dofCount = %d\n",dofCount);
+ ASSERT_EQ(posVarCount,15);
+ ASSERT_EQ(dofCount,14);
}
}
@@ -161,9 +210,21 @@ int main(int argc, char* argv[])
///perform some simulation steps for testing
for ( i=0;i<100;i++)
{
- b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+
+ if (b3CanSubmitCommand(sm))
+ {
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
+ statusType = b3GetStatusType(statusHandle);
+ ASSERT_EQ(statusType, CMD_STEP_FORWARD_SIMULATION_COMPLETED);
+ } else
+ {
+ break;
+ }
}
+ if (b3CanSubmitCommand(sm))
{
b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex));
@@ -191,11 +252,13 @@ int main(int argc, char* argv[])
sensorState.m_jointForceTorque[2]);
}
- }
- {
- b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
+ {
+ b3SharedMemoryStatusHandle statusHandle;
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
+ ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RESET_SIMULATION_COMPLETED);
+ }
}
}
@@ -203,3 +266,48 @@ int main(int argc, char* argv[])
b3DisconnectSharedMemory(sm);
}
+
+
+
+#ifdef ENABLE_GTEST
+
+
+TEST(BulletPhysicsClientServerTest, DirectConnection) {
+ b3PhysicsClientHandle sm = b3ConnectPhysicsDirect();
+ testSharedMemory(sm);
+}
+
+TEST(BulletPhysicsClientServerTest, LoopBackSharedMemory) {
+ b3PhysicsClientHandle sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
+ testSharedMemory(sm);
+}
+
+
+#else
+
+int main(int argc, char* argv[])
+{
+#ifdef PHYSICS_LOOP_BACK
+ b3PhysicsClientHandle sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
+#endif
+
+#ifdef PHYSICS_SERVER_DIRECT
+ b3PhysicsClientHandle sm = b3ConnectPhysicsDirect();
+#endif
+
+#ifdef PHYSICS_IN_PROCESS_EXAMPLE_BROWSER
+
+#ifdef __APPLE__
+ b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc,argv);
+#else
+ b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnect(argc,argv);
+#endif //__APPLE__
+#endif
+#ifdef PHYSICS_SHARED_MEMORY
+ b3PhysicsClientHandle sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
+#endif //PHYSICS_SHARED_MEMORY
+
+ testSharedMemory(sm);
+}
+#endif
+
diff --git a/xcode.command b/xcode.command
new file mode 100755
index 0000000..e5ab2fc
--- /dev/null
+++ b/xcode.command
@@ -0,0 +1,5 @@
+
+cd `dirname $0`
+cd build3
+./premake4_osx xcode4
+open xcode4/0_Bullet3Solution.xcworkspace
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-games/bullet.git
More information about the Pkg-games-commits
mailing list