Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge commit.

Browse Source
This commit is contained in:
Benjamin Ellenberger
2016-07-31 16:22:50 +02:00
parent 46af3ddc54 0c0b4be665
commit 4622ab4299
273 changed files with 47372 additions and 1771 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
examples/BasicDemo/BasicExample.cpp
View File

@@ -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;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(btVector3(
btScalar(2.0*i),
btScalar(20+2.0*k),
btScalar(2.0*j)));
btScalar(0.2*i),
btScalar(2+.2*k),
btScalar(0.2*j)));
createRigidBody(mass,startTransform,colShape);
@@ -121,7 +121,9 @@ void BasicExample::initPhysics()
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}

1
examples/BasicDemo/CMakeLists.txt
View File

@@ -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'

72
examples/BasicDemo/premake4.lua
View File

@@ -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

2
examples/Benchmarks/BenchmarkDemo.cpp
View File

@@ -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);
}

5
examples/Collision/CollisionTutorialBullet2.cpp
View File

@@ -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()
{

4
examples/CommonInterfaces/CommonCameraInterface.h
View File

@@ -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;

2
examples/CommonInterfaces/CommonExampleInterface.h
View File

@@ -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

24
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

@@ -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;
}

2
examples/CommonInterfaces/CommonGraphicsAppInterface.h
View File

@@ -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
{

16
examples/CommonInterfaces/CommonMultiBodyBase.h
View File

@@ -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];

6
examples/CommonInterfaces/CommonRenderInterface.h
View File

@@ -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;

22
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -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

1
examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp
View File

@@ -167,6 +167,7 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans
}
}
}
delete hull;
}
} else
{

45
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -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),

1
examples/ExampleBrowser/GwenGUISupport/GraphingTexture.cpp
View File

@@ -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)

21
examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp
View File

@@ -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

5
examples/ExampleBrowser/GwenGUISupport/gwenInternalData.h
View File

@@ -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;

170
examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
View File

@@ -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;i<m_data->m_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);

1
examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.h
View File

@@ -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();

4
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@@ -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;

369
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -42,19 +42,65 @@
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
#include "../Importers/ImportBullet/SerializeSetup.h"
#include "Bullet3Common/b3HashMap.h"
struct GL3TexLoader : public MyTextureLoader
{
b3HashMap<b3HashString, GLint> m_hashMap;
virtual void LoadTexture(Gwen::Texture* pTexture)
{
Gwen::String namestr = pTexture->name.Get();
const char* n = namestr.c_str();
GLint* texIdPtr = m_hashMap[n];
if (texIdPtr)
{
pTexture->m_intData = *texIdPtr;
}
}
virtual void FreeTexture(Gwen::Texture* pTexture)
{
}
};
struct OpenGLExampleBrowserInternalData
{
Gwen::Renderer::Base* m_gwenRenderer;
CommonGraphicsApp* m_app;
// MyProfileWindow* m_profWindow;
btAlignedObjectArray<Gwen::Controls::TreeNode*> m_nodes;
GwenUserInterface* m_gui;
GL3TexLoader* m_myTexLoader;
struct MyMenuItemHander* m_handler2;
btAlignedObjectArray<MyMenuItemHander*> m_handlers;
OpenGLExampleBrowserInternalData()
: m_gwenRenderer(0),
m_app(0),
// m_profWindow(0),
m_gui(0),
m_myTexLoader(0),
m_handler2(0)
{
}
};
static CommonGraphicsApp* s_app=0;
static CommonWindowInterface* s_window = 0;
static CommonParameterInterface* s_parameterInterface=0;
static CommonRenderInterface* s_instancingRenderer=0;
static OpenGLGuiHelper* s_guiHelper=0;
static MyProfileWindow* s_profWindow =0;
//static MyProfileWindow* s_profWindow =0;
static SharedMemoryInterface* sSharedMem = 0;
#define DEMO_SELECTION_COMBOBOX 13
const char* startFileName = "0_Bullet3Demo.txt";
char staticPngFileName[1024];
static GwenUserInterface* gui = 0;
//static GwenUserInterface* gui = 0;
static GwenUserInterface* gui2 = 0;
static int sCurrentDemoIndex = -1;
static int sCurrentHightlighted = 0;
static CommonExampleInterface* sCurrentDemo = 0;
@@ -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<b3HashString,GLint> m_hashMap;
virtual void LoadTexture( Gwen::Texture* pTexture )
{
Gwen::String namestr = pTexture->name.Get();
const char* n = namestr.c_str();
GLint* texIdPtr = m_hashMap[n];
if (texIdPtr)
{
pTexture->m_intData = *texIdPtr;
}
}
virtual void FreeTexture( Gwen::Texture* pTexture )
{
}
};
void quitCallback()
@@ -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();
}
}

3
examples/ExampleBrowser/OpenGLExampleBrowser.h
View File

@@ -5,6 +5,9 @@
class OpenGLExampleBrowser : public ExampleBrowserInterface
{
struct OpenGLExampleBrowserInternalData* m_internalData;
public:
OpenGLExampleBrowser(class ExampleEntries* examples);

98
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@@ -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<unsigned char> m_rgbaPixelBuffer;
btAlignedObjectArray<float> m_depthBuffer;
btAlignedObjectArray<unsigned char> m_rgbaPixelBuffer1;
btAlignedObjectArray<float> 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;i<numRequestedPixels*numBytesPerPixel;i++)
{
if (pixelsRGBA)
{
pixelsRGBA[i] = m_data->m_rgbaPixelBuffer[i+startPixelIndex*numBytesPerPixel];
getRenderInterface()->setActiveCamera(oldCam);
{
btAlignedObjectArray<unsigned char> sourceRgbaPixelBuffer;
btAlignedObjectArray<float> sourceDepthBuffer;
//copy the image into our local cache
sourceRgbaPixelBuffer.resize(sourceWidth*sourceHeight*numBytesPerPixel);
sourceDepthBuffer.resize(sourceWidth*sourceHeight);
m_data->m_glApp->getScreenPixels(&(sourceRgbaPixelBuffer[0]),sourceRgbaPixelBuffer.size(), &sourceDepthBuffer[0],sizeof(float)*sourceDepthBuffer.size());
m_data->m_rgbaPixelBuffer1.resize(destinationWidth*destinationHeight*numBytesPerPixel);
m_data->m_depthBuffer1.resize(destinationWidth*destinationHeight);
//rescale and flip
for (int i=0;i<destinationWidth;i++)
{
for (int j=0;j<destinationHeight;j++)
{
int xIndex = int(float(i)*(float(sourceWidth)/float(destinationWidth)));
int yIndex = int(float(destinationHeight-1-j)*(float(sourceHeight)/float(destinationHeight)));
btClamp(xIndex,0,sourceWidth);
btClamp(yIndex,0,sourceHeight);
int bytesPerPixel = 4; //RGBA
int sourcePixelIndex = (xIndex+yIndex*sourceWidth)*bytesPerPixel;
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+0] = sourceRgbaPixelBuffer[sourcePixelIndex+0];
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+1] = sourceRgbaPixelBuffer[sourcePixelIndex+1];
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+2] = sourceRgbaPixelBuffer[sourcePixelIndex+2];
m_data->m_rgbaPixelBuffer1[(i+j*destinationWidth)*4+3] = 255;
}
}
}
}
if (pixelsRGBA)
{
for (int i=0;i<numRequestedPixels*numBytesPerPixel;i++)
{
pixelsRGBA[i] = m_data->m_rgbaPixelBuffer1[i+startPixelIndex*numBytesPerPixel];
}
}
if (depthBuffer)
{
for (int i=0;i<numRequestedPixels;i++)
{
depthBuffer[i] = m_data->m_depthBuffer1[i];
}
}
if (numPixelsCopied)
*numPixelsCopied = numRequestedPixels;

9
examples/ExampleBrowser/OpenGLGuiHelper.h
View File

@@ -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) ;

55
examples/ExampleBrowser/main.cpp
View File

@@ -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;
}

13
examples/ExampleBrowser/premake4.lua
View File

@@ -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",

2
examples/ExtendedTutorials/InclinedPlane.cpp
View File

@@ -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);

85
examples/ExtendedTutorials/MultiPendulum.cpp
View File

@@ -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<btSliderConstraint*> constraints; // keep a handle to the slider constraints
std::vector<btRigidBody*> 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);

96
examples/ExtendedTutorials/NewtonsCradle.cpp
View File

@@ -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<btSliderConstraint*> constraints;
std::vector<btRigidBody*> pendula;
std::vector<btSliderConstraint*> constraints; // keep a handle to the slider constraints
std::vector<btRigidBody*> pendula; // keep a handle to the pendula
};
static NewtonsCradleExample* nex = NULL;
void onPendulaLengthChanged(float pendulaLength);
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(

6
examples/ExtendedTutorials/NewtonsCradle.h
View File

@@ -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

387
examples/ExtendedTutorials/NewtonsRopeCradle.cpp Normal file
View File

@@ -0,0 +1,387 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2015 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "NewtonsRopeCradle.h"
#include <vector> // TODO: Should I use another data structure?
#include <iterator>
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
static btScalar gPendulaQty = 5; // Number of pendula in newton's cradle
//TODO: This would actually be an Integer, but the Slider does not like integers, so I floor it when changed
static btScalar gDisplacedPendula = 1; // number of displaced pendula
//TODO: This is an int as well
static btScalar gPendulaRestitution = 1; // pendula restition when hitting against each other
static btScalar gSphereRadius = 1; // pendula radius
static btScalar gInitialPendulumWidth = 4; // default pendula width
static btScalar gInitialPendulumHeight = 8; // default pendula height
static btScalar gRopeResolution = 1; // default rope resolution (number of links as in a chain)
static btScalar gDisplacementForce = 30; // default force to displace the pendula
static btScalar gForceScalar = 0; // default force scalar to apply a displacement
struct NewtonsRopeCradleExample : public CommonRigidBodyBase {
NewtonsRopeCradleExample(struct GUIHelperInterface* helper) :
CommonRigidBodyBase(helper) {
}
virtual ~NewtonsRopeCradleExample(){}
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void renderScene();
virtual void applyPendulumForce(btScalar pendulumForce);
void createEmptyDynamicsWorld()
{
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
m_solver = new btSequentialImpulseConstraintSolver;
m_dynamicsWorld = new btSoftRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
softBodyWorldInfo.m_broadphase = m_broadphase;
softBodyWorldInfo.m_dispatcher = m_dispatcher;
softBodyWorldInfo.m_gravity = m_dynamicsWorld->getGravity();
softBodyWorldInfo.m_sparsesdf.Initialize();
}
virtual void createRopePendulum(btSphereShape* colShape,
const btVector3& position, const btQuaternion& pendulumOrientation, btScalar width, btScalar height, btScalar mass);
virtual void changePendulaRestitution(btScalar restitution);
virtual void connectWithRope(btRigidBody* body1, btRigidBody* body2);
virtual bool keyboardCallback(int key, int state);
virtual btSoftRigidDynamicsWorld* getSoftDynamicsWorld()
{
///just make it a btSoftRigidDynamicsWorld please
///or we will add type checking
return (btSoftRigidDynamicsWorld*) m_dynamicsWorld;
}
void resetCamera()
{
float dist = 41;
float pitch = 52;
float yaw = 35;
float targetPos[3]={0,0.46,0};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
std::vector<btSliderConstraint*> constraints;
std::vector<btRigidBody*> pendula;
btSoftBodyWorldInfo softBodyWorldInfo;
};
static NewtonsRopeCradleExample* nex = NULL;
void onRopePendulaRestitutionChanged(float pendulaRestitution);
void floorRSliderValue(float notUsed);
void applyRForceWithForceScalar(float forceScalar);
void NewtonsRopeCradleExample::initPhysics()
{
{ // create a slider to change the number of pendula
SliderParams slider("Number of Pendula", &gPendulaQty);
slider.m_minVal = 1;
slider.m_maxVal = 50;
slider.m_callback = floorRSliderValue; // hack to get integer values
slider.m_clampToNotches = false;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
{ // create a slider to change the number of displaced pendula
SliderParams slider("Number of Displaced Pendula", &gDisplacedPendula);
slider.m_minVal = 0;
slider.m_maxVal = 49;
slider.m_callback = floorRSliderValue; // hack to get integer values
slider.m_clampToNotches = false;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
{ // create a slider to change the pendula restitution
SliderParams slider("Pendula Restitution", &gPendulaRestitution);
slider.m_minVal = 0;
slider.m_maxVal = 1;
slider.m_clampToNotches = false;
slider.m_callback = onRopePendulaRestitutionChanged;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
{ // create a slider to change the rope resolution
SliderParams slider("Rope Resolution", &gRopeResolution);
slider.m_minVal = 1;
slider.m_maxVal = 20;
slider.m_clampToNotches = false;
slider.m_callback = floorRSliderValue;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
{ // create a slider to change the pendulum width
SliderParams slider("Pendulum Width", &gInitialPendulumWidth);
slider.m_minVal = 0;
slider.m_maxVal = 40;
slider.m_clampToNotches = false;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
{ // create a slider to change the pendulum height
SliderParams slider("Pendulum Height", &gInitialPendulumHeight);
slider.m_minVal = 0;
slider.m_maxVal = 40;
slider.m_clampToNotches = false;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
{ // create a slider to change the force to displace the lowest pendulum
SliderParams slider("Displacement force", &gDisplacementForce);
slider.m_minVal = 0.1;
slider.m_maxVal = 200;
slider.m_clampToNotches = false;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
{ // create a slider to apply the force by slider
SliderParams slider("Apply displacement force", &gForceScalar);
slider.m_minVal = -1;
slider.m_maxVal = 1;
slider.m_clampToNotches = false;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider);
}
m_guiHelper->setUpAxis(1);
createEmptyDynamicsWorld();
// create a debug drawer
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
if (m_dynamicsWorld->getDebugDrawer())
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawWireframe
+ btIDebugDraw::DBG_DrawContactPoints
+ btIDebugDraw::DBG_DrawConstraints
+ btIDebugDraw::DBG_DrawConstraintLimits);
{ // create the pendula starting at the indicated position below and where each pendulum has the following mass
btScalar pendulumMass(1.0f);
btVector3 position(0.0f,15.0f,0.0f); // initial left-most pendulum position
btQuaternion orientation(0,0,0,1); // orientation of the pendula
// Re-using the same collision is better for memory usage and performance
btSphereShape* pendulumShape = new btSphereShape(gSphereRadius);
m_collisionShapes.push_back(pendulumShape);
for (int i = 0; i < floor(gPendulaQty); i++) {
// create pendulum
createRopePendulum(pendulumShape, position, orientation,gInitialPendulumWidth,
gInitialPendulumHeight, pendulumMass);
// displace the pendula 1.05 sphere size, so that they all nearly touch (small spacings in between)
position.setX(position.x()-2.1f * gSphereRadius);
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void NewtonsRopeCradleExample::connectWithRope(btRigidBody* body1, btRigidBody* body2)
{
btSoftBody* softBodyRope0 = btSoftBodyHelpers::CreateRope(softBodyWorldInfo,body1->getWorldTransform().getOrigin(),body2->getWorldTransform().getOrigin(),gRopeResolution,0);
softBodyRope0->setTotalMass(0.1f);
softBodyRope0->appendAnchor(0,body1);
softBodyRope0->appendAnchor(softBodyRope0->m_nodes.size()-1,body2);
softBodyRope0->m_cfg.piterations = 5;
softBodyRope0->m_cfg.kDP = 0.005f;
softBodyRope0->m_cfg.kSHR = 1;
softBodyRope0->m_cfg.kCHR = 1;
softBodyRope0->m_cfg.kKHR = 1;
getSoftDynamicsWorld()->addSoftBody(softBodyRope0);
}
void NewtonsRopeCradleExample::stepSimulation(float deltaTime) {
applyRForceWithForceScalar(gForceScalar); // apply force defined by apply force slider
if (m_dynamicsWorld) {
m_dynamicsWorld->stepSimulation(deltaTime);
}
}
void NewtonsRopeCradleExample::createRopePendulum(btSphereShape* colShape,
const btVector3& position, const btQuaternion& pendulumOrientation, btScalar width, btScalar height, btScalar mass) {
// The pendulum looks like this (names when built):
// O O topSphere1 topSphere2
// \ /
// O bottomSphere
//create a dynamic pendulum
btTransform startTransform;
startTransform.setIdentity();
// calculate sphere positions
btVector3 topSphere1RelPosition(0,0,width);
btVector3 topSphere2RelPosition(0,0,-width);
btVector3 bottomSphereRelPosition(0,-height,0);
// position the top sphere above ground with appropriate orientation
startTransform.setOrigin(btVector3(0,0,0)); // no translation intitially
startTransform.setRotation(pendulumOrientation); // pendulum rotation
startTransform.setOrigin(startTransform * topSphere1RelPosition); // rotate this position
startTransform.setOrigin(position + startTransform.getOrigin()); // add non-rotated position to the relative position
btRigidBody* topSphere1 = createRigidBody(0, startTransform, colShape); // make top sphere static
// position the top sphere above ground with appropriate orientation
startTransform.setOrigin(btVector3(0,0,0)); // no translation intitially
startTransform.setRotation(pendulumOrientation); // pendulum rotation
startTransform.setOrigin(startTransform * topSphere2RelPosition); // rotate this position
startTransform.setOrigin(position + startTransform.getOrigin()); // add non-rotated position to the relative position
btRigidBody* topSphere2 = createRigidBody(0, startTransform, colShape); // make top sphere static
// position the bottom sphere below the top sphere
startTransform.setOrigin(btVector3(0,0,0)); // no translation intitially
startTransform.setRotation(pendulumOrientation); // pendulum rotation
startTransform.setOrigin(startTransform * bottomSphereRelPosition); // rotate this position
startTransform.setOrigin(position + startTransform.getOrigin()); // add non-rotated position to the relative position
btRigidBody* bottomSphere = createRigidBody(mass, startTransform, colShape);
bottomSphere->setFriction(0); // we do not need friction here
pendula.push_back(bottomSphere);
// disable the deactivation when objects do not move anymore
topSphere1->setActivationState(DISABLE_DEACTIVATION);
topSphere2->setActivationState(DISABLE_DEACTIVATION);
bottomSphere->setActivationState(DISABLE_DEACTIVATION);
bottomSphere->setRestitution(gPendulaRestitution); // set pendula restitution
// add ropes between spheres
connectWithRope(topSphere1, bottomSphere);
connectWithRope(topSphere2, bottomSphere);
}
void NewtonsRopeCradleExample::renderScene()
{
CommonRigidBodyBase::renderScene();
btSoftRigidDynamicsWorld* softWorld = getSoftDynamicsWorld();
for ( int i=0;i<softWorld->getSoftBodyArray().size();i++)
{
btSoftBody* psb=(btSoftBody*)softWorld->getSoftBodyArray()[i];
//if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
btSoftBodyHelpers::DrawFrame(psb,softWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb,softWorld->getDebugDrawer(),softWorld->getDrawFlags());
}
}
}
void NewtonsRopeCradleExample::changePendulaRestitution(btScalar restitution) {
for (std::vector<btRigidBody*>::iterator rit = pendula.begin();
rit != pendula.end(); rit++) {
btAssert((*rit) && "Null constraint");
(*rit)->setRestitution(restitution);
}
}
bool NewtonsRopeCradleExample::keyboardCallback(int key, int state) {
//b3Printf("Key pressed: %d in state %d \n",key,state);
// key 3
switch (key) {
case '3' /*ASCII for 3*/: {
applyPendulumForce(gDisplacementForce);
return true;
}
}
return false;
}
void NewtonsRopeCradleExample::applyPendulumForce(btScalar pendulumForce){
if(pendulumForce != 0){
b3Printf("Apply %f to pendulum",pendulumForce);
for (int i = 0; i < gDisplacedPendula; i++) {
if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty)
pendula[i]->applyCentralForce(btVector3(pendulumForce, 0, 0));
}
}
}
// GUI parameter modifiers
void onRopePendulaRestitutionChanged(float pendulaRestitution) {
if (nex){
nex->changePendulaRestitution(pendulaRestitution);
}
}
void floorRSliderValue(float notUsed) {
gPendulaQty = floor(gPendulaQty);
gDisplacedPendula = floor(gDisplacedPendula);
gRopeResolution = floor(gRopeResolution);
}
void applyRForceWithForceScalar(float forceScalar) {
if(nex){
btScalar appliedForce = forceScalar * gDisplacementForce;
if(fabs(gForceScalar) < 0.2f)
gForceScalar = 0;
nex->applyPendulumForce(appliedForce);
}
}
CommonExampleInterface* ET_NewtonsRopeCradleCreateFunc(
CommonExampleOptions& options) {
nex = new NewtonsRopeCradleExample(options.m_guiHelper);
return nex;
}

22
examples/ExtendedTutorials/NewtonsRopeCradle.h Normal file
View File

@@ -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

3
examples/ExtendedTutorials/RigidBodyFromObj.cpp
View File

@@ -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);

14
examples/ExtendedTutorials/premake4.lua
View File

@@ -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",
}

2
examples/Importers/ImportBsp/ImportBspExample.cpp
View File

@@ -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);

33
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
View File

@@ -1,15 +1,12 @@
#include "b3ImportMeshUtility.h"
#include <vector>
#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;
}

1
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h
View File

@@ -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);

28
examples/Importers/ImportObjDemo/ImportObjExample.cpp
View File

@@ -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 =

54
examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
View File

@@ -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;i<numLinks;i++)
{
int mbLinkIndex = i;
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(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_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", jointName->c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
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++;
}
}
}
}
}

