add initial examples, replacing the 'Demos/Demos3'. Will make it work cross-platform, OpenGL3/OpenGL2 and add more examples to it.

This commit is contained in:
erwincoumans
2015-04-16 09:55:32 -07:00
parent d9feaf2d2a
commit a1bf9c5556
425 changed files with 255913 additions and 0 deletions

View File

@@ -0,0 +1,15 @@
#ifndef COMMON_2D_CANVAS_INTERFACE_H
#define COMMON_2D_CANVAS_INTERFACE_H
struct Common2dCanvasInterface
{
virtual ~Common2dCanvasInterface() {}
virtual int createCanvas(const char* canvasName, int width, int height)=0;
virtual void destroyCanvas(int canvasId)=0;
virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha)=0;
virtual void refreshImageData(int canvasId)=0;
};
#endif //COMMON_2D_CANVAS_INTERFACE_H

View File

@@ -0,0 +1,108 @@
#ifndef GUI_HELPER_INTERFACE_H
#define GUI_HELPER_INTERFACE_H
class btRigidBody;
class btVector3;
class btCollisionObject;
class btDiscreteDynamicsWorld;
class btCollisionShape;
struct Common2dCanvasInterface;
struct CommonParameterInterface;
struct CommonRenderInterface;
struct CommonGraphicsApp;
///The Bullet 2 GraphicsPhysicsBridge let's the graphics engine create graphics representation and synchronize
struct GUIHelperInterface
{
virtual ~GUIHelperInterface() {}
virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color) = 0;
virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color) = 0;
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)=0;
virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)=0;
virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld)=0;
virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) =0;
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) =0;
virtual Common2dCanvasInterface* get2dCanvasInterface()=0;
virtual CommonParameterInterface* getParameterInterface()=0;
virtual CommonRenderInterface* getRenderInterface()=0;
virtual CommonGraphicsApp* getAppInterface()=0;
virtual void setUpAxis(int axis)=0;
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0;
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0;
};
///the DummyGUIHelper does nothing, so we can test the examples without GUI/graphics (in 'console mode')
struct DummyGUIHelper : public GUIHelperInterface
{
DummyGUIHelper() {}
virtual ~DummyGUIHelper() {}
virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){}
virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color) {}
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape){}
virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld){}
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 Common2dCanvasInterface* get2dCanvasInterface()
{
return 0;
}
virtual CommonParameterInterface* getParameterInterface()
{
return 0;
}
virtual CommonRenderInterface* getRenderInterface()
{
return 0;
}
virtual CommonGraphicsApp* getAppInterface()
{
return 0;
}
virtual void setUpAxis(int axis)
{
}
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
{
}
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
{
}
};
#endif //GUI_HELPER_INTERFACE_H

View File

@@ -0,0 +1,54 @@
#ifndef COMMON_GRAPHICS_APP_H
#define COMMON_GRAPHICS_APP_H
struct DrawGridData
{
int gridSize;
float upOffset;
int upAxis;
float gridColor[4];
DrawGridData()
:gridSize(10),
upOffset(0.001f),
upAxis(1)
{
gridColor[0] = 0.6f;
gridColor[1] = 0.6f;
gridColor[2] = 0.6f;
gridColor[3] = 1.f;
}
};
struct CommonGraphicsApp
{
CommonGraphicsApp()
:m_window(0),
m_renderer(0),
m_parameterInterface(0),
m_2dCanvasInterface(0)
{
}
virtual ~CommonGraphicsApp()
{
}
class b3gWindowInterface* m_window;
struct CommonRenderInterface* m_renderer;
struct CommonParameterInterface* m_parameterInterface;
struct Common2dCanvasInterface* m_2dCanvasInterface;
virtual void drawGrid(DrawGridData data=DrawGridData()) = 0;
virtual void setUpAxis(int axis) = 0;
virtual int getUpAxis() const = 0;
virtual void swapBuffer() = 0;
virtual void drawText( const char* txt, int posX, int posY) = 0;
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0;
virtual int registerCubeShape(float halfExtentsX,float halfExtentsY, float halfExtentsZ)=0;
virtual int registerGraphicsSphereShape(float radius, bool usePointSprites=true, int largeSphereThreshold=100, int mediumSphereThreshold=10)=0;
virtual void registerGrid(int xres, int yres, float color0[4], float color1[4])=0;
};
#endif //COMMON_GRAPHICS_APP_H

