diff --git a/.travis.yml b/.travis.yml index bb6dcde2e..25053a90d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,10 +5,14 @@ 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/CMakeLists.txt b/CMakeLists.txt index b34c7f996..cc6e258e6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,6 +87,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) @@ -191,14 +193,15 @@ IF (APPLE) ENDIF() OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON) + +FIND_PACKAGE(PythonLibs) OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF) + IF(BUILD_PYBULLET) IF(WIN32) - FIND_PACKAGE(PythonLibs 3.4 REQUIRED) SET(BUILD_SHARED_LIBS OFF CACHE BOOL "Shared Libs" FORCE) ELSE(WIN32) - FIND_PACKAGE(PythonLibs 2.7 REQUIRED) SET(BUILD_SHARED_LIBS ON CACHE BOOL "Shared Libs" FORCE) ENDIF(WIN32) ENDIF(BUILD_PYBULLET) diff --git a/Extras/InverseDynamics/DillCreator.cpp b/Extras/InverseDynamics/DillCreator.cpp index a1ddf00c2..46f68e698 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 d52e0d06d..fbe0e8e22 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/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp index b91a10cce..ef3ebf6b8 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 0e2d614f2..3844cf3d3 100644 --- a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp @@ -25,7 +25,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/Serialize/BulletFileLoader/bDNA.cpp b/Extras/Serialize/BulletFileLoader/bDNA.cpp index f30f7fec6..86d15cfdd 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++; } + + + cp = btAlignPointer(cp,4); - { - nr= (long)cp; - //long mask=3; - nr= ((nr+3)&~3)-nr; - while (nr--) - { - cp++; - } - } - - /* TYPE (4 bytes) 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) (short) the lengths of types diff --git a/Extras/Serialize/BulletFileLoader/bFile.cpp b/Extras/Serialize/BulletFileLoader/bFile.cpp index 35135ba7a..cb8010dc2 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) diff --git a/README.md b/README.md index 711ebcf98..942500cb9 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,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. @@ -88,7 +88,7 @@ You can just run it though a terminal/command prompt, or by clicking it. [--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/build3/premake4.lua b/build3/premake4.lua index ae83944d4..862dfc2a5 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -6,10 +6,15 @@ osversion.majorversion, osversion.minorversion, osversion.revision, osversion.description)) - - -- Multithreaded compiling if _ACTION == "vs2010" or _ACTION=="vs2008" then - buildoptions { "/MP" } + buildoptions + { + -- Multithreaded compiling + "/MP", + -- Disable a few useless warnings + "/wd4244", + "/wd4267" + } end act = "" @@ -29,6 +34,11 @@ description = "Try to link and use the system OpenGL headers version instead of dynamically loading OpenGL (dlopen is default)" } + newoption + { + trigger = "enable_openvr", + description = "Enable experimental Virtual Reality examples, using OpenVR for HTC Vive and Oculus Rift" + } newoption { trigger = "enable_system_x11", diff --git a/build_and_run_cmake.sh b/build_and_run_cmake.sh index 1405060f4..28354357b 100755 --- a/build_and_run_cmake.sh +++ b/build_and_run_cmake.sh @@ -2,6 +2,6 @@ rm CMakeCache.txt mkdir build_cmake cd build_cmake -cmake .. +cmake -DBUILD_PYTHON=OFF -CMAKE_BUILD_TYPE=Release .. make -j12 examples/ExampleBrowser/App_ExampleBrowser diff --git a/build_visual_studio.bat b/build_visual_studio.bat index de3b886c9..0da0089b1 100644 --- a/build_visual_studio.bat +++ b/build_visual_studio.bat @@ -2,7 +2,7 @@ rem premake4 --with-pe vs2010 rem premake4 --bullet2demos vs2010 cd build3 -premake4 --targetdir="../bin" vs2010 +premake4 --enable_openvr --targetdir="../bin" vs2010 rem premake4 --targetdir="../server2bin" vs2010 rem cd vs2010 rem rename 0_Bullet3Solution.sln 0_server.sln @@ -14,6 +14,6 @@ rem cd vs2010 rem rename 0_Bullet3Solution.sln 0_client.sln rem cd .. rem rename vs2010 vs2010_client -start vs2010/0_Bullet3Solution.sln +rem start vs2010/0_Bullet3Solution.sln pause diff --git a/bullet.pc.cmake b/bullet.pc.cmake index 2863ca1f9..8b989faba 100644 --- a/bullet.pc.cmake +++ b/bullet.pc.cmake @@ -3,4 +3,4 @@ Description: Bullet Continuous Collision Detection and Physics Library Requires: Version: @BULLET_VERSION@ Libs: -L@CMAKE_INSTALL_PREFIX@/@LIB_DESTINATION@ -lBulletSoftBody -lBulletDynamics -lBulletCollision -lLinearMath -Cflags: @BULLET_DOUBLE_DEF@ -I@INCLUDE_INSTALL_DIR@ -I@CMAKE_INSTALL_PREFIX@/include +Cflags: @BULLET_DOUBLE_DEF@ -I@CMAKE_INSTALL_PREFIX@/@INCLUDE_INSTALL_DIR@ -I@CMAKE_INSTALL_PREFIX@/include diff --git a/data/Quadrotor/quadrotor.urdf b/data/Quadrotor/quadrotor.urdf new file mode 100644 index 000000000..7158dc40a --- /dev/null +++ b/data/Quadrotor/quadrotor.urdf @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/Quadrotor/quadrotor_base.obj b/data/Quadrotor/quadrotor_base.obj new file mode 100644 index 000000000..dc5129f4b --- /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 000000000..0894b91f1 Binary files /dev/null and b/data/checker_blue.png differ diff --git a/data/cube.mtl b/data/cube.mtl index fca828974..ffce2975d 100644 --- a/data/cube.mtl +++ b/data/cube.mtl @@ -10,5 +10,7 @@ newmtl cube Ks 0.0000 0.0000 0.0000 Ke 0.0000 0.0000 0.0000 map_Ka cube.tga - map_Kd floor_diffuse.tga + map_Kd cube.png + + diff --git a/data/cube.urdf b/data/cube.urdf new file mode 100644 index 000000000..679e106b5 --- /dev/null +++ b/data/cube.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/cube_no_friction.urdf b/data/cube_no_friction.urdf new file mode 100644 index 000000000..aef2bfa15 --- /dev/null +++ b/data/cube_no_friction.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/cube_small.urdf b/data/cube_small.urdf new file mode 100644 index 000000000..804f7f0b1 --- /dev/null +++ b/data/cube_small.urdf @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/gripper/meshes/GUIDE_WSG50_110.stl b/data/gripper/meshes/GUIDE_WSG50_110.stl new file mode 100644 index 000000000..a7b584732 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 000000000..f61a9cf90 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 000000000..fee723bf8 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 000000000..94fb02462 Binary files /dev/null and b/data/gripper/meshes/l_gripper_tip_scaled.stl differ diff --git a/data/gripper/wsg50_with_r2d2_gripper.sdf b/data/gripper/wsg50_with_r2d2_gripper.sdf new file mode 100644 index 000000000..b022faf00 --- /dev/null +++ b/data/gripper/wsg50_with_r2d2_gripper.sdf @@ -0,0 +1,298 @@ + + + + + 0 0 0.27 3.14 0 0 + + + + + + world + base_link + + 0 0 1 + + -0.5 + 10 + 1 + 1 + + + 100 + 100 + 0 + 0 + + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1.2 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0 0 -0 0 + + + 0.2 0.05 0.05 + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/WSG50_110.stl + + + + + + + 1 + + 0 + + + + -0.055 0 0 0 -0 0 + + 0 0 0.0115 0 -0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + + gripper_left + base_link + + 1 0 0 + + -0.001 + 0.055 + 1 + 1 + + + 100 + 100 + 0 + 0 + + + + + + 0.055 0 0 0 -0 3.14159 + + 0 0 0.0115 0 -0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 0.023 0 -0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + + gripper_right + base_link + + -1 0 0 + + -0.055 + 0.001 + 1 + 1 + + + 100 + 100 + 0 + 0 + + + + + + 0.062 0 0.145 0 0 1.5708 + + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + 0 0 0.042 0 0 0 + + + 0.02 0.02 0.15 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/l_gripper_tip_scaled.stl + + + + + + + gripper_right + finger_right + + + + -0.062 0 0.145 0 0 4.71239 + + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + 0 0 0.042 0 0 0 + + + 0.02 0.02 0.15 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/l_gripper_tip_scaled.stl + + + + + + + gripper_left + finger_left + + + + diff --git a/data/husky/husky.urdf b/data/husky/husky.urdf new file mode 100644 index 000000000..e9762701b --- /dev/null +++ b/data/husky/husky.urdf @@ -0,0 +1,345 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/DarkGrey + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + false + + + + + + + + + transmission_interface/SimpleTransmission + + 1 + + + VelocityJointInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + false + + + + + + + + + transmission_interface/SimpleTransmission + + 1 + + + VelocityJointInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + false + + + + + + + + + transmission_interface/SimpleTransmission + + 1 + + + VelocityJointInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + false + + + + + + + + + transmission_interface/SimpleTransmission + + 1 + + + VelocityJointInterface + + + + + + + + + + + + + + + + + Gazebo/Yellow + + + + + + + + + + + + + + + + Gazebo/DarkGrey + + + + + + + + + + + + + + + + Gazebo/DarkGrey + + + + + + + + + + + + + + + + Gazebo/DarkGrey + + + diff --git a/data/husky/meshes/bumper.stl b/data/husky/meshes/bumper.stl new file mode 100644 index 000000000..7cccf4b03 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 000000000..ef4d137b5 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 000000000..7542c59bc 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 000000000..0e76151ca Binary files /dev/null and b/data/husky/meshes/wheel.stl differ diff --git a/data/kiva_shelf/0_Bullet3Demo.txt b/data/kiva_shelf/0_Bullet3Demo.txt new file mode 100644 index 000000000..eb396d29f --- /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 000000000..6e668f1d7 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 000000000..b1f49e66b --- /dev/null +++ b/data/kiva_shelf/model.sdf @@ -0,0 +1,55 @@ + + + + 1 + 0 2 1.21 0 0 0 + + + 0.0 0.0 1.2045 0 0 0 + 76.26 + + 47 + -0.003456 + 0.001474 + 13.075 + -0.014439 + 47 + + + + + 0 0 0 1.5707 0 0 + + + meshes/pod_lowres.stl + + + + 0.9 0.8 0.5 1 + + + + + + 0 0 0 1.5707 0 0 + + + meshes/pod_lowres.stl + + + + + + 0.8 + 0.8 + 0.0 0.0 0.0 + 1.0 + 1.0 + + + + + + + + diff --git a/data/kuka_iiwa/meshes/coarse/link_0.stl b/data/kuka_iiwa/meshes/coarse/link_0.stl deleted file mode 100644 index 84b8ea5d2..000000000 Binary files a/data/kuka_iiwa/meshes/coarse/link_0.stl and /dev/null differ diff --git a/data/kuka_iiwa/meshes/coarse/link_1.stl b/data/kuka_iiwa/meshes/coarse/link_1.stl deleted file mode 100644 index ffe3ec981..000000000 Binary files a/data/kuka_iiwa/meshes/coarse/link_1.stl and /dev/null differ diff --git a/data/kuka_iiwa/meshes/coarse/link_2.stl b/data/kuka_iiwa/meshes/coarse/link_2.stl deleted file mode 100644 index 4a51b27ef..000000000 Binary files a/data/kuka_iiwa/meshes/coarse/link_2.stl and /dev/null differ diff --git a/data/kuka_iiwa/meshes/coarse/link_3.stl b/data/kuka_iiwa/meshes/coarse/link_3.stl deleted file mode 100644 index 32d6d5282..000000000 Binary files a/data/kuka_iiwa/meshes/coarse/link_3.stl and /dev/null differ diff --git a/data/kuka_iiwa/meshes/coarse/link_4.stl b/data/kuka_iiwa/meshes/coarse/link_4.stl deleted file mode 100644 index 35d192181..000000000 Binary files a/data/kuka_iiwa/meshes/coarse/link_4.stl and /dev/null differ diff --git a/data/kuka_iiwa/meshes/coarse/link_5.stl b/data/kuka_iiwa/meshes/coarse/link_5.stl deleted file mode 100644 index 35aa1245d..000000000 Binary files a/data/kuka_iiwa/meshes/coarse/link_5.stl and /dev/null differ diff --git a/data/kuka_iiwa/meshes/coarse/link_6.stl b/data/kuka_iiwa/meshes/coarse/link_6.stl deleted file mode 100644 index bce349eb2..000000000 Binary files a/data/kuka_iiwa/meshes/coarse/link_6.stl and /dev/null differ diff --git a/data/kuka_iiwa/meshes/coarse/link_7.stl b/data/kuka_iiwa/meshes/coarse/link_7.stl deleted file mode 100644 index 2d5d6ecfb..000000000 Binary files a/data/kuka_iiwa/meshes/coarse/link_7.stl and /dev/null differ diff --git a/data/kuka_iiwa/meshes/link_0.stl b/data/kuka_iiwa/meshes/link_0.stl index 84b8ea5d2..ed1447056 100644 Binary files a/data/kuka_iiwa/meshes/link_0.stl 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 index e8e37de9f..d9d043e69 100644 Binary files a/data/kuka_iiwa/meshes/link_1.stl 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 index 47c7885fc..282aa787b 100644 Binary files a/data/kuka_iiwa/meshes/link_2.stl 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 index 027eb2211..50d856cb8 100644 Binary files a/data/kuka_iiwa/meshes/link_3.stl 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 index c0c1213c1..ade9b0cf1 100644 Binary files a/data/kuka_iiwa/meshes/link_4.stl 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 index 82a9337a6..663ece5cd 100644 Binary files a/data/kuka_iiwa/meshes/link_5.stl 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 index 10b558dc5..7fb9fcad2 100644 Binary files a/data/kuka_iiwa/meshes/link_6.stl 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 index 5909e7e01..e4e301b53 100644 Binary files a/data/kuka_iiwa/meshes/link_7.stl and b/data/kuka_iiwa/meshes/link_7.stl differ diff --git a/data/kuka_iiwa/model.sdf b/data/kuka_iiwa/model.sdf index 1b6f8320d..3a962ee74 100644 --- a/data/kuka_iiwa/model.sdf +++ b/data/kuka_iiwa/model.sdf @@ -1,6 +1,7 @@ + 1 0 0 0 0 -0 0 @@ -32,6 +33,12 @@ meshes/link_0.stl + + 1 0 0 1 + 0.2 0.2 0.2 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + @@ -65,6 +72,12 @@ meshes/link_1.stl + + 1 0 0 1 + 0.5 0.7 1.0 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + @@ -118,6 +131,12 @@ meshes/link_2.stl + + 1 0 0 1 + 0.5 0.7 1.0 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + @@ -171,6 +190,12 @@ meshes/link_3.stl + + 1 0 0 1 + 1.0 0.42 0.04 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + @@ -224,6 +249,12 @@ meshes/link_4.stl + + 1 0 0 1 + 0.5 0.7 1.0 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + @@ -277,6 +308,12 @@ meshes/link_5.stl + + 1 0 0 1 + 0.5 0.7 1.0 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + @@ -330,6 +367,12 @@ meshes/link_6.stl + + 1 0 0 1 + 1.0 0.42 0.04 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + @@ -383,6 +426,12 @@ meshes/link_7.stl + + 1 0 0 1 + 0.2 0.2 0.2 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + @@ -407,4 +456,4 @@ - \ No newline at end of file + diff --git a/data/kuka_iiwa/model.urdf b/data/kuka_iiwa/model.urdf index 56123c36d..d8fe60293 100644 --- a/data/kuka_iiwa/model.urdf +++ b/data/kuka_iiwa/model.urdf @@ -77,7 +77,7 @@ - + @@ -106,7 +106,7 @@ - + @@ -135,7 +135,7 @@ - + @@ -164,7 +164,7 @@ - + @@ -193,7 +193,7 @@ - + @@ -222,7 +222,7 @@ - + @@ -251,7 +251,7 @@ - + @@ -280,7 +280,7 @@ - + diff --git a/data/multibody.bullet b/data/multibody.bullet index 1074b55dc..31800308c 100644 Binary files a/data/multibody.bullet and b/data/multibody.bullet differ diff --git a/data/plane.mtl b/data/plane.mtl index 8ce4b8207..cd101528d 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 64342328f..0b77a9912 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 000000000..57b746104 --- /dev/null +++ b/data/plane.urdf @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/plane100.obj b/data/plane100.obj index c60a07b05..3a74f590b 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 000000000..1c6a01cc6 --- /dev/null +++ b/data/plane100.urdf @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/r2d2.urdf b/data/r2d2.urdf index 9f7c750c0..091369db8 100644 --- a/data/r2d2.urdf +++ b/data/r2d2.urdf @@ -79,7 +79,7 @@ - + diff --git a/data/sphere2.urdf b/data/sphere2.urdf index 891e018cb..cf7618d93 100644 --- a/data/sphere2.urdf +++ b/data/sphere2.urdf @@ -9,8 +9,11 @@ - + + + + @@ -19,55 +22,5 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/data/two_cubes.sdf b/data/two_cubes.sdf index 24c0854bd..e19b73b43 100644 --- a/data/two_cubes.sdf +++ b/data/two_cubes.sdf @@ -86,6 +86,7 @@ 0 + 1 0.512455 -1.58317 0.5 0 -0 0 @@ -121,11 +122,13 @@ - - 1 1 1 - - + + 1 1 1 + cube.obj + + + 1 1 1 1 + 1 1 1 1 0 diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/BasicDemo/BasicExample.cpp index f5bd9486c..b687c35b1 100644 --- a/examples/BasicDemo/BasicExample.cpp +++ b/examples/BasicDemo/BasicExample.cpp @@ -39,10 +39,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]); } }; @@ -81,7 +81,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 +108,9 @@ void BasicExample::initPhysics() for(int j = 0;jautogenerateGraphicsObjects(m_dynamicsWorld); + } diff --git a/examples/BasicDemo/CMakeLists.txt b/examples/BasicDemo/CMakeLists.txt index 2ad8f8ccf..f01a03a51 100644 --- a/examples/BasicDemo/CMakeLists.txt +++ b/examples/BasicDemo/CMakeLists.txt @@ -53,6 +53,7 @@ SET(AppBasicExampleGui_SRCS ../ExampleBrowser/OpenGLGuiHelper.cpp ../ExampleBrowser/GL_ShapeDrawer.cpp ../ExampleBrowser/CollisionShape2TriangleMesh.cpp + ../Utils/b3Clock.cpp ) #this define maps StandaloneExampleCreateFunc to the right 'CreateFunc' diff --git a/examples/BasicDemo/premake4.lua b/examples/BasicDemo/premake4.lua index 6474d9298..dd924249e 100644 --- a/examples/BasicDemo/premake4.lua +++ b/examples/BasicDemo/premake4.lua @@ -49,6 +49,8 @@ files { "../ExampleBrowser/OpenGLGuiHelper.cpp", "../ExampleBrowser/GL_ShapeDrawer.cpp", "../ExampleBrowser/CollisionShape2TriangleMesh.cpp", + "../Utils/b3Clock.cpp", + "../Utils/b3Clock.h", } if os.is("Linux") then initX11() end @@ -59,6 +61,7 @@ end + project "App_BasicExampleGuiWithSoftwareRenderer" if _OPTIONS["ios"] then @@ -92,7 +95,9 @@ files { "../TinyRenderer/tgaimage.cpp", "../TinyRenderer/our_gl.cpp", "../TinyRenderer/TinyRenderer.cpp", - "../Utils/b3ResourcePath.cpp" + "../Utils/b3ResourcePath.cpp", + "../Utils/b3Clock.cpp", + "../Utils/b3Clock.h", } if os.is("Linux") then initX11() end @@ -131,5 +136,68 @@ files { "../TinyRenderer/tgaimage.cpp", "../TinyRenderer/our_gl.cpp", "../TinyRenderer/TinyRenderer.cpp", - "../Utils/b3ResourcePath.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 ca9f75786..f659d017c 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/Collision/CollisionTutorialBullet2.cpp b/examples/Collision/CollisionTutorialBullet2.cpp index 1198f9e5d..0479a73e8 100644 --- a/examples/Collision/CollisionTutorialBullet2.cpp +++ b/examples/Collision/CollisionTutorialBullet2.cpp @@ -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 73147d760..9c1740788 100644 --- a/examples/CommonInterfaces/CommonCameraInterface.h +++ b/examples/CommonInterfaces/CommonCameraInterface.h @@ -6,7 +6,9 @@ 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 setVRCamera(const float viewMat[16], const float projectionMatrix[16])=0; + virtual void disableVRCamera()=0; + virtual bool isVRCamera() const =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 f3fb20047..14d19ec3d 100644 --- a/examples/CommonInterfaces/CommonExampleInterface.h +++ b/examples/CommonInterfaces/CommonExampleInterface.h @@ -46,6 +46,8 @@ 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]) {} + virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]){} }; class ExampleEntries diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index b9a4dfd26..726303aed 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,7 +46,7 @@ struct GUIHelperInterface virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)=0; - virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* width, int* height, int* numPixelsCopied)=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)=0; virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0; @@ -73,10 +74,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; @@ -105,12 +107,8 @@ struct DummyGUIHelper : public GUIHelperInterface { } - virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* width, int* height, int* numPixelsCopied) + virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int width, int height, int* numPixelsCopied) { - if (width) - *width = 0; - if (height) - *height = 0; if (numPixelsCopied) *numPixelsCopied = 0; } diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h index 877c31880..242c63ccd 100644 --- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h +++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h @@ -77,7 +77,7 @@ struct CommonGraphicsApp virtual void dumpNextFrameToPng(const char* pngFilename){} virtual void dumpFramesToVideo(const char* mp4Filename){} - virtual void getScreenPixels(unsigned char* rgbaBuffer, int bufferSizeInBytes){}; + virtual void getScreenPixels(unsigned char* rgbaBuffer, int bufferSizeInBytes, float* depthBuffer, int depthBufferSizeInBytes){} virtual void getBackgroundColor(float* red, float* green, float* blue) const { diff --git a/examples/CommonInterfaces/CommonMultiBodyBase.h b/examples/CommonInterfaces/CommonMultiBodyBase.h index 3219f8294..48130b2d1 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/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index e1d6170ef..0ac09975f 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/examples/CommonInterfaces/CommonRenderInterface.h @@ -22,7 +22,7 @@ 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; @@ -52,6 +52,10 @@ struct CommonRenderInterface 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/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index e6570b890..fc84bb4a6 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -165,6 +165,8 @@ SET(BulletExampleBrowser_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 @@ -173,6 +175,8 @@ SET(BulletExampleBrowser_SRCS ../Tutorial/Dof6ConstraintTutorial.h ../Evolution/NN3DWalkers.cpp ../Evolution/NN3DWalkers.h + ../ExtendedTutorials/NewtonsRopeCradle.cpp + ../ExtendedTutorials/NewtonsRopeCradle.h ../Collision/CollisionSdkC_Api.cpp ../Collision/CollisionSdkC_Api.h ../Collision/CollisionTutorialBullet2.cpp @@ -204,7 +208,12 @@ SET(BulletExampleBrowser_SRCS ../RenderingExamples/TimeSeriesCanvas.h ../RenderingExamples/TimeSeriesFontData.cpp ../RenderingExamples/TimeSeriesFontData.h - + ../RoboticsLearning/GripperGraspExample.cpp + ../RoboticsLearning/GripperGraspExample.h + ../RoboticsLearning/b3RobotSimAPI.cpp + ../RoboticsLearning/b3RobotSimAPI.h + ../RoboticsLearning/R2D2GraspExample.cpp + ../RoboticsLearning/R2D2GraspExample.h ../RenderingExamples/CoordinateSystemDemo.cpp ../RenderingExamples/CoordinateSystemDemo.h ../RenderingExamples/RaytracerSetup.cpp @@ -291,6 +300,17 @@ SET(BulletExampleBrowser_SRCS ../ThirdPartyLibs/stb_image/stb_image.cpp ../ThirdPartyLibs/stb_image/stb_image.h + + ../ThirdPartyLibs/BussIK/Jacobian.cpp + ../ThirdPartyLibs/BussIK/Tree.cpp + ../ThirdPartyLibs/BussIK/Node.cpp + ../ThirdPartyLibs/BussIK/LinearR2.cpp + ../ThirdPartyLibs/BussIK/LinearR3.cpp + ../ThirdPartyLibs/BussIK/LinearR4.cpp + ../ThirdPartyLibs/BussIK/MatrixRmn.cpp + ../ThirdPartyLibs/BussIK/VectorRn.cpp + ../ThirdPartyLibs/BussIK/Misc.cpp + ../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp ../ThirdPartyLibs/tinyxml/tinystr.cpp diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp index 1105955dc..2bc8a0ea4 100644 --- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp +++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp @@ -167,6 +167,7 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans } } } + delete hull; } } else { diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index fed2d4690..30a20ad33 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -45,6 +45,10 @@ #include "../Tutorial/Dof6ConstraintTutorial.h" #include "../MultiThreading/MultiThreadingExample.h" #include "../InverseDynamics/InverseDynamicsExample.h" +#include "../RoboticsLearning/R2D2GraspExample.h" +#include "../RoboticsLearning/GripperGraspExample.h" +#include "../InverseKinematics/InverseKinematicsExample.h" + #ifdef ENABLE_LUA #include "../LuaDemo/LuaPhysicsSetup.h" #endif @@ -66,6 +70,7 @@ #include "../ExtendedTutorials/RigidBodyFromObj.h" #include "../ExtendedTutorials/InclinedPlane.h" #include "../ExtendedTutorials/NewtonsCradle.h" +#include "../ExtendedTutorials/NewtonsRopeCradle.h" #include "../ExtendedTutorials/MultiPendulum.h" #include "../Evolution/NN3DWalkers.h" @@ -97,7 +102,6 @@ static ExampleEntry gDefaultExamples[]= 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.", @@ -117,21 +121,30 @@ static ExampleEntry gDefaultExamples[]= 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(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 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(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), @@ -187,7 +200,7 @@ static ExampleEntry gDefaultExamples[]= #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. ", @@ -197,7 +210,7 @@ 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 @@ -244,8 +257,13 @@ static ExampleEntry gDefaultExamples[]= 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, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc), + ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP), + 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,"Contact for Grasp","Grasp experiment to improve contact model", GripperGraspExampleCreateFunc), + #ifdef ENABLE_LUA @@ -279,8 +297,9 @@ static ExampleEntry gDefaultExamples[]= 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 of pendula in total (reset simulation), the number of displaced pendula and other options.", ET_NewtonsCradleCreateFunc), - 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 of pendula in total (reset simulation), the number of displaced pendula and other options.",ET_MultiPendulumCreateFunc), + 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), ExampleEntry(9,"Evolution"), ExampleEntry(1,"Neural Network 3D Walkers","A simple example of using evolution to make a creature walk.",ET_NN3DWalkersCreateFunc), diff --git a/examples/ExampleBrowser/GwenGUISupport/GraphingTexture.cpp b/examples/ExampleBrowser/GwenGUISupport/GraphingTexture.cpp index 4cfcc8f10..7a87be819 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/GwenProfileWindow.cpp b/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp index d376246ec..4991ee04e 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) { @@ -266,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; } @@ -290,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 ec0982571..3fc3bb4b4 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 a70d9268a..fcff13968 100644 --- a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp +++ b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp @@ -17,6 +17,83 @@ GwenUserInterface::GwenUserInterface() } +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; + + MyTestMenuBar(Gwen::Controls::Base* pParent) + :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"); + + } + } + 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;im_handlers.size();i++) @@ -26,72 +103,15 @@ GwenUserInterface::~GwenUserInterface() m_data->m_handlers.clear(); - - delete m_data->pCanvas; - - + delete m_data; - - } -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; - - MyTestMenuBar(Gwen::Controls::Base* pParent) - :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" ); - - } - } - -}; void GwenUserInterface::resize(int width, int 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 c448e9fff..d66b59828 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 index b7254db71..e562b5d52 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -226,7 +226,7 @@ enum TestExampleBrowserCommunicationEnums void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory) { - printf("thread started\n"); + printf("ExampleBrowserThreadFunc started\n"); ExampleBrowserThreadLocalStorage* localStorage = (ExampleBrowserThreadLocalStorage*) lsMemory; @@ -369,7 +369,7 @@ void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data) } }; - printf("stopping threads\n"); + printf("btShutDownExampleBrowser stopping threads\n"); delete data->m_threadSupport; delete data->m_sharedMem; delete data; diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 13933af95..d0fae2254 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -42,19 +42,65 @@ #include "../Importers/ImportURDFDemo/ImportURDFSetup.h" #include "../Importers/ImportBullet/SerializeSetup.h" +#include "Bullet3Common/b3HashMap.h" + +struct GL3TexLoader : public MyTextureLoader +{ + b3HashMap 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 m_nodes; + GwenUserInterface* m_gui; + GL3TexLoader* m_myTexLoader; + struct MyMenuItemHander* m_handler2; + btAlignedObjectArray 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; @@ -111,6 +157,7 @@ void deleteDemo() } const char* gPngFileName = 0; +int gPngSkipFrames = 0; @@ -123,9 +170,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) @@ -223,11 +270,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) @@ -244,8 +291,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) { @@ -333,7 +380,10 @@ 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); @@ -341,12 +391,15 @@ void selectDemo(int demoIndex) 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) @@ -399,7 +452,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]}; @@ -438,10 +490,10 @@ void MyComboBoxCallback(int comboId, const char* item) void MyGuiPrintf(const char* msg) { printf("b3Printf: %s\n",msg); - if (gui) + if (gui2) { - gui->textOutput(msg); - gui->forceUpdateScrollBars(); + gui2->textOutput(msg); + gui2->forceUpdateScrollBars(); } } @@ -450,10 +502,10 @@ void MyGuiPrintf(const char* msg) void MyStatusBarPrintf(const char* msg) { printf("b3Printf: %s\n", msg); - if (gui) + if (gui2) { bool isLeft = true; - gui->setStatusBarMessage(msg,isLeft); + gui2->setStatusBarMessage(msg,isLeft); } } @@ -461,13 +513,15 @@ void MyStatusBarPrintf(const char* msg) void MyStatusBarError(const char* msg) { printf("Warning: %s\n", msg); - if (gui) + if (gui2) { 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 @@ -535,7 +589,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) @@ -550,26 +604,6 @@ struct MyMenuItemHander :public Gwen::Event::Handler -}; -#include "Bullet3Common/b3HashMap.h" - -struct GL3TexLoader : public MyTextureLoader -{ - b3HashMap 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() @@ -624,7 +658,7 @@ 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; @@ -644,7 +678,10 @@ struct QuickCanvas : public Common2dCanvasInterface virtual void destroyCanvas(int canvasId) { 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) @@ -670,12 +707,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; } @@ -688,7 +764,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 @@ -727,17 +803,17 @@ 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 { char title[1024]; sprintf(title,"%s using OpenGL3+. %s", appTitle,optMode); simpleApp = new SimpleOpenGL3App(title,width,height, gAllowRetina); - - s_app = simpleApp; } #endif + m_internalData->m_app = s_app; char* gVideoFileName = 0; args.GetCmdLineArgument("mp4",gVideoFileName); #ifndef NO_OPENGL3 @@ -793,43 +869,67 @@ 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(); - } + sth_stash* fontstash = simpleApp->getFontStash(); + + 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 + + gui2 = new GwenUserInterface; + + m_internalData->m_gui = gui2; + + m_internalData->m_myTexLoader = myTexLoader; + + + + gui2->init(width, height, m_internalData->m_gwenRenderer, s_window->getRetinaScale()); + + + } + //gui = 0;// new GwenUserInterface; + + GL3TexLoader* myTexLoader = m_internalData->m_myTexLoader; + // = myTexLoader; + + + // - gui->init(width,height,gwenRenderer,s_window->getRetinaScale()); - - - - -// gui->getInternalData()->m_explorerPage - Gwen::Controls::TreeControl* tree = gui->getInternalData()->m_explorerTreeCtrl; + if (gui2) + { + + + + + // 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); - //gui->getInternalData()->pRenderer->setTextureLoader(myTexLoader); - - - s_profWindow= setupProfileWindow(gui->getInternalData()); - profileWindowSetVisible(s_profWindow,false); - gui->setFocus(); - - s_parameterInterface = s_app->m_parameterInterface = new GwenParameterInterface(gui->getInternalData()); - s_app->m_2dCanvasInterface = new QuickCanvas(myTexLoader); - ///add some demos to the gAllExamples @@ -840,7 +940,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) //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); @@ -848,7 +948,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; @@ -895,13 +995,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); @@ -911,6 +1016,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) else { curNode = tree->AddNode(nodeUText); + m_internalData->m_nodes.push_back(curNode); } } @@ -929,6 +1035,9 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) } } + free(demoNameFromCommandOption); + demoNameFromCommandOption = 0; + btAssert(sCurrentDemo!=0); if (sCurrentDemo==0) { @@ -936,9 +1045,11 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) exit(0); } - gui->registerFileOpenCallback(fileOpenCallback); - gui->registerQuitCallback(quitCallback); - + gui2->registerFileOpenCallback(fileOpenCallback); + gui2->registerQuitCallback(quitCallback); + } + + return true; } @@ -971,14 +1082,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++; @@ -998,24 +1102,6 @@ void OpenGLExampleBrowser::update(float deltaTime) //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); - } - } - if (gFixedTimeStep>0) { @@ -1026,6 +1112,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) @@ -1042,9 +1136,28 @@ void OpenGLExampleBrowser::update(float deltaTime) } } + 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 (s_guiHelper && s_guiHelper->getRenderInterface() && s_guiHelper->getRenderInterface()->getActiveCamera()) + if (gui2 && s_guiHelper && s_guiHelper->getRenderInterface() && s_guiHelper->getRenderInterface()->getActiveCamera()) { char msg[1024]; float camDist = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraDistance(); @@ -1053,7 +1166,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); } } @@ -1061,16 +1174,24 @@ void OpenGLExampleBrowser::update(float deltaTime) static int toggle = 1; if (renderGui) { - if (!pauseSimulation) - processProfileData(s_profWindow,false); + // 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()); + BT_PROFILE("Draw Gwen GUI"); + 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(); @@ -1084,14 +1205,20 @@ 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) + { + gui2->forceUpdateScrollBars(); + } } diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.h b/examples/ExampleBrowser/OpenGLExampleBrowser.h index ac9acd2f6..1f68abedb 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); diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 23c225677..a5d7e0c3d 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -8,7 +8,7 @@ #include "CollisionShape2TriangleMesh.h" - +#include "../OpenGLWindow/SimpleCamera.h" #include "../OpenGLWindow/GLInstanceGraphicsShape.h" //backwards compatibility #include "GL_ShapeDrawer.h" @@ -144,8 +144,8 @@ struct OpenGLGuiHelperInternalData class MyDebugDrawer* m_debugDraw; GL_ShapeDrawer* m_gl2ShapeDrawer; - btAlignedObjectArray m_rgbaPixelBuffer; - btAlignedObjectArray m_depthBuffer; + btAlignedObjectArray m_rgbaPixelBuffer1; + btAlignedObjectArray m_depthBuffer1; }; @@ -167,6 +167,7 @@ OpenGLGuiHelper::OpenGLGuiHelper(CommonGraphicsApp* glApp, bool useOpenGL2) OpenGLGuiHelper::~OpenGLGuiHelper() { + delete m_data->m_debugDraw; delete m_data->m_gl2ShapeDrawer; delete m_data; } @@ -199,9 +200,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; } @@ -210,6 +218,10 @@ int OpenGLGuiHelper::registerGraphicsInstance(int shapeIndex, const float* posit return m_data->m_glApp->m_renderer->registerGraphicsInstance(shapeIndex,position,quaternion,color,scaling); } +void OpenGLGuiHelper::removeAllGraphicsInstances() +{ + m_data->m_glApp->m_renderer->removeAllInstances(); +} void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* collisionShape) { @@ -247,7 +259,7 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli if (gfxVertices.size() && indices.size()) { - int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.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); } @@ -326,19 +338,15 @@ void OpenGLGuiHelper::resetCamera(float camDist, float pitch, float yaw, float c } -void OpenGLGuiHelper::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied) +void OpenGLGuiHelper::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) { - int w = m_data->m_glApp->m_window->getWidth()*m_data->m_glApp->m_window->getRetinaScale(); - int h = m_data->m_glApp->m_window->getHeight()*m_data->m_glApp->m_window->getRetinaScale(); + 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 (widthPtr) - *widthPtr = w; - if (heightPtr) - *heightPtr = h; if (numPixelsCopied) *numPixelsCopied = 0; - int numTotalPixels = w*h; + int numTotalPixels = destinationWidth*destinationHeight; int numRemainingPixels = numTotalPixels - startPixelIndex; int numBytesPerPixel = 4;//RGBA int numRequestedPixels = btMin(rgbaBufferSizeInPixels,numRemainingPixels); @@ -346,22 +354,58 @@ void OpenGLGuiHelper::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBuf { if (startPixelIndex==0) { - - //quick test: render the scene + CommonCameraInterface* oldCam = getRenderInterface()->getActiveCamera(); + SimpleCamera tempCam; + getRenderInterface()->setActiveCamera(&tempCam); + getRenderInterface()->getActiveCamera()->setVRCamera(viewMatrix,projectionMatrix); getRenderInterface()->renderScene(); - //copy the image into our local cache - m_data->m_rgbaPixelBuffer.resize(w*h*numBytesPerPixel); - m_data->m_depthBuffer.resize(w*h); - m_data->m_glApp->getScreenPixels(&(m_data->m_rgbaPixelBuffer[0]),m_data->m_rgbaPixelBuffer.size()); - } - for (int i=0;im_rgbaPixelBuffer[i+startPixelIndex*numBytesPerPixel]; + getRenderInterface()->setActiveCamera(oldCam); + + { + btAlignedObjectArray sourceRgbaPixelBuffer; + btAlignedObjectArray 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;im_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;im_rgbaPixelBuffer1[i+startPixelIndex*numBytesPerPixel]; + } + } + if (depthBuffer) + { + for (int i=0;im_depthBuffer1[i]; } } - if (numPixelsCopied) *numPixelsCopied = numRequestedPixels; diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index 0184d6ea0..3ebdc4572 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); @@ -45,7 +44,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ); - virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* width, int* height, int* numPixelsCopied); + 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); virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) ; diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index 179f18036..d0f132c1a 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -1,4 +1,5 @@ + #include "OpenGLExampleBrowser.h" #include "Bullet3Common/b3CommandLineArgs.h" @@ -13,36 +14,46 @@ #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; - - - 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); - - clock.reset(); - if (init) { - do - { - float deltaTimeInSeconds = clock.getTimeMicroseconds()/1000000.f; - clock.reset(); - exampleBrowser->update(deltaTimeInSeconds); + 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) + { + 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 c5299244a..84cbcb607 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -45,9 +45,10 @@ project "App_BulletExampleBrowser" defines {"INCLUDE_CLOTH_DEMOS"} files { + "main.cpp", "ExampleEntries.cpp", - + "../InverseKinematics/*", "../TinyRenderer/geometry.cpp", "../TinyRenderer/model.cpp", "../TinyRenderer/tgaimage.cpp", @@ -86,16 +87,11 @@ project "App_BulletExampleBrowser" "../InverseDynamics/InverseDynamicsExample.h", "../BasicDemo/BasicExample.*", "../Tutorial/*", - "../ExtendedTutorials/SimpleBox.cpp", - "../ExtendedTutorials/MultipleBoxes.cpp", - "../ExtendedTutorials/SimpleJoint.cpp", - "../ExtendedTutorials/SimpleCloth.cpp", - "../ExtendedTutorials/Chain.cpp", - "../ExtendedTutorials/Bridge.cpp", - "../ExtendedTutorials/RigidBodyFromObj.cpp", + "../ExtendedTutorials/*", "../Evolution/NN3DWalkers.cpp", "../Evolution/NN3DWalkers.h", "../Collision/*", + "../RoboticsLearning/*", "../Collision/Internal/*", "../Benchmarks/*", "../CommonInterfaces/*", @@ -123,6 +119,7 @@ project "App_BulletExampleBrowser" "../ThirdPartyLibs/stb_image/*", "../ThirdPartyLibs/Wavefront/tiny_obj_loader.*", "../ThirdPartyLibs/tinyxml/*", + "../ThirdPartyLibs/BussIK/*", "../GyroscopicDemo/GyroscopicSetup.cpp", "../GyroscopicDemo/GyroscopicSetup.h", "../ThirdPartyLibs/tinyxml/tinystr.cpp", diff --git a/examples/ExtendedTutorials/InclinedPlane.cpp b/examples/ExtendedTutorials/InclinedPlane.cpp index dfe099094..dceafdbe8 100644 --- a/examples/ExtendedTutorials/InclinedPlane.cpp +++ b/examples/ExtendedTutorials/InclinedPlane.cpp @@ -89,7 +89,7 @@ void InclinedPlaneExample::initPhysics() { // create slider to change the ramp tilt SliderParams slider("Ramp Tilt",&gTilt); slider.m_minVal=0; - slider.m_maxVal=M_PI/2.0f; + slider.m_maxVal=SIMD_PI/2.0f; slider.m_clampToNotches = false; slider.m_callback = onRampInclinationChanged; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); diff --git a/examples/ExtendedTutorials/MultiPendulum.cpp b/examples/ExtendedTutorials/MultiPendulum.cpp index 8c4cff2e2..038aab3ec 100644 --- a/examples/ExtendedTutorials/MultiPendulum.cpp +++ b/examples/ExtendedTutorials/MultiPendulum.cpp @@ -38,6 +38,8 @@ static btScalar gInitialPendulumLength = 8; // Default pendulum length (distance 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) { @@ -47,21 +49,13 @@ struct MultiPendulumExample: public CommonRigidBodyBase { } virtual void initPhysics(); // build a multi pendulum - virtual void renderScene(); // render the scene to screen - - virtual void createMultiPendulum(btSphereShape* colShape, - btScalar pendulaQty, btScalar xPosition, btScalar yPosition,btScalar zPosition, - 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 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; @@ -72,7 +66,6 @@ struct MultiPendulumExample: public CommonRigidBodyBase { } std::vector constraints; // keep a handle to the slider constraints - std::vector pendula; // keep a handle to the pendula }; @@ -84,6 +77,8 @@ void onMultiPendulaRestitutionChanged(float pendulaRestitution); // change the p 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 @@ -135,6 +130,15 @@ void MultiPendulumExample::initPhysics() { // Setup your physics scene 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(); @@ -151,16 +155,14 @@ void MultiPendulumExample::initPhysics() { // Setup your physics scene { // create the multipendulum starting at the indicated position below and where each pendulum has the following mass btScalar pendulumMass(1.f); - btScalar xPosition(0.0f); // initial top-most pendulum position - btScalar yPosition(15.0f); - btScalar zPosition(0.0f); + 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), xPosition, yPosition,zPosition, + createMultiPendulum(pendulumShape, floor(gPendulaQty), position, gInitialPendulumLength, pendulumMass); } @@ -168,14 +170,16 @@ void MultiPendulumExample::initPhysics() { // Setup your physics scene } 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, btScalar xPosition, btScalar yPosition, btScalar zPosition, + btScalar pendulaQty, const btVector3& position, btScalar length, btScalar mass) { // The multi-pendulum looks like this (names when built): @@ -194,8 +198,7 @@ void MultiPendulumExample::createMultiPendulum(btSphereShape* colShape, startTransform.setIdentity(); // position the top sphere - startTransform.setOrigin( - btVector3(btScalar(xPosition), btScalar(yPosition), btScalar(zPosition))); + startTransform.setOrigin(position); startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation @@ -222,9 +225,7 @@ void MultiPendulumExample::createMultiPendulum(btSphereShape* colShape, // create joint element to make the pendulum rotate it // position the joint sphere at the same position as the top sphere - startTransform.setOrigin( - btVector3(btScalar(xPosition), btScalar(yPosition - length*(i)), - btScalar(0))); + startTransform.setOrigin(position - btVector3(0,length*(i),0)); startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation @@ -263,9 +264,7 @@ void MultiPendulumExample::createMultiPendulum(btSphereShape* colShape, startTransform.setIdentity(); // reset start transform // position the child sphere below the joint sphere - startTransform.setOrigin( - btVector3(btScalar(xPosition), btScalar(yPosition - length*(i+1)), - btScalar(0))); + startTransform.setOrigin(position - btVector3(0,length*(i+1),0)); startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation @@ -352,10 +351,10 @@ bool MultiPendulumExample::keyboardCallback(int key, int state) { //key 1, key 2, key 3 switch (key) { - case 49 /*ASCII for 1*/: { + case '1' /*ASCII for 1*/: { //assumption: Sphere are aligned in Z axis - btScalar newLimit = gCurrentPendulumLength + 0.1; + btScalar newLimit = btScalar(gCurrentPendulumLength + 0.1); changePendulaLength(newLimit); gCurrentPendulumLength = newLimit; @@ -363,10 +362,10 @@ bool MultiPendulumExample::keyboardCallback(int key, int state) { b3Printf("Increase pendulum length to %f", gCurrentPendulumLength); return true; } - case 50 /*ASCII for 2*/: { + case '2' /*ASCII for 2*/: { //assumption: Sphere are aligned in Z axis - btScalar newLimit = gCurrentPendulumLength - 0.1; + 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) { @@ -377,11 +376,8 @@ bool MultiPendulumExample::keyboardCallback(int key, int state) { b3Printf("Decrease pendulum length to %f", gCurrentPendulumLength); return true; } - case 51 /*ASCII for 3*/: { - for (int i = gPendulaQty-1; i >= gPendulaQty-gDisplacedPendula; i--) { - if (gDisplacedPendula >= 0 && gDisplacedPendula < gPendulaQty) - pendula[i]->applyCentralForce(btVector3(gDisplacementForce, 0, 0)); - } + case '3' /*ASCII for 3*/: { + applyPendulumForce(gDisplacementForce); return true; } } @@ -389,6 +385,16 @@ bool MultiPendulumExample::keyboardCallback(int key, int state) { 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 @@ -411,6 +417,17 @@ void floorMSliderValue(float notUsed) { // floor the slider values which should 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); diff --git a/examples/ExtendedTutorials/NewtonsCradle.cpp b/examples/ExtendedTutorials/NewtonsCradle.cpp index 76bbf6622..0d4c52095 100644 --- a/examples/ExtendedTutorials/NewtonsCradle.cpp +++ b/examples/ExtendedTutorials/NewtonsCradle.cpp @@ -30,7 +30,7 @@ static btScalar gPendulaQty = 5; // Number of pendula in newton's cradle 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 gPendulaRestitution = 1; // pendula restitution when hitting against each other static btScalar gSphereRadius = 1; // pendula radius @@ -38,7 +38,9 @@ static btScalar gCurrentPendulumLength = 8; // current pendula length static btScalar gInitialPendulumLength = 8; // default pendula length -static btScalar gForcingForce = 30; // default force to displace the pendula +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) : @@ -48,12 +50,12 @@ struct NewtonsCradleExample: public CommonRigidBodyBase { } virtual void initPhysics(); virtual void renderScene(); - virtual void createPendulum(btSphereShape* colShape, btScalar xPosition, - btScalar yPosition, btScalar zPosition, btScalar length, btScalar mass); + 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; @@ -63,17 +65,19 @@ struct NewtonsCradleExample: public CommonRigidBodyBase { targetPos[2]); } - std::vector constraints; - std::vector pendula; + std::vector constraints; // keep a handle to the slider constraints + std::vector pendula; // keep a handle to the pendula }; static NewtonsCradleExample* nex = NULL; -void onPendulaLengthChanged(float pendulaLength); +void onPendulaLengthChanged(float pendulaLength); // Change the pendula length -void onPendulaRestitutionChanged(float pendulaRestitution); +void onPendulaRestitutionChanged(float pendulaRestitution); // change the pendula restitution -void floorSliderValue(float notUsed); +void floorSliderValue(float notUsed); // floor the slider values which should be integers + +void applyForceWithForceScalar(float forceScalar); void NewtonsCradleExample::initPhysics() { @@ -118,7 +122,7 @@ void NewtonsCradleExample::initPhysics() { } { // create a slider to change the force to displace the lowest pendulum - SliderParams slider("Displacement force", &gForcingForce); + SliderParams slider("Displacement force", &gDisplacementForce); slider.m_minVal = 0.1; slider.m_maxVal = 200; slider.m_clampToNotches = false; @@ -126,6 +130,15 @@ void NewtonsCradleExample::initPhysics() { 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(); @@ -139,12 +152,11 @@ void NewtonsCradleExample::initPhysics() { + btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawConstraintLimits); - { // create the pendulum starting at the indicated position below and where each pendulum has the following mass + { // create the pendula starting at the indicated position below and where each pendulum has the following mass btScalar pendulumMass(1.f); - btScalar xPosition(0.0f); // initial left-most pendulum position - btScalar yPosition(15.0f); - btScalar zPosition(0.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); @@ -153,11 +165,10 @@ void NewtonsCradleExample::initPhysics() { for (int i = 0; i < floor(gPendulaQty); i++) { // create pendulum - createPendulum(pendulumShape, xPosition, yPosition,zPosition, - gInitialPendulumLength, pendulumMass); + createPendulum(pendulumShape, position, gInitialPendulumLength, pendulumMass); // displace the pendula 1.05 sphere size, so that they all nearly touch (small spacings in between - xPosition -= 2.1f * gSphereRadius; + position.setX(position.x()-2.1f * gSphereRadius); } } @@ -165,14 +176,15 @@ void NewtonsCradleExample::initPhysics() { } 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, - btScalar xPosition, btScalar yPosition, btScalar zPosition, btScalar length, btScalar mass) { +void NewtonsCradleExample::createPendulum(btSphereShape* colShape, const btVector3& position, btScalar length, btScalar mass) { // The pendulum looks like this (names when built): // O topSphere @@ -184,15 +196,14 @@ void NewtonsCradleExample::createPendulum(btSphereShape* colShape, startTransform.setIdentity(); // position the top sphere above ground with a moving x position - startTransform.setOrigin( - btVector3(btScalar(xPosition), btScalar(yPosition), btScalar(zPosition))); + 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(btScalar(xPosition), btScalar(yPosition - length), - btScalar(zPosition))); + btVector3(position.x(), btScalar(position.y() - length), + position.z())); startTransform.setRotation(btQuaternion(0, 0, 0, 1)); // zero rotation btRigidBody* bottomSphere = createRigidBody(mass, startTransform, colShape); @@ -286,10 +297,10 @@ bool NewtonsCradleExample::keyboardCallback(int key, int state) { //key 1, key 2, key 3 switch (key) { - case 49 /*ASCII for 1*/: { + case '1' /*ASCII for 1*/: { //assumption: Sphere are aligned in Z axis - btScalar newLimit = gCurrentPendulumLength + 0.1; + btScalar newLimit = btScalar(gCurrentPendulumLength + 0.1); changePendulaLength(newLimit); gCurrentPendulumLength = newLimit; @@ -297,10 +308,10 @@ bool NewtonsCradleExample::keyboardCallback(int key, int state) { b3Printf("Increase pendulum length to %f", gCurrentPendulumLength); return true; } - case 50 /*ASCII for 2*/: { + case '2' /*ASCII for 2*/: { //assumption: Sphere are aligned in Z axis - btScalar newLimit = gCurrentPendulumLength - 0.1; + 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) { @@ -311,11 +322,8 @@ bool NewtonsCradleExample::keyboardCallback(int key, int state) { b3Printf("Decrease pendulum length to %f", gCurrentPendulumLength); return true; } - case 51 /*ASCII for 3*/: { - for (int i = 0; i < gDisplacedPendula; i++) { - if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty) - pendula[i]->applyCentralForce(btVector3(gForcingForce, 0, 0)); - } + case '3' /*ASCII for 3*/: { + applyPendulumForce(gDisplacementForce); return true; } } @@ -323,6 +331,16 @@ bool NewtonsCradleExample::keyboardCallback(int key, int state) { 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) { @@ -341,6 +359,18 @@ void onPendulaRestitutionChanged(float 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( diff --git a/examples/ExtendedTutorials/NewtonsCradle.h b/examples/ExtendedTutorials/NewtonsCradle.h index 9b9e2f247..028754f7e 100644 --- a/examples/ExtendedTutorials/NewtonsCradle.h +++ b/examples/ExtendedTutorials/NewtonsCradle.h @@ -13,10 +13,10 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef ET_NEWTONIAN_PENDULUM_EXAMPLE_H -#define ET_NEWTONIAN_PENDULUM_EXAMPLE_H +#ifndef ET_NEWTONS_CRADLE_EXAMPLE_H +#define ET_NEWTONS_CRADLE_EXAMPLE_H class CommonExampleInterface* ET_NewtonsCradleCreateFunc(struct CommonExampleOptions& options); -#endif //ET_NEWTONIAN_PENDULUM_EXAMPLE_H +#endif //ET_NEWTONS_CRADLE_EXAMPLE_H diff --git a/examples/ExtendedTutorials/NewtonsRopeCradle.cpp b/examples/ExtendedTutorials/NewtonsRopeCradle.cpp new file mode 100644 index 000000000..94c96a71f --- /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 // TODO: Should I use another data structure? +#include + +#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 constraints; + std::vector 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;igetSoftBodyArray().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::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/ExtendedTutorials/NewtonsRopeCradle.h b/examples/ExtendedTutorials/NewtonsRopeCradle.h new file mode 100644 index 000000000..3edbcd5af --- /dev/null +++ b/examples/ExtendedTutorials/NewtonsRopeCradle.h @@ -0,0 +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 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 index 64d52793d..9e09f9dd9 100644 --- a/examples/ExtendedTutorials/RigidBodyFromObj.cpp +++ b/examples/ExtendedTutorials/RigidBodyFromObj.cpp @@ -134,7 +134,8 @@ void RigidBodyFromObjExample::initPhysics() int shapeId = m_guiHelper->registerGraphicsShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, &glmesh->m_indices->at(0), - glmesh->m_numIndices); + glmesh->m_numIndices, + B3_GL_TRIANGLES, -1); shape->setUserIndex(shapeId); int renderInstance = m_guiHelper->registerGraphicsInstance(shapeId,pos,orn,color,scaling); body->setUserIndex(renderInstance); diff --git a/examples/ExtendedTutorials/premake4.lua b/examples/ExtendedTutorials/premake4.lua index ea1fc507e..41126e0af 100644 --- a/examples/ExtendedTutorials/premake4.lua +++ b/examples/ExtendedTutorials/premake4.lua @@ -38,7 +38,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } @@ -89,7 +90,10 @@ files { "../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 @@ -153,7 +157,8 @@ files { "../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 @@ -213,6 +218,7 @@ files { "../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/Importers/ImportBsp/ImportBspExample.cpp b/examples/Importers/ImportBsp/ImportBspExample.cpp index 038342379..eec8a4fb9 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/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index 3f6553b81..67b64fd26 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -1,15 +1,12 @@ #include "b3ImportMeshUtility.h" #include -#include "../../OpenGLWindow/GLInstancingRenderer.h" -#include"Wavefront/tiny_obj_loader.h" -#include "../../OpenGLWindow/GLInstanceGraphicsShape.h" -#include "btBulletDynamicsCommon.h" -#include "../../OpenGLWindow/SimpleOpenGL3App.h" +#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" +#include "LinearMath/btVector3.h" #include "../ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h" #include "../../Utils/b3ResourcePath.h" #include "Bullet3Common/b3FileUtils.h" -#include "stb_image/stb_image.h" +#include "../../ThirdPartyLibs/stb_image/stb_image.h" bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData) @@ -88,28 +85,4 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& return false; } -int b3ImportMeshUtility::loadAndRegisterMeshFromFile(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; -} diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h index 96c061eaf..7bf6af841 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h @@ -15,7 +15,6 @@ struct b3ImportMeshData class b3ImportMeshUtility { public: -static int loadAndRegisterMeshFromFile(const std::string& fileName, class CommonRenderInterface* renderer); static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData); diff --git a/examples/Importers/ImportObjDemo/ImportObjExample.cpp b/examples/Importers/ImportObjDemo/ImportObjExample.cpp index a8995c148..2fc11afa3 100644 --- a/examples/Importers/ImportObjDemo/ImportObjExample.cpp +++ b/examples/Importers/ImportObjDemo/ImportObjExample.cpp @@ -56,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; +} @@ -77,7 +101,7 @@ void ImportObjSetup::initPhysics() btVector3 scaling(1,1,1); btVector3 color(1,1,1); - int shapeId = b3ImportMeshUtility::loadAndRegisterMeshFromFile(m_fileName, m_guiHelper->getRenderInterface()); + int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface()); if (shapeId>=0) { //int id = diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index 54883298b..f55cc9758 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -235,6 +235,60 @@ void ImportSDFSetup::initPhysics() ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true); mb = creation.getBulletMultiBody(); + + if (m_useMultiBody && mb ) + { + std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); + m_nameMemory.push_back(name); +#ifdef TEST_MULTIBODY_SERIALIZATION + s->registerNameForPointer(name->c_str(),name->c_str()); +#endif//TEST_MULTIBODY_SERIALIZATION + mb->setBaseName(name->c_str()); + //create motors for each btMultiBody joint + int numLinks = mb->getNumLinks(); + for (int i=0;iregisterNameForPointer(jointName->c_str(),jointName->c_str()); + s->registerNameForPointer(linkName->c_str(),linkName->c_str()); +#endif//TEST_MULTIBODY_SERIALIZATION + m_nameMemory.push_back(jointName); + m_nameMemory.push_back(linkName); + + mb->getLink(i).m_linkName = linkName->c_str(); + mb->getLink(i).m_jointName = jointName->c_str(); + + if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute + ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic + ) + { + if (m_data->m_numMotorsc_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); + float maxMotorImpulse = 10.1f; + btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse); + //motor->setMaxAppliedImpulse(0); + m_data->m_jointMotors[m_data->m_numMotors]=motor; + m_dynamicsWorld->addMultiBodyConstraint(motor); + m_data->m_numMotors++; + } + } + + } + } } diff --git a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp index b7f25c3dc..c05138ef1 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 f670dfeec..65bdd330b 100644 --- a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h +++ b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h @@ -66,17 +66,18 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName) for (int i=0;ivertex0[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 7e9624d64..425620c1a 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: #include "BulletUrdfImporter.h" - +#include "../../CommonInterfaces/CommonRenderInterface.h" #include "URDFImporterInterface.h" #include "btBulletCollisionCommon.h" @@ -26,12 +26,20 @@ subject to the following restrictions: #include #include "../../Utils/b3ResourcePath.h" +#include "../ImportMeshUtility/b3ImportMeshUtility.h" #include #include #include "UrdfParser.h" +struct MyTexture +{ + int m_width; + int m_height; + unsigned char* textureData; +}; + struct BulletURDFInternalData { UrdfParser m_urdfParser; @@ -100,7 +108,8 @@ struct BulletErrorLogger : public ErrorLogger bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) { - + if (strlen(fileName)==0) + return false; //int argc=0; char relativeFileName[1024]; @@ -124,7 +133,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); @@ -166,7 +175,7 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) m_data->m_pathPrefix[0] = 0; if (!fileFound){ - std::cerr << "URDF file not found" << std::endl; + std::cerr << "SDF file not found" << std::endl; return false; } else { @@ -267,14 +276,25 @@ 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; + if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase) + { + mass = 0.f; + localInertiaDiagonal.setValue(0,0,0); + } + else + { + mass = link->m_inertia.m_mass; + localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy, + link->m_inertia.m_izz); + } inertialFrame = link->m_inertia.m_linkLocalFrame; - localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy, - link->m_inertia.m_izz); + } else { @@ -578,7 +598,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co } -static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) +static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) { @@ -682,7 +702,23 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha { case FILE_OBJ: { - glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); +// 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; } @@ -903,7 +939,8 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP btAlignedObjectArray vertices; btAlignedObjectArray indices; btTransform startTrans; startTrans.setIdentity(); - + btAlignedObjectArray textures; + const UrdfModel& model = m_data->m_urdfParser.getModel(); UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex); if (linkPtr) @@ -923,14 +960,35 @@ 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); } - convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.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()); + //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); + + } + } + + //delete textures + for (int i=0;im_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) const { @@ -989,13 +1058,14 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index) { 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); - } + } } } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 14032e388..ea945aa33 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -37,6 +37,8 @@ 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; diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index e7a1607ad..46b69cc08 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -65,7 +65,8 @@ btAlignedObjectArray gFileNameArray; struct ImportUrdfInternalData { ImportUrdfInternalData() - :m_numMotors(0) + :m_numMotors(0), + m_mb(0) { for (int i=0;i