31
examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp
View File

@@ -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);
}

11
examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
View File

@@ -66,17 +66,18 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
for (int i=0;i<numTriangles;i++)
{
char* curPtr = &memoryBuffer[84+i*50];
MySTLTriangle* tri = (MySTLTriangle*) curPtr;
MySTLTriangle tmp;
memcpy(&tmp,curPtr,sizeof(MySTLTriangle));
GLInstanceVertex v0,v1,v2;
v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
for (int v=0;v<3;v++)
{
v0.xyzw[v] = tri->vertex0[v];
v1.xyzw[v] = tri->vertex1[v];
v2.xyzw[v] = tri->vertex2[v];
v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v];
v0.xyzw[v] = tmp.vertex0[v];
v1.xyzw[v] = tmp.vertex1[v];
v2.xyzw[v] = tmp.vertex2[v];
v0.normal[v] = v1.normal[v] = v2.normal[v] = tmp.normal[v];
}
v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;

96
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -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 <string>
#include "../../Utils/b3ResourcePath.h"
#include "../ImportMeshUtility/b3ImportMeshUtility.h"
#include <iostream>
#include <fstream>
#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<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture>& 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<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
btAlignedObjectArray<MyTexture> textures;
const UrdfModel& model = m_data->m_urdfParser.getModel();
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
if (linkPtr)
@@ -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;i<textures.size();i++)
{
delete textures[i].textureData;
}
return graphicsIndex;
}
@@ -947,6 +1005,17 @@ bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
return false;
}
bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const
{
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
if (linkPtr)
{
const UrdfLink* link = *linkPtr;
contactInfo = link->m_contactInfo;
return true;
}
return false;
}
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj) 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);
}
}
}
}

2
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -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;

21
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -65,7 +65,8 @@ btAlignedObjectArray<std::string> gFileNameArray;
struct ImportUrdfInternalData
{
ImportUrdfInternalData()
:m_numMotors(0)
:m_numMotors(0),
m_mb(0)
{
for (int i=0;i<MAX_NUM_MOTORS;i++)
{
@@ -74,10 +75,13 @@ struct ImportUrdfInternalData
}
}
btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
int m_numMotors;
btMultiBody* m_mb;
btRigidBody* m_rb;
};
@@ -203,7 +207,7 @@ void ImportUrdfSetup::initPhysics()
BulletURDFImporter u2b(m_guiHelper, 0);
bool loadOk = u2b.loadURDF(m_fileName);
bool loadOk = u2b.loadURDF(m_fileName);
#ifdef TEST_MULTIBODY_SERIALIZATION
//test to serialize a multibody to disk or shared memory, with base, link and joint names
@@ -223,7 +227,6 @@ void ImportUrdfSetup::initPhysics()
{
btMultiBody* mb = 0;
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
@@ -232,7 +235,13 @@ void ImportUrdfSetup::initPhysics()
MyMultiBodyCreator creation(m_guiHelper);
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
mb = creation.getBulletMultiBody();
m_data->m_rb = creation.getRigidBody();
m_data->m_mb = creation.getBulletMultiBody();
btMultiBody* mb = m_data->m_mb;
for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
{
m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
}
if (m_useMultiBody && mb )
{
@@ -343,6 +352,7 @@ void ImportUrdfSetup::initPhysics()
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
m_collisionShapes.push_back(box);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
@@ -357,8 +367,7 @@ void ImportUrdfSetup::initPhysics()
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
}
#ifdef TEST_MULTIBODY_SERIALIZATION

7
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -13,6 +13,7 @@
MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
:m_bulletMultiBody(0),
m_rigidBody(0),
m_guiHelper(guiHelper)
{
}
@@ -31,10 +32,10 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
{
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
rbci.m_startWorldTransform = initialWorldTrans;
btRigidBody* body = new btRigidBody(rbci);
body->forceActivationState(DISABLE_DEACTIVATION);
m_rigidBody = new btRigidBody(rbci);
m_rigidBody->forceActivationState(DISABLE_DEACTIVATION);
return body;
return m_rigidBody;
}
class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)

5
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h
View File

@@ -24,6 +24,7 @@ class MyMultiBodyCreator : public MultiBodyCreationInterface
protected:
btMultiBody* m_bulletMultiBody;
btRigidBody* m_rigidBody;
struct GUIHelperInterface* m_guiHelper;
@@ -62,6 +63,10 @@ public:
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
btMultiBody* getBulletMultiBody();
btRigidBody* getRigidBody()
{
return m_rigidBody;
}
int getNum6DofConstraints() const
{

20
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -232,6 +232,13 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
if (mass)
{
compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
//temporary inertia scaling until we load inertia from URDF
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
{
localInertiaDiagonal*=contactInfo.m_inertiaScaling;
}
}
btRigidBody* linkRigidBody = 0;
@@ -392,9 +399,18 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col);
btScalar friction = 0.5f;
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
col->setFriction(friction);
if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0)
{
col->setFriction(contactInfo.m_lateralFriction);
}
if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION)!=0)
{
col->setRollingFriction(contactInfo.m_rollingFriction);
}
if (mbLinkIndex>=0) //???? double-check +/- 1
{

4
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -6,6 +6,7 @@
#include "LinearMath/btTransform.h"
#include "URDFJointTypes.h"
class URDFImporterInterface
{
@@ -28,6 +29,9 @@ public:
virtual std::string getLinkName(int linkIndex) const =0;
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;}
///this API will likely change, don't override it!
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;}
virtual std::string getJointName(int linkIndex) const = 0;

30
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@@ -11,6 +11,36 @@ enum UrdfJointTypes
URDFPlanarJoint,
URDFFixedJoint,
};
#include "LinearMath/btScalar.h"
enum URDF_LinkContactFlags
{
URDF_CONTACT_HAS_LATERAL_FRICTION=1,
URDF_CONTACT_HAS_ROLLING_FRICTION=2,
URDF_CONTACT_HAS_INERTIA_SCALING=2,
URDF_CONTACT_HAS_CONTACT_CFM=4,
URDF_CONTACT_HAS_CONTACT_ERP=8
};
struct URDFLinkContactInfo
{
btScalar m_lateralFriction;
btScalar m_rollingFriction;
btScalar m_inertiaScaling;
btScalar m_contactCfm;
btScalar m_contactErp;
int m_flags;
URDFLinkContactInfo()
:m_lateralFriction(0.5),
m_rollingFriction(0),
m_inertiaScaling(1),
m_contactCfm(0),
m_contactErp(0)
{
m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
}
};
#endif //URDF_JOINT_TYPES_H

206
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -61,7 +61,9 @@ static bool parseVector4(btVector4& vec4, const std::string& vector_str)
vec4.setZero();
btArray<std::string> pieces;
btArray<float> rgba;
urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" "));
btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, vector_str, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
@@ -82,7 +84,9 @@ static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLo
vec3.setZero();
btArray<std::string> pieces;
btArray<float> rgba;
urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" "));
btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, vector_str, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
@@ -225,6 +229,15 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
{
inertia.m_linkLocalFrame.setIdentity();
inertia.m_mass = 0.f;
if(m_parseSDF)
{
TiXmlElement* pose = config->FirstChildElement("pose");
if (pose)
{
parseTransform(inertia.m_linkLocalFrame, pose,logger,m_parseSDF);
}
}
// Origin
@@ -393,13 +406,22 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
}
geom.m_meshFileName = shape->Attribute("filename");
if (shape->Attribute("scale"))
geom.m_meshScale.setValue(1,1,1);
if (shape->Attribute("scale"))
{
parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger);
if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger))
{
logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n");
std::string scalar_str = shape->Attribute("scale");
double scaleFactor = urdfLexicalCast<double>(scalar_str.c_str());
if (scaleFactor)
{
geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor);
}
}
} else
{
geom.m_meshScale.setValue(1,1,1);
}
}
}
@@ -439,6 +461,15 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
collision.m_linkLocalFrame.setIdentity();
if(m_parseSDF)
{
TiXmlElement* pose = config->FirstChildElement("pose");
if (pose)
{
parseTransform(collision.m_linkLocalFrame, pose,logger,m_parseSDF);
}
}
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
@@ -465,7 +496,15 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
{
visual.m_linkLocalFrame.setIdentity();
if(m_parseSDF)
{
TiXmlElement* pose = config->FirstChildElement("pose");
if (pose)
{
parseTransform(visual.m_linkLocalFrame, pose,logger,m_parseSDF);
}
}
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
@@ -490,29 +529,49 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
// Material
TiXmlElement *mat = config->FirstChildElement("material");
//todo(erwincoumans) skip materials in SDF for now (due to complexity)
if (mat && !m_parseSDF)
if (mat)
{
// get material name
if (!mat->Attribute("name"))
{
logger->reportError("Visual material must contain a name attribute");
return false;
}
visual.m_materialName = mat->Attribute("name");
// try to parse material element in place
TiXmlElement *t = mat->FirstChildElement("texture");
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_hasLocalMaterial = true;
}
}
if (m_parseSDF)
{
UrdfMaterial* matPtr = new UrdfMaterial;
matPtr->m_name = "mat";
if (name_char)
matPtr->m_name = name_char;
TiXmlElement *diffuse = mat->FirstChildElement("diffuse");
if (diffuse) {
std::string diffuseText = diffuse->GetText();
btVector4 rgba(1,0,0,1);
parseVector4(rgba,diffuseText);
matPtr->m_rgbaColor = rgba;
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_materialName = matPtr->m_name;
visual.m_hasLocalMaterial = true;
}
}
else
{
// get material name
if (!mat->Attribute("name"))
{
logger->reportError("Visual material must contain a name attribute");
return false;
}
visual.m_materialName = mat->Attribute("name");
// try to parse material element in place
TiXmlElement *t = mat->FirstChildElement("texture");
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_hasLocalMaterial = true;
}
}
}
}
return true;
@@ -529,6 +588,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
@@ -540,6 +601,54 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
}
{
//optional 'contact' parameters
TiXmlElement* ci = config->FirstChildElement("contact");
if (ci)
{
TiXmlElement *damping_xml = ci->FirstChildElement("inertia_scaling");
if (damping_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
} else
{
if (!damping_xml->Attribute("value"))
{
logger->reportError("Link/contact: damping element must have value attribute");
return false;
}
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
}
}
{
TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
if (friction_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->GetText());
} else
{
if (!friction_xml->Attribute("value"))
{
logger->reportError("Link/contact: lateral_friction element must have value attribute");
return false;
}
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
}
}
}
}
}
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
@@ -572,7 +681,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
logger->reportWarning(link.m_name.c_str());
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
@@ -616,6 +725,8 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL
joint.m_upperLimit = 0.f;
joint.m_effortLimit = 0.f;
joint.m_velocityLimit = 0.f;
joint.m_jointDamping = 0.f;
joint.m_jointFriction = 0.f;
if (m_parseSDF)
{
@@ -872,7 +983,7 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
return false;
}
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
TiXmlElement *prop_xml = axis_xml->FirstChildElement("dynamics");
if (prop_xml)
{
if (!parseJointDynamics(joint, prop_xml,logger))
@@ -1227,19 +1338,36 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
return false;
}
TiXmlElement *world_xml = sdf_xml->FirstChildElement("world");
if (!world_xml)
{
logger->reportError("expected a world element");
return false;
}
//apparently, SDF doesn't require a "world" element, optional? URDF does.
TiXmlElement *world_xml = sdf_xml->FirstChildElement("world");
TiXmlElement* robot_xml = 0;
if (!world_xml)
{
logger->reportWarning("expected a world element, continuing without it.");
robot_xml = sdf_xml->FirstChildElement("model");
} else
{
robot_xml = world_xml->FirstChildElement("model");
}
// Get all model (robot) elements
for (TiXmlElement* robot_xml = world_xml->FirstChildElement("model"); robot_xml; robot_xml = robot_xml->NextSiblingElement("model"))
for (; robot_xml; robot_xml = robot_xml->NextSiblingElement("model"))
{
UrdfModel* localModel = new UrdfModel;
m_tmpModels.push_back(localModel);
TiXmlElement* stat = robot_xml->FirstChildElement("static");
if (0!=stat)
{
int val = int(atof(stat->GetText()));
if (val==1)
{
localModel->m_overrideFixedBase = true;
}
}
// Get robot name
const char *name = robot_xml->Attribute("name");

18
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -99,6 +99,8 @@ struct UrdfLink
int m_linkIndex;
URDFLinkContactInfo m_contactInfo;
UrdfLink()
:m_parentLink(0),
m_parentJoint(0)
@@ -123,6 +125,15 @@ struct UrdfJoint
double m_jointDamping;
double m_jointFriction;
UrdfJoint()
:m_lowerLimit(0),
m_upperLimit(0),
m_effortLimit(0),
m_velocityLimit(0),
m_jointDamping(0),
m_jointFriction(0)
{
}
};
struct UrdfModel
@@ -134,6 +145,13 @@ struct UrdfModel
btHashMap<btHashString, UrdfJoint*> m_joints;
btArray<UrdfLink*> m_rootLinks;
bool m_overrideFixedBase;
UrdfModel()
:m_overrideFixedBase(false)
{
m_rootTransformInWorld.setIdentity();
}
};