View File

@@ -0,0 +1,451 @@
#ifndef COMMON_MULTI_BODY_SETUP_H
#define COMMON_MULTI_BODY_SETUP_H
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "btBulletDynamicsCommon.h"
#include "ExampleInterface.h"
#include "CommonGUIHelperInterface.h"
#include "CommonRenderInterface.h"
#include "CommonGraphicsAppInterface.h"
#include "CommonWindowInterface.h"
struct CommonMultiBodyBase : public ExampleInterface
{
//keep the collision shapes, for deletion/cleanup
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
btBroadphaseInterface* m_broadphase;
btCollisionDispatcher* m_dispatcher;
btMultiBodyConstraintSolver* m_solver;
btDefaultCollisionConfiguration* m_collisionConfiguration;
btMultiBodyDynamicsWorld* m_dynamicsWorld;
//data for picking objects
class btRigidBody* m_pickedBody;
class btTypedConstraint* m_pickedConstraint;
class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
btVector3 m_oldPickingPos;
btVector3 m_hitPos;
btScalar m_oldPickingDist;
bool m_prevCanSleep;
struct GUIHelperInterface* m_guiHelper;
CommonMultiBodyBase(GUIHelperInterface* helper)
:m_broadphase(0),
m_dispatcher(0),
m_solver(0),
m_collisionConfiguration(0),
m_dynamicsWorld(0),
m_pickedBody(0),
m_pickedConstraint(0),
m_pickingMultiBodyPoint2Point(0),
m_prevCanSleep(false),
m_guiHelper(helper)
{
}
virtual void createEmptyDynamicsWorld()
{
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
//m_collisionConfiguration->setConvexConvexMultipointIterations();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
m_solver = new btMultiBodyConstraintSolver;
m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
}
virtual void stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
{
m_dynamicsWorld->stepSimulation(deltaTime);
}
}
virtual void exitPhysics()
{
removePickingConstraint();
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
if (m_dynamicsWorld)
{
int i;
for (i = m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
{
m_dynamicsWorld->removeConstraint(m_dynamicsWorld->getConstraint(i));
}
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
}
//delete collision shapes
for (int j = 0; j<m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
virtual void syncPhysicsToGraphics()
{
if (m_dynamicsWorld)
{
m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
}
}
virtual void renderScene()
{
m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
if (m_guiHelper->getRenderInterface())
{
m_guiHelper->getRenderInterface()->renderScene();
}
}
virtual void physicsDebugDraw(int debugDrawFlags)
{
if (m_dynamicsWorld)
{
if (m_dynamicsWorld->getDebugDrawer())
{
m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
}
m_dynamicsWorld->debugDrawWorld();
}
}
virtual bool keyboardCallback(int key, int state)
{
return false;//don't handle this key
}
btVector3 getRayTo(int x,int y)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (!renderer)
{
btAssert(0);
return btVector3(0,0,0);
}
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = btScalar(2.0) * btAtan(tanFov);
btVector3 camPos,camTarget;
renderer->getCameraPosition(camPos);
renderer->getCameraTargetPosition(camTarget);
btVector3 rayFrom = camPos;
btVector3 rayForward = (camTarget-camPos);
rayForward.normalize();
float farPlane = 10000.f;
rayForward*= farPlane;
btVector3 rightOffset;
btVector3 cameraUp=btVector3(0,0,0);
cameraUp[m_guiHelper->getAppInterface()->getUpAxis()]=1;
btVector3 vertical = cameraUp;
btVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
btScalar aspect;
float width = float(renderer->getScreenWidth());
float height = float (renderer->getScreenHeight());
aspect = width / height;
hor*=aspect;
btVector3 rayToCenter = rayFrom + rayForward;
btVector3 dHor = hor * 1.f/width;
btVector3 dVert = vertical * 1.f/height;
btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += btScalar(x) * dHor;
rayTo -= btScalar(y) * dVert;
return rayTo;
}
virtual bool mouseMoveCallback(float x,float y)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (!renderer)
{
btAssert(0);
return false;
}
btVector3 rayTo = getRayTo(int(x), int(y));
btVector3 rayFrom;
renderer->getCameraPosition(rayFrom);
movePickedBody(rayFrom,rayTo);
return false;
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (!renderer)
{
btAssert(0);
return false;
}
b3gWindowInterface* window = m_guiHelper->getAppInterface()->m_window;
if (state==1)
{
if(button==0 && (!window->isModifiedKeyPressed(B3G_ALT) && !window->isModifiedKeyPressed(B3G_CONTROL) ))
{
btVector3 camPos;
renderer->getCameraPosition(camPos);
btVector3 rayFrom = camPos;
btVector3 rayTo = getRayTo(int(x),int(y));
pickBody(rayFrom, rayTo);
}
} else
{
if (button==0)
{
removePickingConstraint();
//remove p2p
}
}
//printf("button=%d, state=%d\n",button,state);
return false;
}
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
if (m_dynamicsWorld==0)
return false;
btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld);
m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback);
if (rayCallback.hasHit())
{
btVector3 pickPos = rayCallback.m_hitPointWorld;
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
if (body)
{
//other exclusions?
if (!(body->isStaticObject() || body->isKinematicObject()))
{
m_pickedBody = body;
m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot);
m_dynamicsWorld->addConstraint(p2p, true);
m_pickedConstraint = p2p;
btScalar mousePickClamping = 30.f;
p2p->m_setting.m_impulseClamp = mousePickClamping;
//very weak constraint for picking
p2p->m_setting.m_tau = 0.001f;
}
} else
{
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
if (multiCol && multiCol->m_multiBody)
{
m_prevCanSleep = multiCol->m_multiBody->getCanSleep();
multiCol->m_multiBody->setCanSleep(false);
btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos);
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos);
//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
btScalar scaling=1;
p2p->setMaxAppliedImpulse(2*scaling);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
world->addMultiBodyConstraint(p2p);
m_pickingMultiBodyPoint2Point =p2p;
}
}
// pickObject(pickPos, rayCallback.m_collisionObject);
m_oldPickingPos = rayToWorld;
m_hitPos = pickPos;
m_oldPickingDist = (pickPos - rayFromWorld).length();
// printf("hit !\n");
//add p2p
}
return false;
}
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
if (m_pickedBody && m_pickedConstraint)
{
btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_pickedConstraint);
if (pickCon)
{
//keep it at the same picking distance
btVector3 dir = rayToWorld-rayFromWorld;
dir.normalize();
dir *= m_oldPickingDist;
btVector3 newPivotB = rayFromWorld + dir;
pickCon->setPivotB(newPivotB);
}
}
if (m_pickingMultiBodyPoint2Point)
{
//keep it at the same picking distance
btVector3 dir = rayToWorld-rayFromWorld;
dir.normalize();
dir *= m_oldPickingDist;
btVector3 newPivotB = rayFromWorld + dir;
m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
}
return false;
}
virtual void removePickingConstraint()
{
if (m_pickedConstraint)
{
m_dynamicsWorld->removeConstraint(m_pickedConstraint);
delete m_pickedConstraint;
m_pickedConstraint = 0;
m_pickedBody = 0;
}
if (m_pickingMultiBodyPoint2Point)
{
m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_prevCanSleep);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point);
delete m_pickingMultiBodyPoint2Point;
m_pickingMultiBodyPoint2Point = 0;
}
}
btBoxShape* createBoxShape(const btVector3& halfExtents)
{
btBoxShape* box = new btBoxShape(halfExtents);
return box;
}
btRigidBody* createRigidBody(float mass, const btTransform& startTransform, btCollisionShape* shape, const btVector4& color = btVector4(1, 0, 0, 1))
{
btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
shape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
#define USE_MOTIONSTATE 1
#ifdef USE_MOTIONSTATE
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo cInfo(mass, myMotionState, shape, localInertia);
btRigidBody* body = new btRigidBody(cInfo);
//body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
#else
btRigidBody* body = new btRigidBody(mass, 0, shape, localInertia);
body->setWorldTransform(startTransform);
#endif//
body->setUserIndex(-1);
m_dynamicsWorld->addRigidBody(body);
return body;
}
};
#endif //COMMON_MULTI_BODY_SETUP_H