9
examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
View File

@@ -8,7 +8,7 @@
#include "urdfStringSplit.h"
void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, btAlignedObjectArray<std::string> separators)
void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators)
{
assert(separators.size()==1);
if (separators.size()==1)
@@ -20,10 +20,9 @@ void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::strin
urdfStrArrayFree(strArray);
}
}
btAlignedObjectArray<std::string> urdfIsAnyOf(const char* seps)
void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray)
{
btAlignedObjectArray<std::string> strArray;
int numSeps = strlen(seps);
for (int i=0;i<numSeps;i++)
{
@@ -32,8 +31,6 @@ void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::strin
sep2[0] = seps[i];
strArray.push_back(sep2);
}
return strArray;
}

4
examples/Importers/ImportURDFDemo/urdfStringSplit.h
View File

@@ -7,9 +7,9 @@
#include "LinearMath/btAlignedObjectArray.h"
void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, btAlignedObjectArray<std::string> separators);
void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators);
btAlignedObjectArray<std::string> urdfIsAnyOf(const char* seps);
void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray);
///The string split C code is by Lars Wirzenius
///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char

27
examples/InverseDynamics/InverseDynamicsExample.cpp
View File

@@ -105,7 +105,7 @@ InverseDynamicsExample::InverseDynamicsExample(struct GUIHelperInterface* helper
InverseDynamicsExample::~InverseDynamicsExample()
{
delete m_multiBody;
delete m_inverseModel;
delete m_timeSeriesCanvas;
}
@@ -157,7 +157,7 @@ void InverseDynamicsExample::initPhysics()
BulletURDFImporter u2b(m_guiHelper,0);
bool loadOk = u2b.loadURDF("kuka_lwr/kuka.urdf");
bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf");
if (loadOk)
{
int rootLinkIndex = u2b.getRootLinkIndex();
@@ -166,6 +166,10 @@ void InverseDynamicsExample::initPhysics()
btTransform identityTrans;
identityTrans.setIdentity();
ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix());
for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
{
m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
}
m_multiBody = creation.getBulletMultiBody();
if (m_multiBody)
{
@@ -221,13 +225,13 @@ void InverseDynamicsExample::initPhysics()
{
qd[dof] = 0;
char tmp[25];
sprintf(tmp,"q_desired[%zu]",dof);
sprintf(tmp,"q_desired[%u]",dof);
qd_name[dof] = tmp;
SliderParams slider(qd_name[dof].c_str(),&qd[dof]);
slider.m_minVal=-3.14;
slider.m_maxVal=3.14;
sprintf(tmp,"q[%zu]",dof);
sprintf(tmp,"q[%u]",dof);
q_name[dof] = tmp;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
btVector4 color = sJointCurveColors[dof&7];
@@ -336,6 +340,21 @@ void InverseDynamicsExample::stepSimulation(float deltaTime)
// todo(thomas) check that this is correct:
// want to advance by 10ms, with 1ms timesteps
m_dynamicsWorld->stepSimulation(1e-3,0);//,1e-3);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
m_multiBody->forwardKinematics(scratch_q, scratch_m);
for (int i = 0; i < m_multiBody->getNumLinks(); i++)
{
//btVector3 pos = m_multiBody->getLink(i).m_cachedWorldTransform.getOrigin();
btTransform tr = m_multiBody->getLink(i).m_cachedWorldTransform;
btVector3 pos = tr.getOrigin() - quatRotate(tr.getRotation(), m_multiBody->getLink(i).m_dVector);
btVector3 localAxis = m_multiBody->getLink(i).m_axes[0].m_topVec;
//printf("link %d: %f,%f,%f, local axis:%f,%f,%f\n", i, pos.x(), pos.y(), pos.z(), localAxis.x(), localAxis.y(), localAxis.z());
}
}
}

13
examples/InverseDynamics/premake4.lua
View File

@@ -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,9 @@ 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",
}
if os.is("Linux") then initX11() end
@@ -153,7 +156,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
@@ -211,6 +215,7 @@ files {
"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}

385
examples/InverseKinematics/InverseKinematicsExample.cpp Normal file
View File

@@ -0,0 +1,385 @@
#include "InverseKinematicsExample.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Transform.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../OpenGLWindow/OpenGLInclude.h"
#include "BussIK/Node.h"
#include "BussIK/Tree.h"
#include "BussIK/Jacobian.h"
#include "BussIK/VectorRn.h"
#define RADIAN(X) ((X)*RadiansToDegrees)
#define MAX_NUM_NODE 1000
#define MAX_NUM_THETA 1000
#define MAX_NUM_EFFECT 100
double T = 0;
VectorR3 target1[MAX_NUM_EFFECT];
// Make slowdown factor larger to make the simulation take larger, less frequent steps
// Make the constant factor in Tstep larger to make time pass more quickly
//const int SlowdownFactor = 40;
const int SlowdownFactor = 0; // Make higher to take larger steps less frequently
const int SleepsPerStep=SlowdownFactor;
int SleepCounter=0;
//const double Tstep = 0.0005*(double)SlowdownFactor; // Time step
int AxesList; /* list to hold the axes */
int AxesOn; /* ON or OFF */
float Scale, Scale2; /* scaling factors */
int JointLimitsOn;
int RestPositionOn;
int UseJacobianTargets1;
int numIteration = 1;
double error = 0.0;
double errorDLS = 0.0;
double errorSDLS = 0.0;
double sumError = 0.0;
double sumErrorDLS = 0.0;
double sumErrorSDLS = 0.0;
#ifdef _DYNAMIC
bool initMaxDist = true;
extern double Excess[];
extern double dsnorm[];
#endif
void Reset(Tree &tree, Jacobian* m_ikJacobian)
{
AxesOn = false;
Scale = 1.0;
Scale2 = 0.0; /* because add 1. to it in Display() */
JointLimitsOn = true;
RestPositionOn = false;
UseJacobianTargets1 = false;
tree.Init();
tree.Compute();
m_ikJacobian->Reset();
}
// Update target positions
void UpdateTargets( double T2, Tree & treeY) {
double T = T2 / 5.;
target1[0].Set(0.6*b3Sin(0), 0.6*b3Cos(0), 0.5+0.4*b3Sin(3 * T));
}
// Does a single update (on one kind of tree)
void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) {
if ( SleepCounter==0 ) {
T += Tstep;
UpdateTargets( T , treeY);
}
if ( UseJacobianTargets1 ) {
jacob->SetJtargetActive();
}
else {
jacob->SetJendActive();
}
jacob->ComputeJacobian(); // Set up Jacobian and deltaS vectors
// Calculate the change in theta values
switch (ikMethod) {
case IK_JACOB_TRANS:
jacob->CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK_DLS:
jacob->CalcDeltaThetasDLS(); // Damped least squares method
break;
case IK_DLS_SVD:
jacob->CalcDeltaThetasDLSwithSVD();
break;
case IK_PURE_PSEUDO:
jacob->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK_SDLS:
jacob->CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
jacob->ZeroDeltaThetas();
break;
}
if ( SleepCounter==0 ) {
jacob->UpdateThetas(); // Apply the change in the theta values
jacob->UpdatedSClampValue();
SleepCounter = SleepsPerStep;
}
else {
SleepCounter--;
}
}
///quick demo showing the right-handed coordinate system and positive rotations around each axis
class InverseKinematicsExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
int m_ikMethod;
Tree m_ikTree;
b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
float m_x;
float m_y;
float m_z;
b3AlignedObjectArray<int> m_movingInstances;
int m_targetInstance;
enum
{
numCubesX = 20,
numCubesY = 20
};
public:
InverseKinematicsExample(CommonGraphicsApp* app, int option)
:m_app(app),
m_x(0),
m_y(0),
m_z(0),
m_targetInstance(-1),
m_ikMethod(option)
{
m_app->setUpAxis(2);
{
b3Vector3 extents=b3MakeVector3(100,100,100);
extents[m_app->getUpAxis()]=1;
int xres = 20;
int yres = 20;
b3Vector4 color0=b3MakeVector4(0.4, 0.4, 0.4,1);
b3Vector4 color1=b3MakeVector4(0.6, 0.6, 0.6,1);
m_app->registerGrid(xres, yres, color0, color1);
}
///create some graphics proxy for the tracking target
///the endeffector tries to track it using Inverse Kinematics
{
int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM);
b3Vector3 pos = b3MakeVector3(1,1,1);
pos[app->getUpAxis()] = 1;
b3Quaternion orn(0, 0, 0, 1);
b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
m_targetInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling);
}
m_app->m_renderer->writeTransforms();
}
virtual ~InverseKinematicsExample()
{
m_app->m_renderer->enableBlend(false);
}
virtual void physicsDebugDraw(int debugDrawMode)
{
}
virtual void initPhysics()
{
BuildKukaIIWAShape();
m_ikJacobian = new Jacobian(&m_ikTree);
Reset(m_ikTree,m_ikJacobian);
}
virtual void exitPhysics()
{
delete m_ikJacobian;
m_ikJacobian = 0;
}
void BuildKukaIIWAShape();
void getLocalTransform(const Node* node, b3Transform& act)
{
b3Vector3 axis = b3MakeVector3(node->v.x, node->v.y, node->v.z);
b3Quaternion rot(0, 0, 0, 1);
if (axis.length())
{
rot = b3Quaternion (axis, node->theta);
}
act.setIdentity();
act.setRotation(rot);
act.setOrigin(b3MakeVector3(node->r.x, node->r.y, node->r.z));
}
void MyDrawTree(Node* node, const b3Transform& tr)
{
b3Vector3 lineColor = b3MakeVector3(0, 0, 0);
int lineWidth = 2;
if (node != 0) {
// glPushMatrix();
b3Vector3 pos = b3MakeVector3(tr.getOrigin().x, tr.getOrigin().y, tr.getOrigin().z);
b3Vector3 color = b3MakeVector3(0, 1, 0);
int pointSize = 10;
m_app->m_renderer->drawPoint(pos, color, pointSize);
m_app->m_renderer->drawLine(pos, pos+ 0.05*tr.getBasis().getColumn(0), b3MakeVector3(1,0,0), lineWidth);
m_app->m_renderer->drawLine(pos, pos + 0.05*tr.getBasis().getColumn(1), b3MakeVector3(0, 1, 0), lineWidth);
m_app->m_renderer->drawLine(pos, pos + 0.05*tr.getBasis().getColumn(2), b3MakeVector3(0, 0, 1), lineWidth);
b3Vector3 axisLocal = b3MakeVector3(node->v.x, node->v.y, node->v.z);
b3Vector3 axisWorld = tr.getBasis()*axisLocal;
m_app->m_renderer->drawLine(pos, pos + 0.1*axisWorld, b3MakeVector3(.2, 0.2, 0.7), 5);
//node->DrawNode(node == root); // Recursively draw node and update ModelView matrix
if (node->left) {
b3Transform act;
getLocalTransform(node->left, act);
b3Transform trl = tr*act;
m_app->m_renderer->drawLine(tr.getOrigin(), trl.getOrigin(), lineColor, lineWidth);
MyDrawTree(node->left, trl); // Draw tree of children recursively
}
// glPopMatrix();
if (node->right) {
b3Transform act;
getLocalTransform(node->right, act);
b3Transform trr = tr*act;
m_app->m_renderer->drawLine(tr.getOrigin(), trr.getOrigin(), lineColor, lineWidth);
MyDrawTree(node->right,trr); // Draw right siblings recursively
}
}
}
virtual void stepSimulation(float deltaTime)
{
DoUpdateStep(deltaTime, m_ikTree, m_ikJacobian, m_ikMethod);
}
virtual void renderScene()
{
b3Transform act;
getLocalTransform(m_ikTree.GetRoot(), act);
MyDrawTree(m_ikTree.GetRoot(), act);
b3Vector3 pos = b3MakeVector3(target1[0].x, target1[0].y, target1[0].z);
b3Quaternion orn(0, 0, 0, 1);
m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstance);
m_app->m_renderer->writeTransforms();
m_app->m_renderer->renderScene();
}
virtual void physicsDebugDraw()
{
}
virtual bool mouseMoveCallback(float x,float y)
{
return false;
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return false;
}
virtual bool keyboardCallback(int key, int state)
{
return false;
}
virtual void resetCamera()
{
float dist = 1.3;
float pitch = 120;
float yaw = 13;
float targetPos[3]={-0.35,0.14,0.25};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
}
}
};
void InverseKinematicsExample::BuildKukaIIWAShape()
{
const VectorR3& unitx = VectorR3::UnitX;
const VectorR3& unity = VectorR3::UnitY;
const VectorR3& unitz = VectorR3::UnitZ;
const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0);
const VectorR3& zero = VectorR3::Zero;
float minTheta = -4 * PI;
float maxTheta = 4 * PI;
m_ikNodes.resize(8);//7DOF+additional endeffector
m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.));
m_ikTree.InsertRoot(m_ikNodes[0]);
m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.));
m_ikTree.InsertLeftChild(m_ikNodes[0], m_ikNodes[1]);
m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_ikTree.InsertLeftChild(m_ikNodes[1], m_ikNodes[2]);
m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_ikTree.InsertLeftChild(m_ikNodes[2], m_ikNodes[3]);
m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_ikTree.InsertLeftChild(m_ikNodes[3], m_ikNodes[4]);
m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_ikTree.InsertLeftChild(m_ikNodes[4], m_ikNodes[5]);
m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_ikTree.InsertLeftChild(m_ikNodes[5], m_ikNodes[6]);
m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR);
m_ikTree.InsertLeftChild(m_ikNodes[6], m_ikNodes[7]);
}
class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options)
{
return new InverseKinematicsExample(options.m_guiHelper->getAppInterface(), options.m_option);
}

8
examples/InverseKinematics/InverseKinematicsExample.h Normal file
View File

@@ -0,0 +1,8 @@
#ifndef INVERSE_KINEMATICSEXAMPLE_H
#define INVERSE_KINEMATICSEXAMPLE_H
enum Method {IK_JACOB_TRANS=0, IK_PURE_PSEUDO, IK_DLS, IK_SDLS , IK_DLS_SVD};
class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options);
#endif //INVERSE_KINEMATICSEXAMPLE_H

3
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -367,9 +367,8 @@ void TestJointTorqueSetup::initPhysics()
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
b3ResourcePath p;
char resourcePath[1024];
if (p.findResourcePath("multibody.bullet",resourcePath,1024))
if (b3ResourcePath::findResourcePath("multibody.bullet",resourcePath,1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);

2
examples/MultiThreading/MultiThreadingExample.cpp
View File

@@ -119,7 +119,7 @@ struct SampleThreadLocalStorage
void SampleThreadFunc(void* userPtr,void* lsMemory)
{
printf("thread started\n");
printf("SampleThreadFunc thread started\n");
SampleThreadLocalStorage* localStorage = (SampleThreadLocalStorage*) lsMemory;

26
examples/MultiThreading/b3PosixThreadSupport.cpp
View File

@@ -44,8 +44,6 @@ b3PosixThreadSupport::~b3PosixThreadSupport()
#define NAMED_SEMAPHORES
#endif
// this semaphore will signal, if and how many threads are finished with their work
static sem_t* mainSemaphore=0;
static sem_t* createSem(const char* baseName)
{
@@ -53,7 +51,7 @@ static sem_t* createSem(const char* baseName)
#ifdef NAMED_SEMAPHORES
/// Named semaphore begin
char name[32];
snprintf(name, 32, "/%s-%d-%4.4d", baseName, getpid(), semCount++);
snprintf(name, 32, "/%8.s-%4.d-%4.4d", baseName, getpid(), semCount++);
sem_t* tempSem = sem_open(name, O_CREAT, 0600, 0);
if (tempSem != reinterpret_cast<sem_t *>(SEM_FAILED))
@@ -100,12 +98,12 @@ static void *threadFunction(void *argument)
b3Assert(status->m_status);
status->m_userThreadFunc(userPtr,status->m_lsMemory);
status->m_status = 2;
checkPThreadFunction(sem_post(mainSemaphore));
checkPThreadFunction(sem_post(status->m_mainSemaphore));
status->threadUsed++;
} else {
//exit Thread
status->m_status = 3;
checkPThreadFunction(sem_post(mainSemaphore));
checkPThreadFunction(sem_post(status->m_mainSemaphore));
printf("Thread with taskId %i exiting\n",status->m_taskId);
break;
}
@@ -160,13 +158,14 @@ bool b3PosixThreadSupport::isTaskCompleted(int *puiArgument0, int *puiArgument1,
b3Assert(m_activeThreadStatus.size());
// wait for any of the threads to finish
int result = sem_trywait(mainSemaphore);
int result = sem_trywait(m_mainSemaphore);
if (result==0)
{
// get at least one thread which has finished
size_t last = -1;
for(size_t t=0; t < size_t(m_activeThreadStatus.size()); ++t) {
int last = -1;
int status = -1;
for(int t=0; t < int (m_activeThreadStatus.size()); ++t) {
status = m_activeThreadStatus[t].m_status;
if(2 == m_activeThreadStatus[t].m_status) {
last = t;
break;
@@ -200,7 +199,7 @@ void b3PosixThreadSupport::waitForResponse( int *puiArgument0, int *puiArgument
b3Assert(m_activeThreadStatus.size());
// wait for any of the threads to finish
checkPThreadFunction(sem_wait(mainSemaphore));
checkPThreadFunction(sem_wait(m_mainSemaphore));
// get at least one thread which has finished
size_t last = -1;
@@ -231,7 +230,7 @@ void b3PosixThreadSupport::startThreads(ThreadConstructionInfo& threadConstructi
printf("%s creating %i threads.\n", __FUNCTION__, threadConstructionInfo.m_numThreads);
m_activeThreadStatus.resize(threadConstructionInfo.m_numThreads);
mainSemaphore = createSem("main");
m_mainSemaphore = createSem("main");
//checkPThreadFunction(sem_wait(mainSemaphore));
for (int i=0;i < threadConstructionInfo.m_numThreads;i++)
@@ -249,6 +248,7 @@ void b3PosixThreadSupport::startThreads(ThreadConstructionInfo& threadConstructi
spuStatus.m_taskId = i;
spuStatus.m_commandId = 0;
spuStatus.m_status = 0;
spuStatus.m_mainSemaphore = m_mainSemaphore;
spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
spuStatus.threadUsed = 0;
@@ -271,7 +271,7 @@ void b3PosixThreadSupport::stopThreads()
spuStatus.m_userPtr = 0;
checkPThreadFunction(sem_post(spuStatus.startSemaphore));
checkPThreadFunction(sem_wait(mainSemaphore));
checkPThreadFunction(sem_wait(m_mainSemaphore));
printf("destroy semaphore\n");
destroySem(spuStatus.startSemaphore);
@@ -280,7 +280,7 @@ void b3PosixThreadSupport::stopThreads()
}
printf("destroy main semaphore\n");
destroySem(mainSemaphore);
destroySem(m_mainSemaphore);
printf("main semaphore destroyed\n");
m_activeThreadStatus.clear();
}

12
examples/MultiThreading/b3PosixThreadSupport.h
View File

@@ -57,14 +57,22 @@ public:
void* m_userPtr; //for taskDesc etc
void* m_lsMemory; //initialized using PosixLocalStoreMemorySetupFunc
pthread_t thread;
sem_t* startSemaphore;
pthread_t thread;
//each tread will wait until this signal to start its work
sem_t* startSemaphore;
// this is a copy of m_mainSemaphore,
//each tread will signal once it is finished with its work
sem_t* m_mainSemaphore;
unsigned long threadUsed;
};
private:
b3AlignedObjectArray<b3ThreadStatus> m_activeThreadStatus;
// m_mainSemaphoresemaphore will signal, if and how many threads are finished with their work
sem_t* m_mainSemaphore;
public:
///Setup and initialize SPU/CELL/Libspe2

35
examples/MultiThreading/b3Win32ThreadSupport.cpp
View File

@@ -223,7 +223,8 @@ bool b3Win32ThreadSupport::isTaskCompleted(int *puiArgument0, int *puiArgument1,
void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadConstructionInfo)
{
static int uniqueId = 0;
uniqueId++;
m_activeThreadStatus.resize(threadConstructionInfo.m_numThreads);
m_completeHandles.resize(threadConstructionInfo.m_numThreads);
@@ -244,18 +245,40 @@ void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threa
threadStatus.m_userPtr=0;
sprintf(threadStatus.m_eventStartHandleName,"eventStart%s%d",threadConstructionInfo.m_uniqueName,i);
sprintf(threadStatus.m_eventStartHandleName,"es%.8s%d%d",threadConstructionInfo.m_uniqueName,uniqueId,i);
threadStatus.m_eventStartHandle = CreateEventA (0,false,false,threadStatus.m_eventStartHandleName);
sprintf(threadStatus.m_eventCompletetHandleName,"eventComplete%s%d",threadConstructionInfo.m_uniqueName,i);
sprintf(threadStatus.m_eventCompletetHandleName,"ec%.8s%d%d",threadConstructionInfo.m_uniqueName,uniqueId,i);
threadStatus.m_eventCompletetHandle = CreateEventA (0,false,false,threadStatus.m_eventCompletetHandleName);
m_completeHandles[i] = threadStatus.m_eventCompletetHandle;
HANDLE handle = CreateThread(lpThreadAttributes,dwStackSize,lpStartAddress,lpParameter, dwCreationFlags,lpThreadId);
//SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST);
SetThreadPriority(handle,THREAD_PRIORITY_TIME_CRITICAL);
switch(threadConstructionInfo.m_priority)
{
case 0:
{
SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST);
break;
}
case 1:
{
SetThreadPriority(handle,THREAD_PRIORITY_TIME_CRITICAL);
break;
}
case 2:
{
SetThreadPriority(handle,THREAD_PRIORITY_BELOW_NORMAL);
break;
}
default:
{
}
}
SetThreadAffinityMask(handle, 1<<i);
threadStatus.m_taskId = i;
@@ -265,7 +288,7 @@ void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threa
threadStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
printf("started thread %d with threadHandle %p\n",i,handle);
printf("started %s thread %d with threadHandle %p\n",threadConstructionInfo.m_uniqueName,i,handle);
}

4
examples/MultiThreading/b3Win32ThreadSupport.h
View File

@@ -74,7 +74,8 @@ public:
m_userThreadFunc(userThreadFunc),
m_lsMemoryFunc(lsMemoryFunc),
m_numThreads(numThreads),
m_threadStackSize(threadStackSize)
m_threadStackSize(threadStackSize),
m_priority(0)
{
}
@@ -84,6 +85,7 @@ public:
b3Win32lsMemorySetupFunc m_lsMemoryFunc;
int m_numThreads;
int m_threadStackSize;
int m_priority;
};

145
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -16,10 +16,10 @@ subject to the following restrictions:
///todo: make this configurable in the gui
bool useShadowMap=true;//false;//true;
int shadowMapWidth=8192;
int shadowMapHeight=8192;
float shadowMapWorldSize=50;
bool useShadowMap = true;// true;//false;//true;
int shadowMapWidth=4096;//8192;
int shadowMapHeight= 4096;
float shadowMapWorldSize=25;
#define MAX_POINTS_IN_BATCH 1024
#define MAX_LINES_IN_BATCH 1024
@@ -75,7 +75,7 @@ float shadowMapWorldSize=50;
static InternalDataRenderer* sData2;
GLint lineWidthRange[2]={1,1};
static b3Vector3 gLightPos=b3MakeVector3(-5,200,-40);
static b3Vector3 gLightPos=b3MakeVector3(-5,12,-4);
struct b3GraphicsInstance
{
@@ -146,8 +146,9 @@ struct InternalTextureHandle
struct InternalDataRenderer : public GLInstanceRendererInternalData
{
SimpleCamera m_defaultCamera;
SimpleCamera m_defaultCamera1;
CommonCameraInterface* m_activeCamera;
GLfloat m_projectionMatrix[16];
GLfloat m_viewMatrix[16];
@@ -158,10 +159,14 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
GLRenderToTexture* m_shadowMap;
GLuint m_shadowTexture;
GLuint m_renderFrameBuffer;
InternalDataRenderer() :
m_shadowMap(0),
m_shadowTexture(0)
m_shadowTexture(0),
m_renderFrameBuffer(0),
m_activeCamera(&m_defaultCamera1)
{
//clear to zero to make it obvious if the matrix is used uninitialized
for (int i=0;i<16;i++)
@@ -258,6 +263,8 @@ GLInstancingRenderer::GLInstancingRenderer(int maxNumObjectCapacity, int maxShap
void GLInstancingRenderer::removeAllInstances()
{
m_data->m_totalNumInstances = 0;
for (int i=0;i<m_graphicsInstances.size();i++)
{
if (m_graphicsInstances[i]->m_index_vbo)
@@ -273,6 +280,7 @@ void GLInstancingRenderer::removeAllInstances()
m_graphicsInstances.clear();
}
GLInstancingRenderer::~GLInstancingRenderer()
{
delete m_data->m_shadowMap;
@@ -320,6 +328,7 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi
*/
}
void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex)
{
m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]);
@@ -337,12 +346,24 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int srcIn
m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3];
}
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(float* scale, int srcIndex)
{
m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0];
m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1];
m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
}
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(double* scale, int srcIndex)
{
m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0];
m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1];
m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
}
void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, float* orientation, int objectIndex)
{
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
glFlush();
//glFlush();
char* orgBase = (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
//b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
@@ -372,7 +393,7 @@ void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, fl
orientations [objectIndex*4+3] = orientation[3];
glUnmapBuffer( GL_ARRAY_BUFFER);
glFlush();
//glFlush();
}
@@ -383,25 +404,46 @@ void GLInstancingRenderer::writeTransforms()
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
glFlush();
//glFlush();
b3Assert(glGetError() ==GL_NO_ERROR);
#ifdef B3_DEBUG
{
int totalNumInstances= 0;
for (int k=0;k<m_graphicsInstances.size();k++)
{
b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
totalNumInstances+=gfxObj->m_numGraphicsInstances;
}
b3Assert(m_data->m_totalNumInstances == totalNumInstances);
}
#endif//B3_DEBUG
int POSITION_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4);
int ORIENTATION_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4);
int COLOR_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4);
// int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);
#if 1
glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes,m_data->m_totalNumInstances*sizeof(float)*4,
&m_data->m_instance_positions_ptr[0]);
glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE,m_data->m_totalNumInstances*sizeof(float)*4,
&m_data->m_instance_quaternion_ptr[0]);
glBufferSubData( GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+ POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE, m_data->m_totalNumInstances*sizeof(float)*4,
&m_data->m_instance_colors_ptr[0]);
glBufferSubData( GL_ARRAY_BUFFER, m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE,m_data->m_totalNumInstances*sizeof(float)*3,
&m_data->m_instance_scale_ptr[0]);
#else
char* orgBase = (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
if (orgBase)
{
int totalNumInstances= 0;
for (int k=0;k<m_graphicsInstances.size();k++)
{
b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
totalNumInstances+=gfxObj->m_numGraphicsInstances;
}
m_data->m_totalNumInstances = totalNumInstances;
for (int k=0;k<m_graphicsInstances.size();k++)
{
@@ -410,10 +452,7 @@ void GLInstancingRenderer::writeTransforms()
int POSITION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
int ORIENTATION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
int COLOR_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
// int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);
char* base = orgBase;
@@ -461,7 +500,10 @@ void GLInstancingRenderer::writeTransforms()
glUnmapBuffer( GL_ARRAY_BUFFER);
//if this glFinish is removed, the animation is not always working/blocks
//@todo: figure out why
glFlush();
//glFlush();
#endif
glBindBuffer(GL_ARRAY_BUFFER, 0);//m_data->m_vbo);
b3Assert(glGetError() ==GL_NO_ERROR);
@@ -879,18 +921,18 @@ void GLInstancingRenderer::resize(int width, int height)
const CommonCameraInterface* GLInstancingRenderer::getActiveCamera() const
{
return &m_data->m_defaultCamera;
return m_data->m_activeCamera;
}
CommonCameraInterface* GLInstancingRenderer::getActiveCamera()
{
return &m_data->m_defaultCamera;
return m_data->m_activeCamera;
}
void GLInstancingRenderer::setActiveCamera(CommonCameraInterface* cam)
{
b3Assert(0);//not supported yet
m_data->m_activeCamera = cam;
}
@@ -911,14 +953,12 @@ void GLInstancingRenderer::updateCamera(int upAxis)
b3Assert(0);
};
m_data->m_defaultCamera.setCameraUpAxis(upAxis);
m_data->m_defaultCamera.setAspectRatio((float)m_screenWidth/(float)m_screenHeight);
m_data->m_defaultCamera.update();
m_data->m_activeCamera->setCameraUpAxis(upAxis);
m_data->m_activeCamera->setAspectRatio((float)m_screenWidth/(float)m_screenHeight);
m_data->m_defaultCamera1.update();
m_data->m_activeCamera->getCameraProjectionMatrix(m_data->m_projectionMatrix);
m_data->m_activeCamera->getCameraViewMatrix(m_data->m_viewMatrix);
m_data->m_defaultCamera.getCameraProjectionMatrix(m_data->m_projectionMatrix);
m_data->m_defaultCamera.getCameraViewMatrix(m_data->m_viewMatrix);
}
@@ -997,13 +1037,13 @@ void GLInstancingRenderer::renderScene()
//avoid some Intel driver on a Macbook Pro to lock-up
//todo: figure out what is going on on that machine
glFlush();
//glFlush();
if (useShadowMap)
{
renderSceneInternal(B3_CREATE_SHADOWMAP_RENDERMODE);
// glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
//glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE);
} else
@@ -1072,6 +1112,9 @@ void GLInstancingRenderer::drawPoints(const float* positions, const float color[
void GLInstancingRenderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float lineWidthIn)
{
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D,0);
float lineWidth = lineWidthIn;
b3Clamp(lineWidth,(float)lineWidthRange[0],(float)lineWidthRange[1]);
glLineWidth(lineWidth);
@@ -1446,7 +1489,7 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
float depthViewMatrix[4][4];
b3Vector3 center = b3MakeVector3(0,0,0);
float upf[3];
m_data->m_defaultCamera.getCameraUpVector(upf);
m_data->m_activeCamera->getCameraUpVector(upf);
b3Vector3 up = b3MakeVector3(upf[0],upf[1],upf[2]);
b3CreateLookAt(gLightPos,center,up,&depthViewMatrix[0][0]);
//b3CreateLookAt(lightPos,m_data->m_cameraTargetPosition,b3Vector3(0,1,0),(float*)depthModelViewMatrix2);
@@ -1483,6 +1526,9 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
{
B3_PROFILE("updateCamera");
// updateCamera();
m_data->m_activeCamera->getCameraProjectionMatrix(m_data->m_projectionMatrix);
m_data->m_activeCamera->getCameraViewMatrix(m_data->m_viewMatrix);
}
b3Assert(glGetError() ==GL_NO_ERROR);
@@ -1493,7 +1539,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
B3_PROFILE("glFlush2");
glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
glFlush();
//glFlush();
}
b3Assert(glGetError() ==GL_NO_ERROR);
@@ -1507,7 +1553,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
int curOffset = 0;
//GLuint lastBindTexture = 0;
for (int i=0;i<m_graphicsInstances.size();i++)
{
@@ -1631,6 +1677,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
printf("gfxObj->m_numGraphicsInstances=%d\n",gfxObj->m_numGraphicsInstances);
printf("indexCount=%d\n",indexCount);
*/
glUseProgram(createShadowMapInstancingShader);
glUniformMatrix4fv(createShadow_depthMVP, 1, false, &depthMVP[0][0]);
glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
@@ -1663,7 +1710,8 @@ b3Assert(glGetError() ==GL_NO_ERROR);
{
glDisable (GL_BLEND);
}
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D,0);
break;
}
default:
@@ -1682,12 +1730,13 @@ b3Assert(glGetError() ==GL_NO_ERROR);
{
B3_PROFILE("glFlush");
glFlush();
//glFlush();
}
if (renderMode==B3_CREATE_SHADOWMAP_RENDERMODE)
{
// writeTextureToPng(shadowMapWidth,shadowMapHeight,"shadowmap.png",4);
m_data->m_shadowMap->disable();
glBindFramebuffer( GL_FRAMEBUFFER, m_data->m_renderFrameBuffer);
glViewport(dims[0],dims[1],dims[2],dims[3]);
}
@@ -1733,4 +1782,16 @@ int GLInstancingRenderer::getInstanceCapacity() const
{
return m_data->m_maxNumObjectCapacity;
}
void GLInstancingRenderer::setRenderFrameBuffer(unsigned int renderFrameBuffer)
{
m_data->m_renderFrameBuffer = (GLuint) renderFrameBuffer;
}
int GLInstancingRenderer::getTotalNumInstances() const
{
return m_data->m_totalNumInstances;
}
#endif //NO_OPENGL3