View File

@@ -0,0 +1,47 @@
#ifndef PARAM_INTERFACE_H
#define PARAM_INTERFACE_H
#pragma once
typedef void (*SliderParamChangedCallback) (float newVal);
#include "LinearMath/btScalar.h"
struct SliderParams
{
const char* m_name;
float m_minVal;
float m_maxVal;
SliderParamChangedCallback m_callback;
btScalar* m_paramValuePointer;
void* m_userPointer;
bool m_clampToNotches;
bool m_showValues;
SliderParams(const char* name, btScalar* targetValuePointer)
:m_name(name),
m_minVal(-100),
m_maxVal(100),
m_callback(0),
m_paramValuePointer(targetValuePointer),
m_userPointer(0),
m_clampToNotches(false),
m_showValues(true)
{
}
};
struct CommonParameterInterface
{
virtual ~CommonParameterInterface() {}
virtual void registerSliderFloatParameter(SliderParams& params)=0;
virtual void syncParameters()=0;
virtual void removeAllParameters()=0;
virtual void setSliderValue(int sliderIndex, double sliderValue)=0;
};
#endif //PARAM_INTERFACE_H

View File

@@ -0,0 +1,111 @@
#ifndef COMMON_RENDER_INTERFACE_H
#define COMMON_RENDER_INTERFACE_H
enum
{
B3_GL_TRIANGLES = 1,
B3_GL_POINTS
};
enum
{
B3_DEFAULT_RENDERMODE=1,
//B3_WIREFRAME_RENDERMODE,
B3_CREATE_SHADOWMAP_RENDERMODE,
B3_USE_SHADOWMAP_RENDERMODE,
};
struct CommonRenderInterface
{
virtual void init()=0;
virtual void updateCamera(int upAxis)=0;
virtual void removeAllInstances() = 0;
virtual void setCameraDistance(float dist) = 0;
virtual void setCameraPitch(float pitch) = 0;
virtual void setCameraTargetPosition(float x, float y, float z)=0;
virtual void getCameraPosition(float cameraPos[4])=0;
virtual void getCameraPosition(double cameraPos[4])=0;
virtual void setCameraTargetPosition(float cameraPos[4])=0;
virtual void getCameraTargetPosition(float cameraPos[4]) const=0;
virtual void getCameraTargetPosition(double cameraPos[4]) const=0;
virtual void getCameraViewMatrix(float viewMat[16]) const=0;
virtual void getCameraProjectionMatrix(float projMat[16]) const=0;
virtual void renderScene()=0;
virtual int getScreenWidth() = 0;
virtual int getScreenHeight() = 0;
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)=0;
virtual int registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling)=0;
virtual void drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float pointDrawSize)=0;
virtual void drawLine(const float from[4], const float to[4], const float color[4], float lineWidth) = 0;
virtual void drawLine(const double from[4], const double to[4], const double color[4], double lineWidth) = 0;
virtual void drawPoint(const float* position, const float color[4], float pointDrawSize)=0;
virtual void drawPoint(const double* position, const double color[4], double pointDrawSize)=0;
virtual int registerShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType=B3_GL_TRIANGLES, int textureIndex=-1)=0;
virtual void updateShape(int shapeIndex, const float* vertices)=0;
virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)=0;
virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)=0;
virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex)=0;
virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex)=0;
virtual void writeTransforms()=0;
virtual void enableBlend(bool blend)=0;
};
template <typename T>
inline int projectWorldCoordToScreen(T objx, T objy, T objz,
const T modelMatrix[16],
const T projMatrix[16],
const int viewport[4],
T *winx, T *winy, T *winz)
{
int i;
T in2[4];
T tmp[4];
in2[0]=objx;
in2[1]=objy;
in2[2]=objz;
in2[3]=T(1.0);
for (i=0; i<4; i++)
{
tmp[i] = in2[0] * modelMatrix[0*4+i] + in2[1] * modelMatrix[1*4+i] +
in2[2] * modelMatrix[2*4+i] + in2[3] * modelMatrix[3*4+i];
}
T out[4];
for (i=0; i<4; i++)
{
out[i] = tmp[0] * projMatrix[0*4+i] + tmp[1] * projMatrix[1*4+i] + tmp[2] * projMatrix[2*4+i] + tmp[3] * projMatrix[3*4+i];
}
if (out[3] == T(0.0))
return 0;
out[0] /= out[3];
out[1] /= out[3];
out[2] /= out[3];
/* Map x, y and z to range 0-1 */
out[0] = out[0] * T(0.5) + T(0.5);
out[1] = out[1] * T(0.5) + T(0.5);
out[2] = out[2] * T(0.5) + T(0.5);
/* Map x,y to viewport */
out[0] = out[0] * viewport[2] + viewport[0];
out[1] = out[1] * viewport[3] + viewport[1];
*winx=out[0];
*winy=out[1];
*winz=out[2];
return 1;
}
#endif//COMMON_RENDER_INTERFACE_H