8
examples/OpenGLWindow/GLInstancingRenderer.h
View File

@@ -56,7 +56,7 @@ public:
void InitShaders();
void CleanupShaders();
virtual void removeAllInstances();
virtual void updateShape(int shapeIndex, const float* vertices);
///vertices must be in the format x,y,z, nx,ny,nz, u,v
@@ -95,6 +95,9 @@ public:
virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);
virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex);
virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex);
virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex);
virtual struct GLInstanceRendererInternalData* getInternalData();
@@ -125,6 +128,8 @@ public:
virtual int getInstanceCapacity() const;
virtual int getTotalNumInstances() const;
virtual void enableShadowMap();
virtual void enableBlend(bool blend)
{
@@ -132,6 +137,7 @@ public:
}
virtual void clearZBuffer();
virtual void setRenderFrameBuffer(unsigned int renderFrameBuffer);
};
#endif //GL_INSTANCING_RENDERER_H

30
examples/OpenGLWindow/MacOpenGLWindow.mm
View File

@@ -102,11 +102,11 @@ float loop;
}
-(void)drawRect:(NSRect)rect
{
if (([self frame].size.width != m_lastWidth) || ([self frame].size.height != m_lastHeight))
{
m_lastWidth = [self frame].size.width;
m_lastHeight = [self frame].size.height;
// Only needed on resize:
[m_context clearDrawable];
@@ -114,7 +114,6 @@ float loop;
float width = [self frame].size.width;
float height = [self frame].size.height;
// Get view dimensions in pixels
// glViewport(0,0,10,10);
@@ -209,16 +208,12 @@ struct MacOpenGLWindowInternalData
m_myview = 0;
m_pool = 0;
m_window = 0;
m_width = -1;
m_height = -1;
m_exitRequested = false;
}
NSApplication* m_myApp;
TestView* m_myview;
NSAutoreleasePool* m_pool;
NSWindow* m_window;
int m_width;
int m_height;
bool m_exitRequested;
};
@@ -294,8 +289,6 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
if (m_internalData)
closeWindow();
int width = ci.m_width;
int height = ci.m_height;
const char* windowTitle = ci.m_title;
@@ -303,9 +296,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
m_internalData = new MacOpenGLWindowInternalData;
m_internalData->m_width = width;
m_internalData->m_height = height;
m_internalData->m_pool = [NSAutoreleasePool new];
m_internalData->m_myApp = [NSApplication sharedApplication];
//myApp = [MyApp sharedApplication];
@@ -373,7 +364,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
[newItem release];
*/
NSRect frame = NSMakeRect(0., 0., width, height);
NSRect frame = NSMakeRect(0., 0., ci.m_width, ci.m_height);
m_internalData->m_window = [NSWindow alloc];
[m_internalData->m_window initWithContentRect:frame
@@ -423,9 +414,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
[m_internalData->m_window makeKeyAndOrderFront: nil];
[m_internalData->m_myview MakeCurrent];
m_internalData->m_width = m_internalData->m_myview.GetWindowWidth;
m_internalData->m_height = m_internalData->m_myview.GetWindowHeight;
[NSApp activateIgnoringOtherApps:YES];
@@ -1035,13 +1024,13 @@ void MacOpenGLWindow::startRendering()
float aspect;
//b3Vector3 extents;
if (m_internalData->m_width > m_internalData->m_height)
if (getWidth() > getHeight())
{
aspect = (float)m_internalData->m_width / (float)m_internalData->m_height;
aspect = (float)getWidth() / (float)getHeight();
//extents.setValue(aspect * 1.0f, 1.0f,0);
} else
{
aspect = (float)m_internalData->m_height / (float)m_internalData->m_width;
aspect = (float)getHeight() / (float)getWidth();
//extents.setValue(1.0f, aspect*1.f,0);
}
@@ -1082,7 +1071,7 @@ int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
NSOpenGLContext *foo = [NSOpenGLContext currentContext];
// get the url of a .txt file
NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel];
NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",nil];
NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",@"stl",nil];
[zOpenPanel setAllowedFileTypes:zAryOfExtensions];
NSInteger zIntResult = [zOpenPanel runModal];
@@ -1136,6 +1125,7 @@ int MacOpenGLWindow::getWidth() const
{
if (m_internalData && m_internalData->m_myview && m_internalData->m_myview.GetWindowWidth)
return m_internalData->m_myview.GetWindowWidth;
return 0;
}
@@ -1152,7 +1142,7 @@ void MacOpenGLWindow::setResizeCallback(b3ResizeCallback resizeCallback)
[m_internalData->m_myview setResizeCallback:resizeCallback];
if (resizeCallback)
{
(resizeCallback)(m_internalData->m_width,m_internalData->m_height);
(resizeCallback)(getWidth(), getHeight());
}
}

3
examples/OpenGLWindow/Shaders/instancingPS.glsl
View File

@@ -25,7 +25,8 @@ void main(void)
vec4 texel = fragment.color*texture(Diffuse,vert.texcoord);//fragment.color;
vec3 ct,cf;
float intensity,at,af;
intensity = max(dot(lightDir,normalize(normal)),0);
intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );
cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;
af = 1.0;

3
examples/OpenGLWindow/Shaders/instancingPS.h
View File

@@ -22,7 +22,8 @@ static const char* instancingFragmentShader= \
" vec4 texel = fragment.color*texture(Diffuse,vert.texcoord);//fragment.color;\n"
" vec3 ct,cf;\n"
" float intensity,at,af;\n"
" intensity = max(dot(lightDir,normalize(normal)),0);\n"
" \n"
" intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );\n"
" cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;\n"
" af = 1.0;\n"
" \n"

2
examples/OpenGLWindow/Shaders/instancingVS.glsl
View File

@@ -61,7 +61,7 @@ out vec3 lightDir,normal,ambient;
void main(void)
{
vec4 q = instance_quaternion;
ambient = vec3(0.6,.6,0.6);
ambient = vec3(0.5,.5,0.5);
vec4 worldNormal = (quatRotate3( vertexnormal,q));
normal = normalize(worldNormal).xyz;

2
examples/OpenGLWindow/Shaders/instancingVS.h
View File

@@ -52,7 +52,7 @@ static const char* instancingVertexShader= \
"void main(void)\n"
"{\n"
" vec4 q = instance_quaternion;\n"
" ambient = vec3(0.6,.6,0.6);\n"
" ambient = vec3(0.5,.5,0.5);\n"
" \n"
" vec4 worldNormal = (quatRotate3( vertexnormal,q));\n"
" normal = normalize(worldNormal).xyz;\n"

3
examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl
View File

@@ -27,8 +27,7 @@ void main(void)
vec3 ct,cf;
float intensity,at,af;
intensity = clamp( dot( normalize(normal),lightDir ), 0,1 );
intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );
af = 1.0;

3
examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h
View File

@@ -21,8 +21,7 @@ static const char* useShadowMapInstancingFragmentShader= \
" vec3 ct,cf;\n"
" float intensity,at,af;\n"
" \n"
" intensity = clamp( dot( normalize(normal),lightDir ), 0,1 );\n"
" \n"
" intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );\n"
" \n"
" af = 1.0;\n"
" \n"

2
examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.glsl
View File

@@ -65,7 +65,7 @@ out vec3 lightDir,normal,ambient;
void main(void)
{
vec4 q = instance_quaternion;
ambient = vec3(0.6,.6,0.6);
ambient = vec3(0.5,.5,0.5);
vec4 worldNormal = (quatRotate3( vertexnormal,q));

2
examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.h
View File

@@ -55,7 +55,7 @@ static const char* useShadowMapInstancingVertexShader= \
"void main(void)\n"
"{\n"
" vec4 q = instance_quaternion;\n"
" ambient = vec3(0.6,.6,0.6);\n"
" ambient = vec3(0.5,.5,0.5);\n"
" \n"
" vec4 worldNormal = (quatRotate3( vertexnormal,q));\n"
" \n"

10
examples/OpenGLWindow/SimpleCamera.cpp
View File

@@ -62,6 +62,16 @@ void SimpleCamera::setVRCamera(const float viewMat[16], const float projectionMa
}
}
void SimpleCamera::disableVRCamera()
{
m_data->m_enableVR = false;
}
bool SimpleCamera::isVRCamera() const
{
return m_data->m_enableVR ;
}
static void b3CreateFrustum(
float left,

2
examples/OpenGLWindow/SimpleCamera.h
View File

@@ -15,6 +15,8 @@ struct SimpleCamera : public CommonCameraInterface
virtual void getCameraViewMatrix(float m[16]) const;
virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16]);
virtual void disableVRCamera();
virtual bool isVRCamera() const;
virtual void getCameraTargetPosition(float pos[3]) const;
virtual void getCameraPosition(float pos[3]) const;

14
examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
View File

@@ -64,6 +64,20 @@ void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(double* color, int src
{
}
void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(float* scale, int srcIndex)
{
}
void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(double* scale, int srcIndex)
{
}
int SimpleOpenGL2Renderer::getTotalNumInstances() const
{
return 0;
}
void SimpleOpenGL2Renderer::getCameraViewMatrix(float viewMat[16]) const
{
b3Assert(0);

5
examples/OpenGLWindow/SimpleOpenGL2Renderer.h
View File

@@ -32,6 +32,8 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);
virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex);
virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex);
virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex);
virtual void getCameraViewMatrix(float viewMat[16]) const;
virtual void getCameraProjectionMatrix(float projMat[16]) const;
@@ -68,6 +70,8 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex);
virtual int getTotalNumInstances() const;
virtual void writeTransforms();
virtual void drawLine(const double from[4], const double to[4], const double color[4], double lineWidth);
@@ -82,6 +86,7 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
virtual void clearZBuffer();
virtual struct GLInstanceRendererInternalData* getInternalData()
{
return 0;

35
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -650,14 +650,19 @@ void SimpleOpenGL3App::setBackgroundColor(float red, float green, float blue)
SimpleOpenGL3App::~SimpleOpenGL3App()
{
delete m_instancingRenderer;
delete m_primRenderer ;
sth_delete(m_data->m_fontStash);
delete m_data->m_renderCallbacks;
TwDeleteDefaultFonts();
m_window->closeWindow();
delete m_window;
delete m_data ;
}
void SimpleOpenGL3App::getScreenPixels(unsigned char* rgbaBuffer, int bufferSizeInBytes)
void SimpleOpenGL3App::getScreenPixels(unsigned char* rgbaBuffer, int bufferSizeInBytes, float* depthBuffer, int depthBufferSizeInBytes)
{
int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
@@ -668,6 +673,12 @@ void SimpleOpenGL3App::getScreenPixels(unsigned char* rgbaBuffer, int bufferSize
int glstat = glGetError();
b3Assert(glstat==GL_NO_ERROR);
}
if ((width*height*sizeof(float)) == depthBufferSizeInBytes)
{
glReadPixels(0,0,width, height, GL_DEPTH_COMPONENT, GL_FLOAT, depthBuffer);
int glstat = glGetError();
b3Assert(glstat==GL_NO_ERROR);
}
}
@@ -739,11 +750,13 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char*
void SimpleOpenGL3App::swapBuffer()
{
m_window->endRendering();
if (m_data->m_frameDumpPngFileName)
{
writeTextureToFile((int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(),
(int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(),m_data->m_frameDumpPngFileName,
int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
int height = (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight();
writeTextureToFile(width,
height,m_data->m_frameDumpPngFileName,
m_data->m_ffmpegFile);
m_data->m_renderTexture->disable();
if (m_data->m_ffmpegFile==0)
@@ -751,7 +764,8 @@ void SimpleOpenGL3App::swapBuffer()
m_data->m_frameDumpPngFileName = 0;
}
}
m_window->startRendering();
m_window->endRendering();
m_window->startRendering();
}
// see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/
@@ -762,12 +776,15 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
char cmd[8192];
#ifdef _WIN32
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName);
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
//sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName);
#else
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -crf 0 -b 50000k -vf vflip %s", width, height, mp4FileName);
"-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName);
#endif
//sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "

2
examples/OpenGLWindow/SimpleOpenGL3App.h
View File

@@ -24,7 +24,7 @@ struct SimpleOpenGL3App : public CommonGraphicsApp
virtual void registerGrid(int xres, int yres, float color0[4], float color1[4]);
void dumpNextFrameToPng(const char* pngFilename);
void dumpFramesToVideo(const char* mp4Filename);
void getScreenPixels(unsigned char* rgbaBuffer, int bufferSizeInBytes);
void getScreenPixels(unsigned char* rgbaBuffer, int bufferSizeInBytes, float* depthBuffer, int depthBufferSizeInBytes);
void drawGrid(DrawGridData data=DrawGridData());
virtual void setUpAxis(int axis);

35
examples/OpenGLWindow/TwFonts.cpp
View File

@@ -4883,18 +4883,33 @@ static const unsigned char s_FontFixed1[] = {
void TwGenerateDefaultFonts()
{
g_DefaultSmallFont = TwGenerateFont(s_Font0, FONT0_BM_W, FONT0_BM_H);
assert(g_DefaultSmallFont && g_DefaultSmallFont->m_NbCharRead==224);
if (g_DefaultSmallFont == 0)
{
g_DefaultSmallFont = TwGenerateFont(s_Font0, FONT0_BM_W, FONT0_BM_H);
assert(g_DefaultSmallFont && g_DefaultSmallFont->m_NbCharRead == 224);
}
g_DefaultNormalFont = TwGenerateFont(s_Font1, FONT1_BM_W, FONT1_BM_H);
assert(g_DefaultNormalFont && g_DefaultNormalFont->m_NbCharRead==224);
if (g_DefaultNormalFont == 0)
{
g_DefaultNormalFont = TwGenerateFont(s_Font1, FONT1_BM_W, FONT1_BM_H);
assert(g_DefaultNormalFont && g_DefaultNormalFont->m_NbCharRead == 224);
}
g_DefaultNormalFontAA = TwGenerateFont(s_Font1AA, FONT1AA_BM_W, FONT1AA_BM_H);
assert(g_DefaultNormalFontAA && g_DefaultNormalFontAA->m_NbCharRead==224);
g_DefaultLargeFont = TwGenerateFont(s_Font2AA, FONT2AA_BM_W, FONT2AA_BM_H);
assert(g_DefaultLargeFont && g_DefaultLargeFont->m_NbCharRead==224);
g_DefaultFixed1Font = TwGenerateFont(s_FontFixed1, FONTFIXED1_BM_W, FONTFIXED1_BM_H);
assert(g_DefaultFixed1Font && g_DefaultFixed1Font->m_NbCharRead==224);
if (g_DefaultNormalFontAA == 0)
{
g_DefaultNormalFontAA = TwGenerateFont(s_Font1AA, FONT1AA_BM_W, FONT1AA_BM_H);
assert(g_DefaultNormalFontAA && g_DefaultNormalFontAA->m_NbCharRead == 224);
}
if (g_DefaultLargeFont == 0)
{
g_DefaultLargeFont = TwGenerateFont(s_Font2AA, FONT2AA_BM_W, FONT2AA_BM_H);
assert(g_DefaultLargeFont && g_DefaultLargeFont->m_NbCharRead == 224);
}
if (g_DefaultFixed1Font == 0)
{
g_DefaultFixed1Font = TwGenerateFont(s_FontFixed1, FONTFIXED1_BM_W, FONTFIXED1_BM_H);
assert(g_DefaultFixed1Font && g_DefaultFixed1Font->m_NbCharRead == 224);
}
}
// ---------------------------------------------------------------------------

3
examples/OpenGLWindow/Win32Window.cpp
View File

@@ -435,11 +435,10 @@ void Win32Window::setWindowTitle(const char* titleChar)
wchar_t windowTitle[1024];
swprintf(windowTitle, 1024, L"%hs", titleChar);
DWORD dwResult;
#ifdef _WIN64
SetWindowTextW(m_data->m_hWnd, windowTitle);
#else
DWORD dwResult;
SendMessageTimeoutW(m_data->m_hWnd, WM_SETTEXT, 0,
reinterpret_cast<LPARAM>(windowTitle),
SMTO_ABORTIFHUNG, 2000, &dwResult);

3
examples/OpenGLWindow/X11OpenGLWindow.cpp
View File

@@ -516,6 +516,9 @@ void X11OpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
m_data->m_dpy = MyXOpenDisplay(NULL);
m_data->m_glWidth = ci.m_width;
m_data->m_glHeight = ci.m_height;
if(m_data->m_dpy == NULL) {
printf("\n\tcannot connect to X server\n\n");
exit(0);

2
examples/RenderingExamples/CoordinateSystemDemo.cpp
View File

@@ -156,7 +156,7 @@ public:
}
};
struct CommonExampleInterface* CoordinateSystemCreateFunc(struct CommonExampleOptions& options)
CommonExampleInterface* CoordinateSystemCreateFunc(struct CommonExampleOptions& options)
{
return new CoordinateSystemDemo(options.m_guiHelper->getAppInterface());
}

2
examples/RenderingExamples/RaytracerSetup.cpp
View File

@@ -295,7 +295,7 @@ void RaytracerPhysicsSetup::stepSimulation(float deltaTime)
for (x=0;x<m_internalData->m_width;x++)
{
for (int y=0;y<m_internalData->m_height;y++)
for (y=0;y<m_internalData->m_height;y++)
{
btVector4 rgba(0,0,0,0);
btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;

6
examples/RenderingExamples/TinyRendererSetup.cpp
View File

@@ -147,7 +147,9 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
m_internalData = new TinyRendererSetupInternalData(gui->getAppInterface()->m_window->getWidth(),gui->getAppInterface()->m_window->getHeight());
m_app->m_renderer->enableBlend(true);
const char* fileName = "teddy.obj";//cube.obj";//textured_sphere_smooth.obj";//cube.obj";
const char* fileName = "textured_sphere_smooth.obj";
fileName = "cube.obj";
{
@@ -181,7 +183,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
m_guiHelper->getRenderInterface()->writeTransforms();
m_internalData->m_shapePtr.push_back(0);
TinyRenderObjectData* ob = new TinyRenderObjectData(m_internalData->m_width,m_internalData->m_height,
TinyRenderObjectData* ob = new TinyRenderObjectData(
m_internalData->m_rgbColorBuffer,
m_internalData->m_depthBuffer);
//ob->loadModel("cube.obj");

231
examples/RoboticsLearning/GripperGraspExample.cpp Normal file
View File

@@ -0,0 +1,231 @@
#include "GripperGraspExample.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include <string>
#include "b3RobotSimAPI.h"
#include "../Utils/b3Clock.h"
static btScalar sGripperVerticalVelocity = -0.2f;
static btScalar sGripperClosingTargetVelocity = 0.5f;
class GripperGraspExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper;
b3RobotSimAPI m_robotSim;
int m_options;
int m_r2d2Index;
int m_gripperIndex;
float m_x;
float m_y;
float m_z;
b3AlignedObjectArray<int> m_movingInstances;
enum
{
numCubesX = 20,
numCubesY = 20
};
public:
GripperGraspExample(GUIHelperInterface* helper, int options)
:m_app(helper->getAppInterface()),
m_guiHelper(helper),
m_options(options),
m_r2d2Index(-1),
m_gripperIndex(-1),
m_x(0),
m_y(0),
m_z(0)
{
m_app->setUpAxis(2);
}
virtual ~GripperGraspExample()
{
m_app->m_renderer->enableBlend(false);
}
virtual void physicsDebugDraw(int debugDrawMode)
{
}
virtual void initPhysics()
{
bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
{
{
SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
slider.m_minVal=-2;
slider.m_maxVal=2;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity
);
slider.m_minVal=-1;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf";
args.m_fileType = B3_SDF_FILE;
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
{
m_gripperIndex = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
}
/*
int fingerJointIndices[2]={1,3};
double fingerTargetPositions[2]={-0.04,0.04};
for (int i=0;i<2;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_targetPosition = fingerTargetPositions[i];
controlArgs.m_kp = 5.0;
controlArgs.m_kd = 3.0;
controlArgs.m_maxTorqueValue = 1.0;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
*/
int fingerJointIndices[3]={0,1,3};
double fingerTargetVelocities[3]={-0.2,.5,-.5};
double maxTorqueValues[3]={40.0,50.0,50.0};
for (int i=0;i<3;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
}
}
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_small.urdf";
args.m_startPosition.setValue(0,0,.107);
args.m_startOrientation.setEulerZYX(0,0,0);
m_robotSim.loadFile(args,results);
}
if (1)
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
}
virtual void exitPhysics()
{
m_robotSim.disconnect();
}
virtual void stepSimulation(float deltaTime)
{
if ((m_gripperIndex>=0))
{
int fingerJointIndices[3]={0,1,3};
double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
,-sGripperClosingTargetVelocity
};
double maxTorqueValues[3]={40.0,50.0,50.0};
for (int i=0;i<3;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
}
m_robotSim.stepSimulation();
}
virtual void renderScene()
{
m_robotSim.renderScene();
//m_app->m_renderer->renderScene();
}
virtual void physicsDebugDraw()
{
}
virtual bool mouseMoveCallback(float x,float y)
{
return false;
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return false;
}
virtual bool keyboardCallback(int key, int state)
{
return false;
}
virtual void resetCamera()
{
float dist = 1.5;
float pitch = 12;
float yaw = -10;
float targetPos[3]={-0.2,0.8,0.3};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
}
}
};
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options)
{
return new GripperGraspExample(options.m_guiHelper, options.m_option);
}

23
examples/RoboticsLearning/GripperGraspExample.h Normal file
View File

@@ -0,0 +1,23 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2016 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
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 GRIPPER_GRASP_EXAMPLE_H
#define GRIPPER_GRASP_EXAMPLE_H
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);
#endif //GRIPPER_GRASP_EXAMPLE_H

204
examples/RoboticsLearning/R2D2GraspExample.cpp Normal file
View File

@@ -0,0 +1,204 @@
#include "R2D2GraspExample.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include <string>
#include "b3RobotSimAPI.h"
#include "../Utils/b3Clock.h"
///quick demo showing the right-handed coordinate system and positive rotations around each axis
class R2D2GraspExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper;
b3RobotSimAPI m_robotSim;
int m_options;
int m_r2d2Index;
float m_x;
float m_y;
float m_z;
b3AlignedObjectArray<int> m_movingInstances;
enum
{
numCubesX = 20,
numCubesY = 20
};
public:
R2D2GraspExample(GUIHelperInterface* helper, int options)
:m_app(helper->getAppInterface()),
m_guiHelper(helper),
m_options(options),
m_r2d2Index(-1),
m_x(0),
m_y(0),
m_z(0)
{
m_app->setUpAxis(2);
}
virtual ~R2D2GraspExample()
{
m_app->m_renderer->enableBlend(false);
}
virtual void physicsDebugDraw(int debugDrawMode)
{
}
virtual void initPhysics()
{
bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
if ((m_options & eROBOTIC_LEARN_GRASP)!=0)
{
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "r2d2.urdf";
args.m_startPosition.setValue(0,0,.5);
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
{
int m_r2d2Index = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_r2d2Index);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_r2d2Index,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
}
int wheelJointIndices[4]={2,3,6,7};
int wheelTargetVelocities[4]={-10,-10,-10,-10};
for (int i=0;i<4;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = wheelTargetVelocities[i];
controlArgs.m_maxTorqueValue = 1e30;
m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs);
}
}
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kiva_shelf/model.sdf";
args.m_startPosition.setValue(0,0,.5);
args.m_startOrientation = b3Quaternion(0,B3_HALF_PI,0);
args.m_forceOverrideFixedBase = true;
args.m_fileType = B3_SDF_FILE;
//args.m_startOrientation = b3Quaternion()
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
}
if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0)
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
{
args.m_fileName = "cube.urdf";
args.m_startPosition.setValue(0,0,2.5);
args.m_startOrientation.setEulerZYX(0,0.2,0);
m_robotSim.loadFile(args,results);
}
{
args.m_fileName = "cube_no_friction.urdf";
args.m_startPosition.setValue(0,2,2.5);
args.m_startOrientation.setEulerZYX(0,0.2,0);
m_robotSim.loadFile(args,results);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0.2,0);
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
}
}
virtual void exitPhysics()
{
m_robotSim.disconnect();
}
virtual void stepSimulation(float deltaTime)
{
m_robotSim.stepSimulation();
}
virtual void renderScene()
{
m_robotSim.renderScene();
//m_app->m_renderer->renderScene();
}
virtual void physicsDebugDraw()
{
}
virtual bool mouseMoveCallback(float x,float y)
{
return false;
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return false;
}
virtual bool keyboardCallback(int key, int state)
{
return false;
}
virtual void resetCamera()
{
float dist = 3;
float pitch = -75;
float yaw = 30;
float targetPos[3]={-0.2,0.8,0.3};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
}
}
};
class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options)
{
return new R2D2GraspExample(options.m_guiHelper, options.m_option);
}

28
examples/RoboticsLearning/R2D2GraspExample.h Normal file
View File

@@ -0,0 +1,28 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2016 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
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 R2D2_GRASP_EXAMPLE_H
#define R2D2_GRASP_EXAMPLE_H
enum RobotLearningExampleOptions
{
eROBOTIC_LEARN_GRASP=1,
eROBOTIC_LEARN_COMPLIANT_CONTACT=2,
};
class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options);
#endif //R2D2_GRASP_EXAMPLE_H

762
examples/RoboticsLearning/b3RobotSimAPI.cpp Normal file
View File