View File

@@ -0,0 +1,407 @@
#ifndef COMMON_RIGID_BODY_BASE_H
#define COMMON_RIGID_BODY_BASE_H
#include "btBulletDynamicsCommon.h"
#include "ExampleInterface.h"
#include "CommonGUIHelperInterface.h"
#include "CommonRenderInterface.h"
#include "CommonGraphicsAppInterface.h"
#include "CommonWindowInterface.h"
struct CommonRigidBodyBase : public ExampleInterface
{
//keep the collision shapes, for deletion/cleanup
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
btBroadphaseInterface* m_broadphase;
btCollisionDispatcher* m_dispatcher;
btConstraintSolver* m_solver;
btDefaultCollisionConfiguration* m_collisionConfiguration;
btDiscreteDynamicsWorld* m_dynamicsWorld;
//data for picking objects
class btRigidBody* m_pickedBody;
class btTypedConstraint* m_pickedConstraint;
btVector3 m_oldPickingPos;
btVector3 m_hitPos;
btScalar m_oldPickingDist;
struct GUIHelperInterface* m_guiHelper;
CommonRigidBodyBase(struct GUIHelperInterface* helper)
:m_broadphase(0),
m_dispatcher(0),
m_solver(0),
m_collisionConfiguration(0),
m_dynamicsWorld(0),
m_pickedBody(0),
m_pickedConstraint(0),
m_guiHelper(helper)
{
}
virtual ~CommonRigidBodyBase()
{
}
virtual void createEmptyDynamicsWorld()
{
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
//m_collisionConfiguration->setConvexConvexMultipointIterations();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
m_solver = sol;
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
}
virtual void stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
{
m_dynamicsWorld->stepSimulation(deltaTime);
}
}
virtual void physicsDebugDraw(int debugFlags)
{
if (m_dynamicsWorld && m_dynamicsWorld->getDebugDrawer())
{
m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugFlags);
m_dynamicsWorld->debugDrawWorld();
}
}
virtual void exitPhysics()
{
removePickingConstraint();
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
if (m_dynamicsWorld)
{
int i;
for (i = m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
{
m_dynamicsWorld->removeConstraint(m_dynamicsWorld->getConstraint(i));
}
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
}
//delete collision shapes
for (int j = 0; j<m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
m_dynamicsWorld=0;
delete m_solver;
m_solver=0;
delete m_broadphase;
m_broadphase=0;
delete m_dispatcher;
m_dispatcher=0;
delete m_collisionConfiguration;
m_collisionConfiguration=0;
}
virtual void debugDraw(int debugDrawFlags)
{
if (m_dynamicsWorld)
{
if (m_dynamicsWorld->getDebugDrawer())
{
m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
}
m_dynamicsWorld->debugDrawWorld();
}
}
virtual bool keyboardCallback(int key, int state)
{
return false;//don't handle this key
}
btVector3 getRayTo(int x,int y)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (!renderer)
{
btAssert(0);
return btVector3(0,0,0);
}
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = btScalar(2.0) * btAtan(tanFov);
btVector3 camPos,camTarget;
renderer->getCameraPosition(camPos);
renderer->getCameraTargetPosition(camTarget);
btVector3 rayFrom = camPos;
btVector3 rayForward = (camTarget-camPos);
rayForward.normalize();
float farPlane = 10000.f;
rayForward*= farPlane;
btVector3 rightOffset;
btVector3 cameraUp=btVector3(0,0,0);
cameraUp[m_guiHelper->getAppInterface()->getUpAxis()]=1;
btVector3 vertical = cameraUp;
btVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
btScalar aspect;
float width = float(renderer->getScreenWidth());
float height = float (renderer->getScreenHeight());
aspect = width / height;
hor*=aspect;
btVector3 rayToCenter = rayFrom + rayForward;
btVector3 dHor = hor * 1.f/width;
btVector3 dVert = vertical * 1.f/height;
btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += btScalar(x) * dHor;
rayTo -= btScalar(y) * dVert;
return rayTo;
}
virtual bool mouseMoveCallback(float x,float y)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (!renderer)
{
btAssert(0);
return false;
}
btVector3 rayTo = getRayTo(int(x), int(y));
btVector3 rayFrom;
renderer->getCameraPosition(rayFrom);
movePickedBody(rayFrom,rayTo);
return false;
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (!renderer)
{
btAssert(0);
return false;
}
b3gWindowInterface* window = m_guiHelper->getAppInterface()->m_window;
if (state==1)
{
if(button==0 && (!window->isModifiedKeyPressed(B3G_ALT) && !window->isModifiedKeyPressed(B3G_CONTROL) ))
{
btVector3 camPos;
renderer->getCameraPosition(camPos);
btVector3 rayFrom = camPos;
btVector3 rayTo = getRayTo(int(x),int(y));
pickBody(rayFrom, rayTo);
}
} else
{
if (button==0)
{
removePickingConstraint();
//remove p2p
}
}
//printf("button=%d, state=%d\n",button,state);
return false;
}
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
if (m_dynamicsWorld==0)
return false;
btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld);
m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback);
if (rayCallback.hasHit())
{
btVector3 pickPos = rayCallback.m_hitPointWorld;
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
if (body)
{
//other exclusions?
if (!(body->isStaticObject() || body->isKinematicObject()))
{
m_pickedBody = body;
m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot);
m_dynamicsWorld->addConstraint(p2p, true);
m_pickedConstraint = p2p;
btScalar mousePickClamping = 30.f;
p2p->m_setting.m_impulseClamp = mousePickClamping;
//very weak constraint for picking
p2p->m_setting.m_tau = 0.001f;
}
}
// pickObject(pickPos, rayCallback.m_collisionObject);
m_oldPickingPos = rayToWorld;
m_hitPos = pickPos;
m_oldPickingDist = (pickPos - rayFromWorld).length();
// printf("hit !\n");
//add p2p
}
return false;
}
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
if (m_pickedBody && m_pickedConstraint)
{
btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_pickedConstraint);
if (pickCon)
{
//keep it at the same picking distance
btVector3 newPivotB;
btVector3 dir = rayToWorld - rayFromWorld;
dir.normalize();
dir *= m_oldPickingDist;
newPivotB = rayFromWorld + dir;
pickCon->setPivotB(newPivotB);
return true;
}
}
return false;
}
virtual void removePickingConstraint()
{
if (m_pickedConstraint)
{
m_dynamicsWorld->removeConstraint(m_pickedConstraint);
delete m_pickedConstraint;
m_pickedConstraint = 0;
m_pickedBody = 0;
}
}
btBoxShape* createBoxShape(const btVector3& halfExtents)
{
btBoxShape* box = new btBoxShape(halfExtents);
return box;
}
btRigidBody* createRigidBody(float mass, const btTransform& startTransform, btCollisionShape* shape, const btVector4& color = btVector4(1, 0, 0, 1))
{
btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
shape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
#define USE_MOTIONSTATE 1
#ifdef USE_MOTIONSTATE
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo cInfo(mass, myMotionState, shape, localInertia);
btRigidBody* body = new btRigidBody(cInfo);
//body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
#else
btRigidBody* body = new btRigidBody(mass, 0, shape, localInertia);
body->setWorldTransform(startTransform);
#endif//
body->setUserIndex(-1);
m_dynamicsWorld->addRigidBody(body);
return body;
}
virtual void renderScene()
{
m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
if (m_guiHelper->getRenderInterface())
{
m_guiHelper->getRenderInterface()->renderScene();
}
}
};
#endif //COMMON_RIGID_BODY_SETUP_H