@@ -0,0 +1,762 @@
#include "b3RobotSimAPI.h"
//#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
//#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include <string>
#include "../Utils/b3Clock.h"
#include "../MultiThreading/b3ThreadSupportInterface.h"
void RobotThreadFunc(void* userPtr,void* lsMemory);
void* RobotlsMemoryFunc();
#define MAX_ROBOT_NUM_THREADS 1
enum
{
numCubesX = 20,
numCubesY = 20
};
enum TestRobotSimCommunicationEnums
{
eRequestTerminateRobotSim= 13,
eRobotSimIsUnInitialized,
eRobotSimIsInitialized,
eRobotSimInitializationFailed,
eRobotSimHasTerminated
};
enum MultiThreadedGUIHelperCommunicationEnums
{
eRobotSimGUIHelperIdle= 13,
eRobotSimGUIHelperRegisterTexture,
eRobotSimGUIHelperRegisterGraphicsShape,
eRobotSimGUIHelperRegisterGraphicsInstance,
eRobotSimGUIHelperCreateCollisionShapeGraphicsObject,
eRobotSimGUIHelperCreateCollisionObjectGraphicsObject,
eRobotSimGUIHelperRemoveAllGraphicsInstances,
};
#include <stdio.h>
//#include "BulletMultiThreaded/PlatformDefinitions.h"
#ifndef _WIN32
#include "../MultiThreading/b3PosixThreadSupport.h"
b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads)
{
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("RobotSimThreads",
RobotThreadFunc,
RobotlsMemoryFunc,
numThreads);
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
return threadSupport;
}
#elif defined( _WIN32)
#include "../MultiThreading/b3Win32ThreadSupport.h"
b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads)
{
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("RobotSimThreads",RobotThreadFunc,RobotlsMemoryFunc,numThreads);
b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
return threadSupport;
}
#endif
struct RobotSimArgs
{
RobotSimArgs()
:m_physicsServerPtr(0)
{
}
b3CriticalSection* m_cs;
PhysicsServerSharedMemory* m_physicsServerPtr;
b3AlignedObjectArray<b3Vector3> m_positions;
};
struct RobotSimThreadLocalStorage
{
int threadId;
};
void RobotThreadFunc(void* userPtr,void* lsMemory)
{
printf("RobotThreadFunc thread started\n");
RobotSimThreadLocalStorage* localStorage = (RobotSimThreadLocalStorage*) lsMemory;
RobotSimArgs* args = (RobotSimArgs*) userPtr;
int workLeft = true;
b3Clock clock;
clock.reset();
bool init = true;
if (init)
{
args->m_cs->lock();
args->m_cs->setSharedParam(0,eRobotSimIsInitialized);
args->m_cs->unlock();
do
{
//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread?
#if 0
double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.;
if (deltaTimeInSeconds<(1./260.))
{
if (deltaTimeInSeconds<.001)
continue;
}
clock.reset();
#endif //
args->m_physicsServerPtr->processClientCommands();
} while (args->m_cs->getSharedParam(0)!=eRequestTerminateRobotSim);
} else
{
args->m_cs->lock();
args->m_cs->setSharedParam(0,eRobotSimInitializationFailed);
args->m_cs->unlock();
}
//do nothing
}
void* RobotlsMemoryFunc()
{
//don't create local store memory, just return 0
return new RobotSimThreadLocalStorage;
}
ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
{
CommonGraphicsApp* m_app;
b3CriticalSection* m_cs;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
GUIHelperInterface* m_childGuiHelper;
const unsigned char* m_texels;
int m_textureWidth;
int m_textureHeight;
int m_shapeIndex;
const float* m_position;
const float* m_quaternion;
const float* m_color;
const float* m_scaling;
const float* m_vertices;
int m_numvertices;
const int* m_indices;
int m_numIndices;
int m_primitiveType;
int m_textureId;
int m_instanceId;
MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
:m_app(app)
,m_cs(0),
m_texels(0),
m_textureId(-1)
{
m_childGuiHelper = guiHelper;;
}
virtual ~MultiThreadedOpenGLGuiHelper()
{
delete m_childGuiHelper;
}
void setCriticalSection(b3CriticalSection* cs)
{
m_cs = cs;
}
b3CriticalSection* getCriticalSection()
{
return m_cs;
}
virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){}
btCollisionObject* m_obj;
btVector3 m_color2;
virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
{
m_obj = obj;
m_color2 = color;
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionObjectGraphicsObject);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
}
btCollisionShape* m_colShape;
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
{
m_colShape = collisionShape;
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionShapeGraphicsObject);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
}
virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
{
//this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects.
//the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms
if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0)
{
m_childGuiHelper->syncPhysicsToGraphics(rbWorld);
}
}
virtual void render(const btDiscreteDynamicsWorld* rbWorld)
{
m_childGuiHelper->render(0);
}
virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
virtual int registerTexture(const unsigned char* texels, int width, int height)
{
m_texels = texels;
m_textureWidth = width;
m_textureHeight = height;
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterTexture);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
return m_textureId;
}
virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
{
m_vertices = vertices;
m_numvertices = numvertices;
m_indices = indices;
m_numIndices = numIndices;
m_primitiveType = primitiveType;
m_textureId = textureId;
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsShape);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
return m_shapeIndex;
}
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
{
m_shapeIndex = shapeIndex;
m_position = position;
m_quaternion = quaternion;
m_color = color;
m_scaling = scaling;
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsInstance);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
return m_instanceId;
}
virtual void removeAllGraphicsInstances()
{
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperRemoveAllGraphicsInstances);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
}
virtual Common2dCanvasInterface* get2dCanvasInterface()
{
return 0;
}
virtual CommonParameterInterface* getParameterInterface()
{
return 0;
}
virtual CommonRenderInterface* getRenderInterface()
{
return 0;
}
virtual CommonGraphicsApp* getAppInterface()
{
return m_childGuiHelper->getAppInterface();
}
virtual void setUpAxis(int axis)
{
m_childGuiHelper->setUpAxis(axis);
}
virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
{
}
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)
{
if (numPixelsCopied)
*numPixelsCopied = 0;
}
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
{
}
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
{
}
};
struct b3RobotSimAPI_InternalData
{
//GUIHelperInterface* m_guiHelper;
PhysicsServerSharedMemory m_physicsServer;
b3PhysicsClientHandle m_physicsClient;
b3ThreadSupportInterface* m_threadSupport;
RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS];
MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper;
bool m_connected;
b3RobotSimAPI_InternalData()
:m_multiThreadedHelper(0),
m_physicsClient(0),
m_connected(false)
{
}
};
b3RobotSimAPI::b3RobotSimAPI()
{
m_data = new b3RobotSimAPI_InternalData;
}
void b3RobotSimAPI::stepSimulation()
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3Assert(b3CanSubmitCommand(m_data->m_physicsClient));
if (b3CanSubmitCommand(m_data->m_physicsClient))
{
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3InitStepSimulationCommand(m_data->m_physicsClient));
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED);
}
}
void b3RobotSimAPI::setGravity(const b3Vector3& gravityAcceleration)
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient);
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
}
b3RobotSimAPI::~b3RobotSimAPI()
{
delete m_data;
}
void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
{
switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
{
case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject:
{
m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_data->m_multiThreadedHelper->m_colShape);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperCreateCollisionObjectGraphicsObject:
{
m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_data->m_multiThreadedHelper->m_obj,
m_data->m_multiThreadedHelper->m_color2);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperRegisterTexture:
{
m_data->m_multiThreadedHelper->m_textureId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_data->m_multiThreadedHelper->m_texels,
m_data->m_multiThreadedHelper->m_textureWidth,m_data->m_multiThreadedHelper->m_textureHeight);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperRegisterGraphicsShape:
{
m_data->m_multiThreadedHelper->m_shapeIndex = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape(
m_data->m_multiThreadedHelper->m_vertices,
m_data->m_multiThreadedHelper->m_numvertices,
m_data->m_multiThreadedHelper->m_indices,
m_data->m_multiThreadedHelper->m_numIndices,
m_data->m_multiThreadedHelper->m_primitiveType,
m_data->m_multiThreadedHelper->m_textureId);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperRegisterGraphicsInstance:
{
m_data->m_multiThreadedHelper->m_instanceId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance(
m_data->m_multiThreadedHelper->m_shapeIndex,
m_data->m_multiThreadedHelper->m_position,
m_data->m_multiThreadedHelper->m_quaternion,
m_data->m_multiThreadedHelper->m_color,
m_data->m_multiThreadedHelper->m_scaling);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperRemoveAllGraphicsInstances:
{
m_data->m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
int numRenderInstances = m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
b3Assert(numRenderInstances==0);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperIdle:
default:
{
}
}
#if 0
if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK)
{
btClock rtc;
btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
while (rtc.getTimeMilliseconds()<endTime)
{
m_physicsServer.processClientCommands();
}
} else
{
//for (int i=0;i<10;i++)
m_physicsServer.processClientCommands();
}
#endif
}
b3SharedMemoryStatusHandle b3RobotSimAPI::submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle)
{
int timeout = 1024*1024*1024;
b3SharedMemoryStatusHandle statusHandle=0;
b3SubmitClientCommand(physClient,commandHandle);
while ((statusHandle==0) && (timeout-- > 0))
{
statusHandle =b3ProcessServerStatus(physClient);
processMultiThreadedGraphicsRequests();
}
return (b3SharedMemoryStatusHandle) statusHandle;
}
int b3RobotSimAPI::getNumJoints(int bodyUniqueId) const
{
return b3GetNumJoints(m_data->m_physicsClient,bodyUniqueId);
}
bool b3RobotSimAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
{
return (b3GetJointInfo(m_data->m_physicsClient,bodyUniqueId, jointIndex,jointInfo)!=0);
}
void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointMotorArgs& args)
{
b3SharedMemoryStatusHandle statusHandle;
switch (args.m_controlMode)
{
case CONTROL_MODE_VELOCITY:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClient, bodyUniqueId, CONTROL_MODE_VELOCITY);
b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex;
b3JointControlSetKd(command,uIndex,args.m_kd);
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClient, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex;
int qIndex = jointInfo.m_qIndex;
b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition);
b3JointControlSetKp(command,uIndex,args.m_kp);
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
b3JointControlSetKd(command,uIndex,args.m_kd);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
break;
}
case CONTROL_MODE_TORQUE:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClient, bodyUniqueId, CONTROL_MODE_TORQUE);
b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex;
b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
break;
}
default:
{
b3Error("Unknown control command in b3RobotSimAPI::setJointMotorControl");
}
}
}
bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotSimLoadFileResults& results)
{
bool statusOk = false;
int robotUniqueId = -1;
b3Assert(m_data->m_connected);
switch (args.m_fileType)
{
case B3_URDF_FILE:
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str());
//setting the initial position, orientation and other arguments are optional
b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
args.m_startPosition[1],
args.m_startPosition[2]);
b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0]
,args.m_startOrientation[1]
,args.m_startOrientation[2]
,args.m_startOrientation[3]);
if (args.m_forceOverrideFixedBase)
{
b3LoadUrdfCommandSetUseFixedBase(command,true);
}
statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
robotUniqueId = b3GetStatusBodyIndex(statusHandle);
results.m_uniqueObjectIds.push_back(robotUniqueId);
statusOk = true;
break;
}
case B3_SDF_FILE:
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str());
statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
if (statusType == CMD_SDF_LOADING_COMPLETED)
{
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
if (numBodies)
{
results.m_uniqueObjectIds.resize(numBodies);
int numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
}
statusOk = true;
}
break;
}
default:
{
b3Warning("Unknown file type in b3RobotSimAPI::loadFile");
}
}
return statusOk;
}
bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper)
{
m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper);
MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper);
m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS);
for (int i=0;i<m_data->m_threadSupport->getNumTasks();i++)
{
RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*) m_data->m_threadSupport->getThreadLocalMemory(i);
b3Assert(storage);
storage->threadId = i;
//storage->m_sharedMem = data->m_sharedMem;
}
for (int w=0;w<MAX_ROBOT_NUM_THREADS;w++)
{
m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection();
m_data->m_args[w].m_cs->setSharedParam(0,eRobotSimIsUnInitialized);
int numMoving = 0;
m_data->m_args[w].m_positions.resize(numMoving);
m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer;
int index = 0;
m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &m_data->m_args[w], w);
while (m_data->m_args[w].m_cs->getSharedParam(0)==eRobotSimIsUnInitialized)
{
}
}
m_data->m_args[0].m_cs->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs);
m_data->m_connected = m_data->m_physicsServer.connectSharedMemory( m_data->m_multiThreadedHelper);
b3Assert(m_data->m_connected);
m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
int canSubmit = b3CanSubmitCommand(m_data->m_physicsClient);
b3Assert(canSubmit);
return m_data->m_connected && canSubmit;
}
void b3RobotSimAPI::disconnect()
{
for (int i=0;i<MAX_ROBOT_NUM_THREADS;i++)
{
m_data->m_args[i].m_cs->lock();
m_data->m_args[i].m_cs->setSharedParam(0,eRequestTerminateRobotSim);
m_data->m_args[i].m_cs->unlock();
}
int numActiveThreads = MAX_ROBOT_NUM_THREADS;
while (numActiveThreads)
{
int arg0,arg1;
if (m_data->m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
{
numActiveThreads--;
printf("numActiveThreads = %d\n",numActiveThreads);
} else
{
}
};
printf("stopping threads\n");
delete m_data->m_threadSupport;
m_data->m_threadSupport = 0;
b3DisconnectSharedMemory(m_data->m_physicsClient);
m_data->m_physicsServer.disconnectSharedMemory(true);
m_data->m_connected = false;
}
void b3RobotSimAPI::renderScene()
{
if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
{
m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
}
m_data->m_physicsServer.renderScene();
}

99
examples/RoboticsLearning/b3RobotSimAPI.h Normal file
View File

@@ -0,0 +1,99 @@
#ifndef B3_ROBOT_SIM_API_H
#define B3_ROBOT_SIM_API_H
///todo: remove those includes from this header
#include "../SharedMemory/PhysicsClientC_API.h"
#include "../SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include <string>
enum b3RigidSimFileType
{
B3_URDF_FILE=1,
B3_SDF_FILE,
B3_AUTO_DETECT_FILE//todo based on extension
};
struct b3RobotSimLoadFileArgs
{
std::string m_fileName;
b3Vector3 m_startPosition;
b3Quaternion m_startOrientation;
bool m_forceOverrideFixedBase;
int m_fileType;
b3RobotSimLoadFileArgs(const std::string& fileName)
:m_fileName(fileName),
m_startPosition(b3MakeVector3(0,0,0)),
m_startOrientation(b3Quaternion(0,0,0,1)),
m_forceOverrideFixedBase(false),
m_fileType(B3_URDF_FILE)
{
}
};
struct b3RobotSimLoadFileResults
{
b3AlignedObjectArray<int> m_uniqueObjectIds;
b3RobotSimLoadFileResults()
{
}
};
struct b3JointMotorArgs
{
int m_controlMode;
double m_targetPosition;
double m_kp;
double m_targetVelocity;
double m_kd;
double m_maxTorqueValue;
b3JointMotorArgs(int controlMode)
:m_controlMode(controlMode),
m_targetPosition(0),
m_kp(0.1),
m_targetVelocity(0),
m_kd(0.9),
m_maxTorqueValue(1000)
{
}
};
class b3RobotSimAPI
{
struct b3RobotSimAPI_InternalData* m_data;
void processMultiThreadedGraphicsRequests();
b3SharedMemoryStatusHandle submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
public:
b3RobotSimAPI();
virtual ~b3RobotSimAPI();
bool connect(struct GUIHelperInterface* guiHelper);
void disconnect();
bool loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotSimLoadFileResults& results);
int getNumJoints(int bodyUniqueId) const;
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args);
void stepSimulation();
void setGravity(const b3Vector3& gravityAcceleration);
void renderScene();
};
#endif //B3_ROBOT_SIM_API_H

3
examples/RollingFrictionDemo/RollingFrictionDemo.cpp
View File

@@ -234,9 +234,8 @@ void RollingFrictionDemo::initPhysics()
{
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
b3ResourcePath p;
char resourcePath[1024];
if (p.findResourcePath("slope.bullet",resourcePath,1024))
if (b3ResourcePath::findResourcePath("slope.bullet",resourcePath,1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);

2
examples/SharedMemory/PhysicsClient.h
View File

@@ -26,7 +26,7 @@ public:
virtual int getNumJoints(int bodyIndex) const = 0;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0;

302
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1,10 +1,34 @@
#include "PhysicsClientC_API.h"
#include "PhysicsClientSharedMemory.h"
#include "Bullet3Common/b3Scalar.h"
#include "Bullet3Common/b3Vector3.h"
#include <string.h>
#include "SharedMemoryCommands.h"
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_SDF;
int len = strlen(sdfFileName);
if (len<MAX_SDF_FILENAME_LENGTH)
{
strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
} else
{
command->m_sdfArguments.m_sdfFileName[0] = 0;
}
command->m_updateFlags = SDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle) command;
}
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
@@ -91,6 +115,15 @@ int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, doub
return 0;
}
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_allowRealTimeSimulation = enableRealTimeSimulation;
command->m_updateFlags |= SIM_PARAM_UPDATE_REAL_TIME_SIMULATION;
return 0;
}
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -127,7 +160,12 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand
}
b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode)
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode)
{
return b3JointControlCommandInit2(physClient,0,controlMode);
}
b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -136,8 +174,12 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy
b3Assert(command);
command->m_type = CMD_SEND_DESIRED_STATE;
command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = 0;
command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId;
command->m_updateFlags = 0;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0;
}
return (b3SharedMemoryCommandHandle) command;
}
@@ -146,6 +188,9 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
return 0;
}
@@ -154,6 +199,9 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
return 0;
}
@@ -162,6 +210,9 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
return 0;
}
@@ -170,6 +221,9 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
return 0;
}
@@ -179,6 +233,9 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0;
}
@@ -187,6 +244,9 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0;
}
@@ -335,6 +395,7 @@ int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
@@ -343,6 +404,11 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl
command->m_type = CMD_INIT_POSE;
command->m_updateFlags =0;
command->m_initPoseArgs.m_bodyUniqueId = bodyIndex;
//a bit slow, initialing the full range to zero...
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command->m_initPoseArgs.m_hasInitialStateQ[i] = 0;
}
return (b3SharedMemoryCommandHandle) command;
}
@@ -355,6 +421,11 @@ int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle
command->m_initPoseArgs.m_initialStateQ[0] = startPosX;
command->m_initPoseArgs.m_initialStateQ[1] = startPosY;
command->m_initPoseArgs.m_initialStateQ[2] = startPosZ;
command->m_initPoseArgs.m_hasInitialStateQ[0] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[1] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[2] = 1;
return 0;
}
@@ -368,6 +439,12 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan
command->m_initPoseArgs.m_initialStateQ[4] = startOrnY;
command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ;
command->m_initPoseArgs.m_initialStateQ[6] = startOrnW;
command->m_initPoseArgs.m_hasInitialStateQ[3] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[4] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[5] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[6] = 1;
return 0;
}
@@ -380,6 +457,7 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
for (int i=0;i<numJointPositions;i++)
{
command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i];
command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1;
}
return 0;
}
@@ -396,6 +474,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0)
{
command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition;
command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1;
}
return 0;
}
@@ -403,7 +482,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient)
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -414,7 +493,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys
command->m_type = CMD_CREATE_SENSOR;
command->m_updateFlags = 0;
command->m_createSensorArguments.m_numJointSensorChanges = 0;
command->m_createSensorArguments.m_bodyUniqueId = 0;
command->m_createSensorArguments.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
@@ -489,6 +568,33 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
return CMD_INVALID_STATUS;
}
int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity)
{
int numBodies = 0;
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
if (status)
{
switch (status->m_type)
{
case CMD_SDF_LOADING_COMPLETED:
{
int i,maxBodies;
numBodies = status->m_sdfLoadedArgs.m_numBodies;
maxBodies = btMin(bodyIndicesCapacity, numBodies);
for (i=0;i<maxBodies;i++)
{
bodyIndicesOut[i] = status->m_sdfLoadedArgs.m_bodyUniqueIds[i];
}
break;
}
}
}
return numBodies;
}
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
@@ -528,6 +634,10 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* jointReactionForces[]) {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
const SendActualStateArgs &args = status->m_sendActualStateArgs;
btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED);
if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
return false;
if (bodyUniqueId) {
*bodyUniqueId = args.m_bodyUniqueId;
}
@@ -592,10 +702,10 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
}
void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
cl->getJointInfo(bodyIndex, linkIndex,*info);
return cl->getJointInfo(bodyIndex, jointIndex, *info);
}
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
@@ -691,6 +801,14 @@ b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physC
return (b3SharedMemoryCommandHandle) command;
}
void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
b3Assert(renderer>(1<<15));
command->m_updateFlags |= renderer;
}
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16])
{
@@ -705,6 +823,131 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3])
{
b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]);
b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
b3Vector3 up = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]);
b3Vector3 f = (center - eye).normalized();
b3Vector3 u = up.normalized();
b3Vector3 s = (f.cross(u)).normalized();
u = s.cross(f);
float viewMatrix[16];
viewMatrix[0*4+0] = s.x;
viewMatrix[1*4+0] = s.y;
viewMatrix[2*4+0] = s.z;
viewMatrix[0*4+1] = u.x;
viewMatrix[1*4+1] = u.y;
viewMatrix[2*4+1] = u.z;
viewMatrix[0*4+2] =-f.x;
viewMatrix[1*4+2] =-f.y;
viewMatrix[2*4+2] =-f.z;
viewMatrix[0*4+3] = 0.f;
viewMatrix[1*4+3] = 0.f;
viewMatrix[2*4+3] = 0.f;
viewMatrix[3*4+0] = -s.dot(eye);
viewMatrix[3*4+1] = -u.dot(eye);
viewMatrix[3*4+2] = f.dot(eye);
viewMatrix[3*4+3] = 1.f;
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_viewMatrix[i] = viewMatrix[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal)
{
float frustum[16];
frustum[0*4+0] = (float(2) * nearVal) / (right - left);
frustum[0*4+1] = float(0);
frustum[0*4+2] = float(0);
frustum[0*4+3] = float(0);
frustum[1*4+0] = float(0);
frustum[1*4+1] = (float(2) * nearVal) / (top - bottom);
frustum[1*4+2] = float(0);
frustum[1*4+3] = float(0);
frustum[2*4+0] = (right + left) / (right - left);
frustum[2*4+1] = (top + bottom) / (top - bottom);
frustum[2*4+2] = -(farVal + nearVal) / (farVal - nearVal);
frustum[2*4+3] = float(-1);
frustum[3*4+0] = float(0);
frustum[3*4+1] = float(0);
frustum[3*4+2] = -(float(2) * farVal * nearVal) / (farVal - nearVal);
frustum[3*4+3] = float(0);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_projectionMatrix[i] = frustum[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float fov, float aspect, float nearVal, float farVal)
{
float yScale = 1.0 / tan((3.141592538 / 180.0) * fov / 2);
float xScale = yScale / aspect;
float frustum[16];
frustum[0*4+0] = xScale;
frustum[0*4+1] = float(0);
frustum[0*4+2] = float(0);
frustum[0*4+3] = float(0);
frustum[1*4+0] = float(0);
frustum[1*4+1] = yScale;
frustum[1*4+2] = float(0);
frustum[1*4+3] = float(0);
frustum[2*4+0] = 0;
frustum[2*4+1] = 0;
frustum[2*4+2] = (nearVal + farVal) / (nearVal - farVal);
frustum[2*4+3] = float(-1);
frustum[3*4+0] = float(0);
frustum[3*4+1] = float(0);
frustum[3*4+2] = (float(2) * farVal * nearVal) / (nearVal - farVal);
frustum[3*4+3] = float(0);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_projectionMatrix[i] = frustum[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height )
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_pixelWidth = width;
command->m_requestPixelDataArguments.m_pixelHeight = height;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT;
}
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -714,3 +957,50 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
}
}
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_APPLY_EXTERNAL_FORCE;
command->m_updateFlags = 0;
command->m_externalForceArguments.m_numForcesAndTorques = 0;
return (b3SharedMemoryCommandHandle) command;
}
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
int index = command->m_externalForceArguments.m_numForcesAndTorques;
command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
command->m_externalForceArguments.m_linkIds[index] = linkId;
command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE+flag;
for (int i = 0; i < 3; ++i) {
command->m_externalForceArguments.m_forcesAndTorques[index+i] = force[i];
command->m_externalForceArguments.m_positions[index+i] = position[i];
}
command->m_externalForceArguments.m_numForcesAndTorques++;
}
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
int index = command->m_externalForceArguments.m_numForcesAndTorques;
command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
command->m_externalForceArguments.m_linkIds[index] = linkId;
command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE+flag;
for (int i = 0; i < 3; ++i) {
command->m_externalForceArguments.m_forcesAndTorques[index+i] = torque[i];
}
command->m_externalForceArguments.m_numForcesAndTorques++;
}

33
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -40,6 +40,8 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien
/// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes.
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
@@ -54,8 +56,8 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
///give a unique body index (after loading the body) return the number of joints.
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
///given a body and link index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
@@ -68,12 +70,18 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]);
void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal);
void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal);
void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height );
void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
@@ -88,14 +96,22 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
///Set joint control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
///Set joint motor control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
//Only use when controlMode is CONTROL_MODE_VELOCITY
///Only use when controlMode is CONTROL_MODE_VELOCITY
int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///Only use if when controlMode is CONTROL_MODE_TORQUE,
@@ -127,7 +143,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
///This is rather inconsistent, to mix programmatical creation with loading from file.
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
///b3CreateSensorEnableIMUForLink is not implemented yet.
///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.
@@ -146,6 +162,11 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d
double rayToWorldZ);
b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
#ifdef __cplusplus
}
#endif

238
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -21,8 +21,8 @@ struct MyMotorInfo2
int m_qIndex;
};
int camVisualizerWidth = 640;//1024/3;
int camVisualizerHeight = 480;//768/3;
int camVisualizerWidth = 320;//1024/3;
int camVisualizerHeight = 240;//768/3;
#define MAX_NUM_MOTORS 128
@@ -37,6 +37,9 @@ protected:
bool m_wantsTermination;
btAlignedObjectArray<int> m_userCommandRequests;
btAlignedObjectArray<int> m_bodyUniqueIds;
int m_sharedMemoryKey;
int m_selectedBody;
int m_prevSelectedBody;
@@ -66,7 +69,8 @@ protected:
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
int bodyIndex = comboIndex;
int itemIndex = int(atoi(name));
int bodyIndex = m_bodyUniqueIds[itemIndex];
if (m_selectedBody != bodyIndex)
{
m_selectedBody = bodyIndex;
@@ -77,10 +81,10 @@ protected:
virtual void resetCamera()
{
float dist = 5;
float pitch = 50;
float yaw = 35;
float targetPos[3]={0,0,0};//-3,2.8,-2.5};
float dist = 4;
float pitch = 193;
float yaw = 25;
float targetPos[3]={0,0,0.5};//-3,2.8,-2.5};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
@@ -154,20 +158,23 @@ protected:
void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle)
{
for (int i=0;i<m_numMotors;i++)
{
// btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
// int uIndex = m_motorTargetVelocities[i].m_uIndex;
// b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
int qIndex = m_motorTargetPositions[i].m_qIndex;
int uIndex = m_motorTargetPositions[i].m_uIndex;
b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
b3JointControlSetKp(commandHandle, uIndex, 0.1);
b3JointControlSetKd(commandHandle, uIndex, 0.0);
static int serial=0;
serial++;
// b3Printf("# motors = %d, cmd[%d] qIndex = %d, uIndex = %d, targetPos = %f", m_numMotors, serial, qIndex,uIndex,targetPos);
b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
b3JointControlSetDesiredVelocity(commandHandle,uIndex,0);
b3JointControlSetKp(commandHandle, qIndex, 0.2);
b3JointControlSetKd(commandHandle, uIndex, 1.);
b3JointControlSetMaximumForce(commandHandle,uIndex,5000);
}
}
virtual void physicsDebugDraw(int debugFlags)
@@ -191,7 +198,7 @@ protected:
void MyComboBoxCallback (int combobox, const char* item, void* userPointer)
{
b3Printf("Item selected %s", item);
//b3Printf("Item selected %s", item);
PhysicsClientExample* cl = (PhysicsClientExample*) userPointer;
b3Assert(cl);
@@ -227,18 +234,22 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
case CMD_LOAD_URDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
//setting the initial position, orientation and other arguments are optional
double startPosX = 0;
static double startPosY = 0;
static double startPosY = 1;
double startPosZ = 0;
b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
startPosY += 2.f;
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_LOAD_SDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.sdf");
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_CAMERA_IMAGE_DATA:
@@ -246,13 +257,16 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
//b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL);
float viewMatrix[16];
float projectionMatrix[16];
this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
@@ -363,11 +377,18 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
}
case CMD_SEND_DESIRED_STATE:
{
// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD);
if (m_selectedBody>=0)
{
// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
// b3Printf("prepare control command for body %d", m_selectedBody);
prepareControlCommand(command);
b3SubmitClientCommand(m_physicsClientHandle, command);
break;
}
break;
}
case CMD_RESET_SIMULATION:
{
@@ -430,6 +451,11 @@ PhysicsClientExample::~PhysicsClientExample()
bool deInitializeSharedMemory = true;
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
if (m_canvas && (m_canvasIndex>=0))
{
m_canvas->destroyCanvas(m_canvasIndex);
}
b3Printf("~PhysicsClientExample\n");
}
@@ -450,6 +476,7 @@ void PhysicsClientExample::createButtons()
m_guiHelper->getParameterInterface()->removeAllParameters();
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Load SDF",CMD_LOAD_SDF, isTrigger);
createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
@@ -459,14 +486,41 @@ void PhysicsClientExample::createButtons()
}
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger);
createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger);
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
if (m_bodyUniqueIds.size())
{
if (m_selectedBody<0)
m_selectedBody = 0;
ComboBoxParams comboParams;
comboParams.m_comboboxId = 0;
comboParams.m_numItems = m_bodyUniqueIds.size();
comboParams.m_startItem = m_selectedBody;
comboParams.m_callback = MyComboBoxCallback;
comboParams.m_userPointer = this;
//todo: get the real object name
const char** blarray = new const char*[m_bodyUniqueIds.size()];
for (int i=0;i<m_bodyUniqueIds.size();i++)
{
char* bla = new char[16];
sprintf(bla,"%d", i);
blarray[i] = bla;
comboParams.m_items=blarray;//{&bla};
}
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
}
if (m_physicsClientHandle && m_selectedBody>=0)
{
m_numMotors = 0;
int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
for (int i=0;i<numJoints;i++)
{
@@ -527,7 +581,6 @@ void PhysicsClientExample::initPhysics()
m_selectedBody = -1;
m_prevSelectedBody = -1;
if (m_options == eCLIENTEXAMPLE_SERVER)
{
m_canvas = m_guiHelper->get2dCanvasInterface();
if (m_canvas)
@@ -557,13 +610,22 @@ void PhysicsClientExample::initPhysics()
}
m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
}
m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
//m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
//m_physicsClientHandle = b3ConnectPhysicsDirect();
if (m_options == eCLIENTEXAMPLE_SERVER)
{
m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
}
if (m_options == eCLIENTEXAMPLE_DIRECT)
{
m_physicsClientHandle = b3ConnectPhysicsDirect();
} else
{
m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
//m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
}
if (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3Warning("Cannot connect to physics client");
@@ -603,48 +665,77 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
}
if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
{
static int counter=0;
char msg[1024];
sprintf(msg,"Camera image %d OK\n",counter++);
// static int counter=0;
// char msg[1024];
// sprintf(msg,"Camera image %d OK\n",counter++);
b3CameraImageData imageData;
b3GetCameraImageData(m_physicsClientHandle,&imageData);
if (m_canvas && m_canvasIndex >=0)
{
for (int i=0;i<imageData.m_pixelWidth;i++)
for (int i=0;i<camVisualizerWidth;i++)
{
for (int j=0;j<imageData.m_pixelHeight;j++)
for (int j=0;j<camVisualizerHeight;j++)
{
int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth)));
int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight)));
int xIndex = int(float(i)*(float(imageData.m_pixelWidth)/float(camVisualizerWidth)));
int yIndex = int(float(j)*(float(imageData.m_pixelHeight)/float(camVisualizerHeight)));
btClamp(yIndex,0,imageData.m_pixelHeight);
btClamp(xIndex,0,imageData.m_pixelWidth);
int bytesPerPixel = 4;
int bytesPerPixel = 4; //RGBA
int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex,
int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,i,j,
imageData.m_rgbColorData[pixelIndex],
imageData.m_rgbColorData[pixelIndex+1],
imageData.m_rgbColorData[pixelIndex+2],
255);
// imageData.m_rgbColorData[pixelIndex+3]);
255); //alpha set to 255
}
}
m_canvas->refreshImageData(m_canvasIndex);
}
b3Printf(msg);
// b3Printf(msg);
}
if (statusType == CMD_CAMERA_IMAGE_FAILED)
{
b3Printf("Camera image FAILED\n");
b3Warning("Camera image FAILED\n");
}
if (statusType == CMD_URDF_LOADING_COMPLETED)
if (statusType == CMD_SDF_LOADING_COMPLETED)
{
int bodyIndex = b3GetStatusBodyIndex(status);
if (bodyIndex>=0)
int bodyIndicesOut[1024];
int bodyCapacity = 1024;
int numBodies = b3GetStatusBodyIndices(status, bodyIndicesOut, bodyCapacity);
if (numBodies > bodyCapacity)
{
b3Warning("loadSDF number of bodies (%d) exceeds the internal body capacity (%d)",numBodies, bodyCapacity);
} else
{
for (int i=0;i<numBodies;i++)
{
int bodyUniqueId = bodyIndicesOut[i];
m_bodyUniqueIds.push_back(bodyUniqueId);
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
if (numJoints>0)
{
m_selectedBody = bodyUniqueId;
}
/* int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
b3Printf("numJoints = %d", numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyUniqueId,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
*/
}
}
//int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
//int bodyIndex = b3GetStatusBodyIndex(status);
/*if (bodyIndex>=0)
{
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
@@ -653,7 +744,6 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
ComboBoxParams comboParams;
comboParams.m_comboboxId = bodyIndex;
@@ -667,12 +757,29 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
comboParams.m_items=blarray;//{&bla};
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
}
*/
}
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
int bodyIndex = b3GetStatusBodyIndex(status);
if (bodyIndex>=0)
{
m_bodyUniqueIds.push_back(bodyIndex);
m_selectedBody = bodyIndex;
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
}
}
}
}
if (b3CanSubmitCommand(m_physicsClientHandle))
@@ -695,6 +802,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
{
m_selectedBody = -1;
m_numMotors=0;
m_bodyUniqueIds.clear();
createButtons();
b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
if (m_options == eCLIENTEXAMPLE_SERVER)
@@ -707,8 +815,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
b3Printf("Status after reset: %d",statusType);
//int statusType = b3GetStatusType(status);
//b3Printf("Status after reset: %d",statusType);
}
}
} else
@@ -725,13 +833,12 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
if (m_numMotors)
{
enqueueCommand(CMD_SEND_DESIRED_STATE);
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
if (m_options != eCLIENTEXAMPLE_SERVER)
{
enqueueCommand(CMD_REQUEST_DEBUG_LINES);
}
//enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
}
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
if (m_options != eCLIENTEXAMPLE_SERVER)
{
//enqueueCommand(CMD_REQUEST_DEBUG_LINES);
}
}
}
@@ -750,3 +857,4 @@ class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOpt
}
return example;
}

2
examples/SharedMemory/PhysicsClientExample.h
View File

@@ -4,7 +4,7 @@
enum ClientExampleOptions
{
eCLIENTEXAMPLE_LOOPBACK=1,
eCLIENTEAXMPLE_DIRECT=2,
eCLIENTEXAMPLE_DIRECT=2,
eCLIENTEXAMPLE_SERVER=3,
};

140
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -37,10 +37,13 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
SharedMemoryStatus m_lastServerStatus;
int m_counter;
bool m_serverLoadUrdfOK;
bool m_isConnected;
bool m_waitingForServer;
bool m_hasLastServerStatus;
@@ -54,7 +57,6 @@ struct PhysicsClientSharedMemoryInternalData {
m_counter(0),
m_cachedCameraPixelsWidth(0),
m_cachedCameraPixelsHeight(0),
m_serverLoadUrdfOK(false),
m_isConnected(false),
m_waitingForServer(false),
m_hasLastServerStatus(false),
@@ -83,15 +85,16 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
}
void PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
bool PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
info = bodyJoints->m_jointInfo[jointIndex];
return true;
}
return false;
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
@@ -168,6 +171,48 @@ bool PhysicsClientSharedMemory::connect() {
return true;
}
///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo
void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
{
bParse::btBulletFile bf(
&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++)
{
int flag = bf.getFlags();
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
{
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
if (bf.ok()) {
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else
{
b3Warning("Robot description not received");
}
}
const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
SharedMemoryStatus* stat = 0;
@@ -204,8 +249,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
break;
}
case CMD_SDF_LOADING_COMPLETED: {
if (m_data->m_verboseOutput) {
b3Printf("Server loading the SDF OK\n");
}
break;
}
case CMD_URDF_LOADING_COMPLETED: {
m_data->m_serverLoadUrdfOK = true;
if (m_data->m_verboseOutput) {
b3Printf("Server loading the URDF OK\n");
}
@@ -265,7 +318,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
if (m_data->m_verboseOutput) {
b3Printf("Server failed loading the URDF...\n");
}
m_data->m_serverLoadUrdfOK = false;
break;
}
case CMD_BODY_INFO_COMPLETED:
{
if (m_data->m_verboseOutput) {
b3Printf("Received body info\n");
}
int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
processBodyJointInfo(bodyUniqueId, serverCmd);
break;
}
case CMD_SDF_LOADING_FAILED: {
if (m_data->m_verboseOutput) {
b3Printf("Server failed loading the SDF...\n");
}
break;
}
@@ -448,8 +519,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
unsigned char* rgbaPixelsReceived =
(unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
printf("pixel = %d\n", rgbaPixelsReceived[0]);
// printf("pixel = %d\n", rgbaPixelsReceived[0]);
float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
{
m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
{
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
@@ -461,7 +539,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_CAMERA_IMAGE_FAILED:
{
b3Printf("Camera image FAILED\n");
b3Warning("Camera image FAILED\n");
break;
}
@@ -483,6 +561,50 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
m_data->m_waitingForServer = true;
}
if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED)
{
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (numBodies>0)
{
m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
for (int i=0;i<numBodies;i++)
{
m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]);
}
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
m_data->m_bodyIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//now transfer the information of the individual objects etc.
command.m_type = CMD_REQUEST_BODY_INFO;
command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
submitClientCommand(command);
return 0;
}
}
if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
{
//are there any bodies left to be processed?
if (m_data->m_bodyIdsRequestInfo.size())
{
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
m_data->m_bodyIdsRequestInfo.pop_back();
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//now transfer the information of the individual objects etc.
command.m_type = CMD_REQUEST_BODY_INFO;
command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
submitClientCommand(command);
return 0;
} else
{
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
}
}
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
@@ -537,6 +659,8 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const {
}
struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() {
static int sequence = 0;
m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++;
return &m_data->m_testBlock1->m_clientCommands[0];
}

4
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -11,7 +11,9 @@ class PhysicsClientSharedMemory : public PhysicsClient {
protected:
virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
public:
PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory();
@@ -34,7 +36,7 @@ public:
virtual int getNumJoints(int bodyIndex) const;
virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key);

Some files were not shown because too many files have changed in this diff Show More