View File

@@ -0,0 +1,126 @@
#ifndef B3G_WINDOW_INTERFACE_H
#define B3G_WINDOW_INTERFACE_H
typedef void (*b3WheelCallback)(float deltax, float deltay);
typedef void (*b3ResizeCallback)( float width, float height);
typedef void (*b3MouseMoveCallback)( float x, float y);
typedef void (*b3MouseButtonCallback)(int button, int state, float x, float y);
typedef void (*b3KeyboardCallback)(int keycode, int state);
typedef void (*b3RenderCallback) ();
enum {
B3G_ESCAPE = 27,
B3G_F1 = 0xff00,
B3G_F2,
B3G_F3,
B3G_F4,
B3G_F5,
B3G_F6,
B3G_F7,
B3G_F8,
B3G_F9,
B3G_F10,
B3G_F11,
B3G_F12,
B3G_F13,
B3G_F14,
B3G_F15,
B3G_LEFT_ARROW,
B3G_RIGHT_ARROW,
B3G_UP_ARROW,
B3G_DOWN_ARROW,
B3G_PAGE_UP,
B3G_PAGE_DOWN,
B3G_END,
B3G_HOME,
B3G_INSERT,
B3G_DELETE,
B3G_BACKSPACE,
B3G_SHIFT,
B3G_CONTROL,
B3G_ALT,
B3G_RETURN
};
struct b3gWindowConstructionInfo
{
int m_width;
int m_height;
bool m_fullscreen;
int m_colorBitsPerPixel;
void* m_windowHandle;
const char* m_title;
int m_openglVersion;
b3gWindowConstructionInfo(int width=1024, int height=768)
:m_width(width),
m_height(height),
m_fullscreen(false),
m_colorBitsPerPixel(32),
m_windowHandle(0),
m_title("title"),
m_openglVersion(3)
{
}
};
class b3gWindowInterface
{
public:
virtual ~b3gWindowInterface()
{
}
virtual void createDefaultWindow(int width, int height, const char* title)
{
b3gWindowConstructionInfo ci(width,height);
ci.m_title = title;
createWindow(ci);
}
virtual void createWindow(const b3gWindowConstructionInfo& ci)=0;
virtual void closeWindow()=0;
virtual void runMainLoop()=0;
virtual float getTimeInSeconds()=0;
virtual bool requestedExit() const = 0;
virtual void setRequestExit() = 0;
virtual void startRendering()=0;
virtual void endRendering()=0;
virtual bool isModifiedKeyPressed(int key) = 0;
virtual void setMouseMoveCallback(b3MouseMoveCallback mouseCallback)=0;
virtual b3MouseMoveCallback getMouseMoveCallback()=0;
virtual void setMouseButtonCallback(b3MouseButtonCallback mouseCallback)=0;
virtual b3MouseButtonCallback getMouseButtonCallback()=0;
virtual void setResizeCallback(b3ResizeCallback resizeCallback)=0;
virtual b3ResizeCallback getResizeCallback()=0;
virtual void setWheelCallback(b3WheelCallback wheelCallback)=0;
virtual b3WheelCallback getWheelCallback()=0;
virtual void setKeyboardCallback( b3KeyboardCallback keyboardCallback)=0;
virtual b3KeyboardCallback getKeyboardCallback()=0;
virtual void setRenderCallback( b3RenderCallback renderCallback) = 0;
virtual void setWindowTitle(const char* title)=0;
virtual float getRetinaScale() const =0;
virtual int fileOpenDialog(char* fileName, int maxFileNameLength) = 0;
};
#endif //B3G_WINDOW_INTERFACE_H

View File

@@ -0,0 +1,28 @@
#ifndef EXAMPLE_INTERFACE_H
#define EXAMPLE_INTERFACE_H
class ExampleInterface
{
public:
typedef class ExampleInterface* (CreateFunc)(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option);
virtual ~ExampleInterface()
{
}
virtual void initPhysics()=0;
virtual void exitPhysics()=0;
virtual void stepSimulation(float deltaTime)=0;
virtual void renderScene()=0;
virtual void physicsDebugDraw(int debugFlags)=0;//for now we reuse the flags in Bullet/src/LinearMath/btIDebugDraw.h
virtual bool mouseMoveCallback(float x,float y)=0;
virtual bool mouseButtonCallback(int button, int state, float x, float y)=0;
virtual bool keyboardCallback(int key, int state)=0;
};
#endif //EXAMPLE_INTERFACE_H