void MultiBodyVehicleSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
int upAxis = 1;
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
gfxBridge.setUpAxis(upAxis);
this->createEmptyDynamicsWorld();
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
createMultiBodyVehicle();
if (1)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
gfxBridge.createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-1.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
gfxBridge.createRigidBodyGraphicsObject(body,color);
}
}
void PhysicsMgr::start()
{
createEmptyDynamicsWorld();
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -50, 0));
{
btScalar mass(0.);
createRigidBodyInternal(mass, groundTransform, groundShape, btVector4(0, 0, 1, 1));
}
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btBoxShape* colShape = createBoxShape(btVector3(.1, .1, .1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//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)
colShape->calculateLocalInertia(mass, localInertia);
//int index = 0;
//for (int k = 0; k < ARRAY_SIZE_Y; k++)
//{
// for (int i = 0; i < ARRAY_SIZE_X; i++)
// {
// for (int j = 0; j < ARRAY_SIZE_Z; j++)
// {
// startTransform.setOrigin(btVector3(
// btScalar(0.2*i),
// btScalar(2 + .2*k),
// btScalar(0.2*j)));
// auto body = createRigidBody(mass, startTransform, colShape, btVector4(1, 0, 0, 1));
// body->setUserIndex(index);
// index++;
// }
// }
//}
}
}
PhysicsRigidBody* PhysicsMgr::createRigidBodyMesh(Mesh* mesh, Matrix44* transform)
{
auto rig = new PhysicsRigidBody();
btScalar mass(0.0f);
btTransform startTransform;
btTriangleMesh * meshInterface = new btTriangleMesh();
auto idxSize = mesh->getIndicesSize();
for(auto i =0; i < idxSize; i+=3)
{
auto v1 = mesh->getVertex(i);
auto v2 = mesh->getVertex(i + 1);
auto v3 = mesh->getVertex(i + 2);
meshInterface->addTriangle(btVector3(v1.m_pos.x, v1.m_pos.y, v1.m_pos.z), btVector3(v2.m_pos.x, v2.m_pos.y, v2.m_pos.z), btVector3(v3.m_pos.x, v3.m_pos.y, v3.m_pos.z));
}
auto tetraShape = new btBvhTriangleMeshShape(meshInterface, true, true);
startTransform.setIdentity();
if (transform)
{
startTransform.setFromOpenGLMatrix(transform->data());
}
auto btRig = shared()->createRigidBodyInternal(mass, startTransform, tetraShape, btVector4(1, 0, 0, 1));
btRig->setContactProcessingThreshold(BT_LARGE_FLOAT);
btRig->setFriction (btScalar(0.9));
btRig->setCcdMotionThreshold(.1);
btRig->setCcdSweptSphereRadius(0);
rig->setRigidBody(btRig);
rig->genUserIndex();
btRig->setUserIndex(rig->userIndex());
btRig->setUserPointer(rig);
return rig;
}
void MultipleBoxesExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
if (m_dynamicsWorld->getDebugDrawer())
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
}
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//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)
colShape->calculateLocalInertia(mass,localInertia);
for(int i=0;i<TOTAL_BOXES;++i) {
startTransform.setOrigin(btVector3(
btScalar(0),
btScalar(20+i*2),
btScalar(0)));
createRigidBody(mass,startTransform,colShape);
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void markNodesToFold() {
if (!pickedNode || !pickedNode2) return;
vector<btVector3> pts; vector<btVector4> colors;
btSoftBody *psb = cloth->psb();
btVector3 line = pickedNode2->m_x - pickedNode->m_x; line.setZ(0); line.normalize();
for (int i = 0; i < psb->m_nodes.size(); ++i) {
const btVector3 &x = psb->m_nodes[i].m_x;
btVector3 ptline = x - pickedNode->m_x; ptline.setZ(0);
if (line.cross(ptline).z() <= 0) continue;
pts.push_back(x); colors.push_back(btVector4(0, 0, 1, 1));
}
tofoldplot->setPoints(pts, colors);
}
PhysicsRigidBody* PhysicsMgr::createRigidBodyFromCompund(float mass, Matrix44* transform, PhysicsCompoundShape * tzwShape)
{
auto rig = new PhysicsRigidBody();
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
auto shape = tzwShape->getShape();
if (isDynamic)
shape->calculateLocalInertia(mass, localInertia);
btTransform startTransform;
startTransform.setFromOpenGLMatrix(transform->data());
auto btRig = shared()->createRigidBodyInternal(mass, startTransform, shape, btVector4(1, 0, 0, 1));
rig->setRigidBody(btRig);
rig->genUserIndex();
btRig->setUserIndex(rig->userIndex());
btRig->setUserPointer(rig);
return rig;
}
renderTexture::renderTexture(int width, int height)
: m_height(height), m_width(width)
{
m_buffer = new unsigned char[m_width * m_height * 4];
//clear screen
memset(m_buffer, 0, m_width * m_height * 4);
//clear screen version 2
for (int x = 0; x < m_width; x++)
{
for (int y = 0; y < m_height; y++)
{
setPixel(x, y, btVector4(float(x), float(y), 0.f, 1.f));
}
}
}
PhysicsRigidBody * PhysicsMgr::createRigidBodyCylinder(float massValue, float topRadius, float bottomRadius, float height, Matrix44 transform)
{
auto rig = new PhysicsRigidBody();
btScalar mass(massValue);
btTransform startTransform;
startTransform.setIdentity();
bool isDynamic = (mass != 0.f);
btCylinderShape* colShape = new btCylinderShapeZ(btVector3(topRadius, bottomRadius, height * 0.5));
btVector3 localInertia(0, 0, 0);
if (isDynamic)
{
colShape->calculateLocalInertia(mass, localInertia);
}
startTransform.setFromOpenGLMatrix(transform.data());
auto btRig = shared()->createRigidBodyInternal(mass, startTransform, colShape, btVector4(1, 0, 0, 1));
rig->setRigidBody(btRig);
rig->genUserIndex();
btRig->setUserIndex(rig->userIndex());
btRig->setUserPointer(rig);
return rig;
}
void markFolds() {
cloth->updateAccel();
foldnodes.clear();
calcFoldNodes(*cloth, foldnodes);
vector<btVector3> pts;
vector<btVector4> colors;
for (int i = 0; i < foldnodes.size(); ++i) {
pts.push_back(cloth->psb()->m_nodes[foldnodes[i]].m_x);
colors.push_back(btVector4(0, 0, 0, 1));
}
foldnodeplot->setPoints(pts, colors);
pts.clear();
for (int i = 0; i < foldnodes.size(); ++i) {
btVector3 dir = calcFoldLineDir(*cloth, foldnodes[i], foldnodes);
btVector3 c = cloth->psb()->m_nodes[foldnodes[i]].m_x;
btScalar l = 0.01*METERS;
pts.push_back(c - l*dir); pts.push_back(c + l*dir);
}
folddirplot->setPoints(pts);
}
PhysicsRigidBody* PhysicsMgr::createRigidBody(float massValue, Matrix44& transform, AABB& aabb)
{
auto rig = new PhysicsRigidBody();
btScalar mass(massValue);
btTransform startTransform;
startTransform.setIdentity();
bool isDynamic = (mass != 0.f);
vec3 half = aabb.half();
btBoxShape* colShape = shared()->createBoxShape(btVector3(half.x, half.y, half.z));
btVector3 localInertia(0, 0, 0);
if (isDynamic)
{
colShape->calculateLocalInertia(mass, localInertia);
}
startTransform.setFromOpenGLMatrix(transform.data());
auto btRig = shared()->createRigidBodyInternal(mass, startTransform, colShape, btVector4(1, 0, 0, 1));
rig->setRigidBody(btRig);
rig->genUserIndex();
btRig->setUserIndex(rig->userIndex());
btRig->setUserPointer(rig);
return rig;
}
void TestJointTorqueSetup::initPhysics()
{
int upAxis = 1;
gJointFeedbackInWorldSpace = true;
gJointFeedbackInJointFrame = true;
m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(1,1,0.2);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6;
start.setOrigin(groundOrigin);
btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
body->setFriction(0);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
{
bool floating = false;
bool damping = false;
bool gyro = false;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if(baseMass)
{
//btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape->calculateLocalInertia(baseMass, baseInertiaDiag);
delete shape;
}
bool isMultiDof = true;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
// baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI/ 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for(int i = 0; i < numLinks; ++i)
{
float linkMass = 1.f;
//if (i==3 || i==2)
// linkMass= 1000;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* shape = 0;
if (i==0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
} else
{
shape = new btSphereShape(radius);
}
shape->calculateLocalInertia(linkMass, linkInertiaDiag);
delete shape;
if(!spherical)
{
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (i==0)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
} else
{
btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
}
else
{
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
pMultiBody->finalizeMultiDof();
//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
for (int i=0;i<pMultiBody->getNumLinks();i++)
{
btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
pMultiBody->getLink(i).m_jointFeedback = fb;
m_jointFeedbacks.push_back(fb);
//break;
}
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
///
world->addMultiBody(pMultiBody);
btMultiBody* mbC = pMultiBody;
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if(!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}else
{ mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
//////////////////////////////////////////////
if(0)//numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
// double friction = 1;
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
// btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
if (1)
{
btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.setOrigin(local_origin[0]);
btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538);
tr.setRotation(orn);
col->setWorldTransform(tr);
bool isDynamic = (baseMass > 0 && floating);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
btVector3 color(0.0,0.0,0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
// col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* shape =0;
if (i==0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
} else
{
shape = new btSphereShape(radius);
}
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
// col->setFriction(friction);
bool isDynamic = 1;//(linkMass > 0);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
//if (i==0||i>numLinks-2)
{
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
pMultiBody->getLink(i).m_collider=col;
}
}
}
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
b3ResourcePath p;
char resourcePath[1024];
if (p.findResourcePath("multibody.bullet",resourcePath,1024))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
fclose(f);
}
}
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#include "../ExampleBrowser/CollisionShape2TriangleMesh.h"
#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btHashMap.h"
#include "../TinyRenderer/TinyRenderer.h"
#include "../OpenGLWindow/SimpleCamera.h"
static btVector4 sMyColors[4] =
{
btVector4(0.3,0.3,1,1),
btVector4(0.6,0.6,1,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
//btVector4(1,1,0,1),
};
struct TinyRendererTexture
{
const unsigned char* m_texels;
int m_width;
int m_height;
};
struct TinyRendererGUIHelper : public GUIHelperInterface
{
}
}
ImportSDFSetup::~ImportSDFSetup()
{
for (int i=0;i<m_nameMemory.size();i++)
{
delete m_nameMemory[i];
}
m_nameMemory.clear();
delete m_data;
}
static btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
static btVector3 selectColor()
{
static int curColor = 0;
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
return color;
}
void BasicDemo::initPhysics()
{
// Bullet2RigidBodyDemo::initPhysics();
m_config = new btDefaultCollisionConfiguration;
m_dispatcher = new btCollisionDispatcher(m_config);
m_bp = new btDbvtBroadphase();
m_solver = new btNNCGConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_bp,m_solver,m_config);
int curColor=0;
//create ground
int cubeShapeId = m_glApp->registerCubeShape();
float pos[]={0,0,0};
float orn[]={0,0,0,1};
createGround(cubeShapeId);
{
float halfExtents[]={scaling,scaling,scaling,1};
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
btTransform startTransform;
startTransform.setIdentity();
btScalar mass = 1.f;
btVector3 localInertia;
btBoxShape* colShape = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2]));
colShape ->calculateLocalInertia(mass,localInertia);
for (int k=0;k<ARRAY_SIZE_Y;k++)
{
for (int i=0;i<ARRAY_SIZE_X;i++)
{
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
startTransform.setOrigin(btVector3(
btScalar(2.0*scaling*i),
btScalar(2.*scaling+2.0*scaling*k),
btScalar(2.0*scaling*j)));
m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body);
}
}
}
}
m_glApp->m_instancingRenderer->writeTransforms();
}
void SimpleJointExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
if (m_dynamicsWorld->getDebugDrawer())
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
}
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//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)
colShape->calculateLocalInertia(mass,localInertia);
startTransform.setOrigin(btVector3(
btScalar(0),
btScalar(10),
btScalar(0)));
btRigidBody* dynamicBox = createRigidBody(mass,startTransform,colShape);
//create a static rigid body
mass = 0;
startTransform.setOrigin(btVector3(
btScalar(0),
btScalar(20),
btScalar(0)));
btRigidBody* staticBox = createRigidBody(mass,startTransform,colShape);
//create a simple p2pjoint constraint
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*dynamicBox, *staticBox, btVector3(0,3,0), btVector3(0,0,0));
p2p->m_setting.m_damping = 0.0625;
p2p->m_setting.m_impulseClamp = 0.95;
m_dynamicsWorld->addConstraint(p2p);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
#include "CollisionSdkC_Api.h"
#include "LinearMath/btQuickprof.h"
///Not Invented Here link reminder http://www.joelonsoftware.com/articles/fog0000000007.html
///todo: use the 'userData' to prevent this use of global variables
static int gTotalPoints = 0;
const int sPointCapacity = 10000;
const int sNumCompounds = 10;
const int sNumSpheres = 10;
lwContactPoint pointsOut[sPointCapacity];
int numNearCallbacks = 0;
static btVector4 sColors[4] =
{
btVector4(1,0.7,0.7,1),
btVector4(1,1,0.7,1),
btVector4(0.7,1,0.7,1),
btVector4(0.7,1,1,1),
};
void myNearCallback(plCollisionSdkHandle sdkHandle, plCollisionWorldHandle worldHandle, void* userData, plCollisionObjectHandle objA, plCollisionObjectHandle objB)
{
numNearCallbacks++;
int remainingCapacity = sPointCapacity-gTotalPoints;
btAssert(remainingCapacity>0);
if (remainingCapacity>0)
{
lwContactPoint* pointPtr = &pointsOut[gTotalPoints];
int numNewPoints = plCollide(sdkHandle, worldHandle, objA,objB,pointPtr,remainingCapacity);
void MultiBodySoftContact::initPhysics()
{
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
this->createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(50,50,50);
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,-50.5);
start.setOrigin(groundOrigin);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
//setContactStiffnessAndDamping will enable compliant rigid body contact
body->setContactStiffnessAndDamping(300,10);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
{
btCollisionShape* childShape = new btSphereShape(btScalar(0.5));
m_guiHelper->createCollisionShapeGraphicsObject(childShape);
btScalar mass = 1;
btVector3 baseInertiaDiag;
bool isFixed = (mass == 0);
childShape->calculateLocalInertia(mass,baseInertiaDiag);
btMultiBody *pMultiBody = new btMultiBody(0, 1, baseInertiaDiag, false, false);
btTransform startTrans;
startTrans.setIdentity();
startTrans.setOrigin(btVector3(0,0,3));
pMultiBody->setBaseWorldTransform(startTrans);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(childShape);
pMultiBody->setBaseCollider(col);
bool isDynamic = (mass > 0 && !isFixed);
int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
m_dynamicsWorld->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
pMultiBody->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(pMultiBody);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
pMultiBody->forwardKinematics(scratch_q,scratch_m);
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
pMultiBody->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void BasicDemoPhysicsSetup::initPhysics()
{
///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));
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
//groundShape->initializePolyhedralFeatures();
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
}
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//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)
colShape->calculateLocalInertia(mass,localInertia);
for (int k=0;k<ARRAY_SIZE_Y;k++)
{
for (int i=0;i<ARRAY_SIZE_X;i++)
{
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(btVector3(
btScalar(2.0*i),
btScalar(20+2.0*k),
btScalar(2.0*j)));
createRigidBody(mass,startTransform,colShape);
}
}
}
}
}
void DefracDemo::mouseFunc(int button, int state, int x, int y)
{
if(button == 0)//left button
{
if(state == 0) //pressed
{
btVector3 rayTo = getRayTo(x,y);
btVector3 rayFrom;
if (m_ortho)
{
rayFrom = rayTo;
rayFrom.setZ(-100.f);
} else
{
rayFrom = m_cameraPosition;
}
btDefracDynamicsWorld* world = getDefracDynamicsWorld();
btScalar minTime(BT_LARGE_FLOAT);
for(int b=0; b < world->getNumDefracBodies(); ++b)
{
btDefracBody* body = world->getDefracBody(b);
for(int i=0; i < body->getTetrahedronCount(); ++i)
{
btTetrahedron* t = body->getTetrahedron(i);
btVector3 min, max;
t->getAABB(min, max);
btScalar time(0);
if(btDefracUtils::SegmentAABBIntersect(rayFrom, rayTo, min, max, &time)
&& time < minTime)
{
minTime = time;
btVector3 pickPos = rayFrom + (rayTo-rayFrom)*time;
m_oldPickingDist = (pickPos - rayFrom).length();
if(m_spring == NULL)
m_spring = new btSpring(t, btVector4(0.25,0.25,0.25,0.25), 1000, 0.2);
else
m_spring->setTetrahedron(t);
m_spring->setSourcePosition(pickPos);
}
}
}
if(m_spring != NULL)
world->addSpring(m_spring);
}
else if(state == 1) //released
{
btDefracDynamicsWorld* world = getDefracDynamicsWorld();
world->removeSpring(m_spring);
delete m_spring;
m_spring = NULL;
}
}
if(m_spring == NULL)
{
DemoApplication::mouseFunc(button,state,x,y);
}
}
btVector4 toVec4(const glm::vec4& v)
{
return btVector4(v.x, v.y, v.z, v.w);
}
void RigidBodySoftContact::initPhysics()
{
m_guiHelper->setUpAxis(1);
//createEmptyDynamicsWorld();
{
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
//m_collisionConfiguration->setConvexConvexMultipointIterations();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
//btMLCPSolver* sol = new btMLCPSolver(new btSolveProjectedGaussSeidel());
m_solver = sol;
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
}
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
if (m_dynamicsWorld->getDebugDrawer())
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
m_dynamicsWorld->getSolverInfo().m_erp2 = 0.f;
m_dynamicsWorld->getSolverInfo().m_globalCfm = 0.f;
m_dynamicsWorld->getSolverInfo().m_numIterations = 3;
m_dynamicsWorld->getSolverInfo().m_solverMode = SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
m_dynamicsWorld->getSolverInfo().m_splitImpulse = false;
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
//groundShape->initializePolyhedralFeatures();
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
body->setContactStiffnessAndDamping(300,10);
}
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
//btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
btCollisionShape* childShape = new btSphereShape(btScalar(0.5));
btCompoundShape* colShape = new btCompoundShape();
colShape->addChildShape(btTransform::getIdentity(),childShape);
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
startTransform.setRotation(btQuaternion(btVector3(1,1,1),SIMD_PI/10.));
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
for (int k=0;k<ARRAY_SIZE_Y;k++)
{
for (int i=0;i<ARRAY_SIZE_X;i++)
{
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(btVector3(
btScalar(2.0*i+0.1),
btScalar(3+2.0*k),
btScalar(2.0*j)));
btRigidBody* body = createRigidBody(mass,startTransform,colShape);
//body->setAngularVelocity(btVector3(1,1,1));
}
}
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void RaytestDemo::castRays()
{
static float up = 0.f;
static float dir = 1.f;
//add some simple animation
if (!m_idle)
{
up+=0.01*dir;
if (btFabs(up)>2)
{
dir*=-1.f;
}
btTransform tr = m_dynamicsWorld->getCollisionObjectArray()[1]->getWorldTransform();
static float angle = 0.f;
angle+=0.01f;
tr.setRotation(btQuaternion(btVector3(0,1,0),angle));
m_dynamicsWorld->getCollisionObjectArray()[1]->setWorldTransform(tr);
}
///step the simulation
if (m_dynamicsWorld)
{
m_dynamicsWorld->updateAabbs();
m_dynamicsWorld->computeOverlappingPairs();
btVector3 red(1,0,0);
btVector3 blue(0,0,1);
///all hits
{
btVector3 from(-30,1+up,0);
btVector3 to(30,1,0);
sDebugDraw.drawLine(from,to,btVector4(0,0,0,1));
btCollisionWorld::AllHitsRayResultCallback allResults(from,to);
allResults.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;
//kF_UseGjkConvexRaytest flag is now enabled by default, use the faster but more approximate algorithm
allResults.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest;
m_dynamicsWorld->rayTest(from,to,allResults);
for (int i=0;i<allResults.m_hitFractions.size();i++)
{
btVector3 p = from.lerp(to,allResults.m_hitFractions[i]);
sDebugDraw.drawSphere(p,0.1,red);
}
}
///first hit
{
btVector3 from(-30,1.2,0);
btVector3 to(30,1.2,0);
sDebugDraw.drawLine(from,to,btVector4(0,0,1,1));
btCollisionWorld::ClosestRayResultCallback closestResults(from,to);
closestResults.m_flags |= btTriangleRaycastCallback::kF_FilterBackfaces;
m_dynamicsWorld->rayTest(from,to,closestResults);
if (closestResults.hasHit())
{
btVector3 p = from.lerp(to,closestResults.m_closestHitFraction);
sDebugDraw.drawSphere(p,0.1,blue);
sDebugDraw.drawLine(p,p+closestResults.m_hitNormalWorld,blue);
}
}
}
}
void BridgeExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
if (m_dynamicsWorld->getDebugDrawer())
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
}
//create two fixed boxes to hold the planks
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btScalar plankWidth = 0.4;
btScalar plankHeight = 0.2;
btScalar plankBreadth = 1;
btScalar plankOffset = plankWidth; //distance between two planks
btScalar bridgeWidth = plankWidth*TOTAL_PLANKS + plankOffset*(TOTAL_PLANKS-1);
btScalar bridgeHeight = 5;
btScalar halfBridgeWidth = bridgeWidth*0.5f;
btBoxShape* colShape = createBoxShape(btVector3(plankWidth,plankHeight,plankBreadth));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//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)
colShape->calculateLocalInertia(mass,localInertia);
//create a set of boxes to represent bridge
btAlignedObjectArray<btRigidBody*> boxes;
int lastBoxIndex = TOTAL_PLANKS-1;
for(int i=0;i<TOTAL_PLANKS;++i) {
float t = float(i)/lastBoxIndex;
t = -(t*2-1.0f) * halfBridgeWidth;
startTransform.setOrigin(btVector3(
btScalar(t),
bridgeHeight,
btScalar(0)
)
);
boxes.push_back(createRigidBody((i==0 || i==lastBoxIndex)?0:mass,startTransform,colShape));
}
//add N-1 spring constraints
for(int i=0;i<TOTAL_PLANKS-1;++i) {
btRigidBody* b1 = boxes[i];
btRigidBody* b2 = boxes[i+1];
btPoint2PointConstraint* leftSpring = new btPoint2PointConstraint(*b1, *b2, btVector3(-0.5,0,-0.5), btVector3(0.5,0,-0.5));
m_dynamicsWorld->addConstraint(leftSpring);
btPoint2PointConstraint* rightSpring = new btPoint2PointConstraint(*b1, *b2, btVector3(-0.5,0,0.5), btVector3(0.5,0,0.5));
m_dynamicsWorld->addConstraint(rightSpring);
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void PhysicsServerExample::renderScene()
{
#if 0
///little VR test to follow/drive Husky vehicle
if (gHuskyId >= 0)
{
gVRTeleportPos1 = huskyTr.getOrigin();
gVRTeleportOrn = huskyTr.getRotation();
}
#endif
B3_PROFILE("PhysicsServerExample::RenderScene");
drawUserDebugLines();
if (gEnableRealTimeSimVR)
{
static int frameCount=0;
static btScalar prevTime = m_clock.getTimeSeconds();
frameCount++;
static btScalar worseFps = 1000000;
int numFrames = 200;
static int count = 0;
count++;
#if 0
if (0 == (count & 1))
{
btScalar curTime = m_clock.getTimeSeconds();
btScalar fps = 1. / (curTime - prevTime);
prevTime = curTime;
if (fps < worseFps)
{
worseFps = fps;
}
if (count > numFrames)
{
count = 0;
sprintf(line0, "fps:%f frame:%d", worseFps, frameCount / 2);
sprintf(line1, "drop:%d tscale:%f dt:%f, substep %f)", gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep);
gDroppedSimulationSteps = 0;
worseFps = 1000000;
}
}
#endif
#ifdef BT_ENABLE_VR
if ((gInternalSimFlags&2 ) && m_tinyVrGui==0)
{
ComboBoxParams comboParams;
comboParams.m_comboboxId = 0;
comboParams.m_numItems = 0;
comboParams.m_startItem = 0;
comboParams.m_callback = 0;//MyComboBoxCallback;
comboParams.m_userPointer = 0;//this;
m_tinyVrGui = new TinyVRGui(comboParams,this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface());
m_tinyVrGui->init();
}
if (m_tinyVrGui)
{
b3Transform tr;tr.setIdentity();
tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2]));
tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3]));
tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
b3Scalar dt = 0.01;
m_tinyVrGui->clearTextArea();
static char line0[1024];
static char line1[1024];
m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255);
m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255);
m_tinyVrGui->tick(dt,tr);
}
#endif//BT_ENABLE_VR
}
///debug rendering
//m_args[0].m_cs->lock();
//gVRTeleportPos[0] += 0.01;
btTransform tr2a, tr2;
tr2a.setIdentity();
tr2.setIdentity();
tr2.setOrigin(gVRTeleportPos1);
tr2a.setRotation(gVRTeleportOrn);
btTransform trTotal = tr2*tr2a;
btTransform trInv = trTotal.inverse();
btMatrix3x3 vrOffsetRot;
vrOffsetRot.setRotation(trInv.getRotation());
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
vrOffset[i + 4 * j] = vrOffsetRot[i][j];
}
}
vrOffset[12]= trInv.getOrigin()[0];
vrOffset[13]= trInv.getOrigin()[1];
vrOffset[14]= trInv.getOrigin()[2];
this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->
getActiveCamera()->setVRCameraOffsetTransform(vrOffset);
m_physicsServer.renderScene();
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{
if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i])
{
btVector3 from = m_args[0].m_vrControllerPos[i];
btMatrix3x3 mat(m_args[0].m_vrControllerOrn[i]);
btVector3 toX = from+mat.getColumn(0);
btVector3 toY = from+mat.getColumn(1);
btVector3 toZ = from+mat.getColumn(2);
int width = 2;
btVector4 color;
color=btVector4(1,0,0,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width);
color=btVector4(0,1,0,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
color=btVector4(0,0,1,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
}
}
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
if (!gEnableRealTimeSimVR)
{
gEnableRealTimeSimVR = true;
m_physicsServer.enableRealTimeSimulation(1);
}
}
//m_args[0].m_cs->unlock();
}
void Raytracer::displayCallback()
{
updateCamera();
for (int i=0;i<numObjects;i++)
{
transforms[i].setIdentity();
btVector3 pos(0.f,0.f,-(2.5* numObjects * 0.5)+i*2.5f);
transforms[i].setOrigin( pos );
btQuaternion orn;
if (i < 2)
{
orn.setEuler(yaw,pitch,roll);
transforms[i].setRotation(orn);
}
}
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
if (!m_initialized)
{
m_initialized = true;
glGenTextures(1, &glTextureId);
}
glBindTexture(GL_TEXTURE_2D,glTextureId );
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glDisable(GL_TEXTURE_2D);
glDisable(GL_BLEND);
btVector4 rgba(1.f,0.f,0.f,0.5f);
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = 2.0 * atanf (tanFov);
btVector3 rayFrom = getCameraPosition();
btVector3 rayForward = getCameraTargetPosition()-getCameraPosition();
rayForward.normalize();
float farPlane = 600.f;
rayForward*= farPlane;
btVector3 rightOffset;
btVector3 vertical(0.f,1.f,0.f);
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;
btVector3 rayToCenter = rayFrom + rayForward;
btVector3 dHor = hor * 1.f/float(screenWidth);
btVector3 dVert = vertical * 1.f/float(screenHeight);
btTransform rayFromTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFrom);
btTransform rayFromLocal;
btTransform rayToLocal;
int x;
///clear texture
for (x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
btVector4 rgba(0.2f,0.2f,0.2f,1.f);
raytracePicture->setPixel(x,y,rgba);
}
}
#if 1
btVector3 rayTo;
btTransform colObjWorldTransform;
colObjWorldTransform.setIdentity();
int mode = 0;
for (x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += x * dHor;
rayTo -= y * dVert;
btVector3 worldNormal(0,0,0);
btVector3 worldPoint(0,0,0);
bool hasHit = false;
int mode = 0;
switch (mode)
{
case 0:
hasHit = lowlevelRaytest(rayFrom,rayTo,worldNormal,worldPoint);
break;
case 1:
hasHit = singleObjectRaytest(rayFrom,rayTo,worldNormal,worldPoint);
break;
case 2:
hasHit = worldRaytest(rayFrom,rayTo,worldNormal,worldPoint);
break;
default:
{
}
}
if (hasHit)
{
float lightVec0 = worldNormal.dot(btVector3(0,-1,-1));//0.4f,-1.f,-0.4f));
float lightVec1= worldNormal.dot(btVector3(-1,0,-1));//-0.4f,-1.f,-0.4f));
rgba = btVector4(lightVec0,lightVec1,0,1.f);
rgba.setMin(btVector3(1,1,1));
rgba.setMax(btVector3(0.2,0.2,0.2));
rgba[3] = 1.f;
raytracePicture->setPixel(x,y,rgba);
} else
btVector4 rgba = raytracePicture->getPixel(x,y);
if (!rgba.length2())
{
raytracePicture->setPixel(x,y,btVector4(1,1,1,1));
}
}
}
#endif
extern unsigned char sFontData[];
if (0)
{
const char* text="ABC abc 123 !@#";
int x=0;
for (int cc = 0;cc<strlen(text);cc++)
{
char testChar = text[cc];//'b';
char ch = testChar-32;
int startx=ch%16;
int starty=ch/16;
//for (int i=0;i<256;i++)
for (int i=startx*16;i<(startx*16+16);i++)
{
int y=0;
//for (int j=0;j<256;j++)
//for (int j=0;j<256;j++)
for (int j=starty*16;j<(starty*16+16);j++)
{
btVector4 rgba(0,0,0,1);
rgba[0] = (sFontData[i*3+255*256*3-(256*j)*3])/255.f;
//rgba[0] += (sFontData[(i+1)*3+255*256*3-(256*j)*3])/255.*0.25f;
//rgba[0] += (sFontData[(i)*3+255*256*3-(256*j+1)*3])/255.*0.25f;
//rgba[0] += (sFontData[(i+1)*3+255*256*3-(256*j+1)*3])/255.*0.25;
//if (rgba[0]!=0.f)
{
rgba[1]=rgba[0];
rgba[2]=rgba[0];
rgba[3]=1.f;
//raytracePicture->setPixel(x,y,rgba);
raytracePicture->addPixel(x,y,rgba);
}
y++;
}
x++;
}
}
}
//raytracePicture->grapicalPrintf("CCD RAYTRACER",sFontData);
char buffer[256];
sprintf(buffer,"%d rays",screenWidth*screenHeight*numObjects);
//sprintf(buffer,"Toggle",screenWidth*screenHeight*numObjects);
//sprintf(buffer,"TEST",screenWidth*screenHeight*numObjects);
//raytracePicture->grapicalPrintf(buffer,sFontData,0,10);//&BMF_font_helv10,0,10);
raytracePicture->grapicalPrintf(buffer,sFontData,0,0);//&BMF_font_helv10,0,10);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glFrustum(-1.0,1.0,-1.0,1.0,3,2020.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity(); // reset The Modelview Matrix
glTranslatef(0.0f,0.0f,-3.1f); // Move Into The Screen 5 Units
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,glTextureId );
const unsigned char *ptr = raytracePicture->getBuffer();
glTexImage2D(GL_TEXTURE_2D,
0,
GL_RGBA,
raytracePicture->getWidth(),raytracePicture->getHeight(),
0,
GL_RGBA,
GL_UNSIGNED_BYTE,
ptr);
glEnable (GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor4f (1,1,1,1); // alpha=0.5=half visible
glBegin(GL_QUADS);
glTexCoord2f(0.0f, 0.0f);
glVertex2f(-1,1);
glTexCoord2f(1.0f, 0.0f);
glVertex2f(1,1);
glTexCoord2f(1.0f, 1.0f);
glVertex2f(1,-1);
glTexCoord2f(0.0f, 1.0f);
glVertex2f(-1,-1);
glEnd();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glDisable(GL_TEXTURE_2D);
glDisable(GL_DEPTH_TEST);
GL_ShapeDrawer::drawCoordSystem();
{
for (int i=0;i<numObjects;i++)
{
btVector3 aabbMin,aabbMax;
shapePtr[i]->getAabb(transforms[i],aabbMin,aabbMax);
}
}
glPushMatrix();
glPopMatrix();
pitch += 0.005f;
yaw += 0.01f;
m_azi += 1.f;
glFlush();
glutSwapBuffers();
}
void PhysicsServerExample::renderScene()
{
B3_PROFILE("PhysicsServerExample::RenderScene");
static char line0[1024];
static char line1[1024];
if (gEnableRealTimeSimVR)
{
static int frameCount=0;
static btScalar prevTime = m_clock.getTimeSeconds();
frameCount++;
static btScalar worseFps = 1000000;
int numFrames = 200;
static int count = 0;
count++;
if (0 == (count & 1))
{
btScalar curTime = m_clock.getTimeSeconds();
btScalar fps = 1. / (curTime - prevTime);
prevTime = curTime;
if (fps < worseFps)
{
worseFps = fps;
}
if (count > numFrames)
{
count = 0;
sprintf(line0, "fps:%f frame:%d", worseFps, frameCount / 2);
sprintf(line1, "drop:%d tscale:%f dt:%f, substep %f)", gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep);
gDroppedSimulationSteps = 0;
worseFps = 1000000;
}
}
#ifdef BT_ENABLE_VR
if ((gInternalSimFlags&2 ) && m_tinyVrGui==0)
{
ComboBoxParams comboParams;
comboParams.m_comboboxId = 0;
comboParams.m_numItems = 0;
comboParams.m_startItem = 0;
comboParams.m_callback = 0;//MyComboBoxCallback;
comboParams.m_userPointer = 0;//this;
m_tinyVrGui = new TinyVRGui(comboParams,this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface());
m_tinyVrGui->init();
}
if (m_tinyVrGui)
{
b3Transform tr;tr.setIdentity();
tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2]));
tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3]));
tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
b3Scalar dt = 0.01;
m_tinyVrGui->clearTextArea();
m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255);
m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255);
m_tinyVrGui->tick(dt,tr);
}
#endif//BT_ENABLE_VR
}
///debug rendering
//m_args[0].m_cs->lock();
//gVRTeleportPos[0] += 0.01;
vrOffset[12]=-gVRTeleportPos[0];
vrOffset[13]=-gVRTeleportPos[1];
vrOffset[14]=-gVRTeleportPos[2];
this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->
getActiveCamera()->setVRCameraOffsetTransform(vrOffset);
m_physicsServer.renderScene();
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{
if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i])
{
btVector3 from = m_args[0].m_vrControllerPos[i];
btMatrix3x3 mat(m_args[0].m_vrControllerOrn[i]);
btVector3 toX = from+mat.getColumn(0);
btVector3 toY = from+mat.getColumn(1);
btVector3 toZ = from+mat.getColumn(2);
int width = 2;
btVector4 color;
color=btVector4(1,0,0,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width);
color=btVector4(0,1,0,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
color=btVector4(0,0,1,1);
m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
}
}
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
gEnableRealTimeSimVR = true;
}
if (gDebugRenderToggle)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
B3_PROFILE("Draw Debug HUD");
//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
float pos[4];
m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos);
pos[0]+=gVRTeleportPos[0];
pos[1]+=gVRTeleportPos[1];
pos[2]+=gVRTeleportPos[2];
btTransform viewTr;
btScalar m[16];
float mf[16];
m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraViewMatrix(mf);
for (int i=0;i<16;i++)
{
m[i] = mf[i];
}
m[12]=+gVRTeleportPos[0];
m[13]=+gVRTeleportPos[1];
m[14]=+gVRTeleportPos[2];
viewTr.setFromOpenGLMatrix(m);
btTransform viewTrInv = viewTr.inverse();
btVector3 side = viewTrInv.getBasis().getColumn(0);
btVector3 up = viewTrInv.getBasis().getColumn(1);
btVector3 fwd = viewTrInv.getBasis().getColumn(2);
float upMag = 0;
float sideMag = 2.2;
float fwdMag = -4;
m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1);
//btVector3 fwd = viewTrInv.getBasis().getColumn(2);
up = viewTrInv.getBasis().getColumn(1);
upMag = -0.3;
m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1);
}
//m_args[0].m_cs->unlock();
}
void BasicExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
createEmptyDynamicsWorld();
//m_dynamicsWorld->setGravity(btVector3(0,0,0));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
if (m_dynamicsWorld->getDebugDrawer())
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
//groundShape->initializePolyhedralFeatures();
//btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
}
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btBoxShape* colShape = createBoxShape(btVector3(.1,.1,.1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//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)
colShape->calculateLocalInertia(mass,localInertia);
for (int k=0;k<ARRAY_SIZE_Y;k++)
{
for (int i=0;i<ARRAY_SIZE_X;i++)
{
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(btVector3(
btScalar(0.2*i),
btScalar(2+.2*k),
btScalar(0.2*j)));
createRigidBody(mass,startTransform,colShape);
}
}
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void ChainDemo::initPhysics()
{
// Bullet2RigidBodyDemo::initPhysics();
m_config = new btDefaultCollisionConfiguration;
m_dispatcher = new btCollisionDispatcher(m_config);
m_bp = new btDbvtBroadphase();
//m_solver = new btNNCGConstraintSolver();
m_solver = new btSequentialImpulseConstraintSolver();
// btDantzigSolver* mlcp = new btDantzigSolver();
//btLemkeSolver* mlcp = new btLemkeSolver();
//m_solver = new btMLCPSolver(mlcp);
// m_solver = new btSequentialImpulseConstraintSolver();
//btMultiBodyConstraintSolver* solver = new btMultiBodyConstraintSolver();
//m_solver = solver;
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_bp,m_solver,m_config);
m_dynamicsWorld->getSolverInfo().m_numIterations = 1000;
m_dynamicsWorld->getSolverInfo().m_splitImpulse = false;
int curColor=0;
//create ground
btScalar radius=scaling;
int unitCubeShapeId = m_glApp->registerCubeShape();
float pos[]={0,0,0};
float orn[]={0,0,0,1};
//eateGround(unitCubeShapeId);
int sphereShapeId = m_glApp->registerGraphicsSphereShape(radius,false);
{
float halfExtents[]={scaling,scaling,scaling,1};
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
btTransform startTransform;
startTransform.setIdentity();
btCollisionShape* colShape = new btSphereShape(scaling);
btScalar largeMass[]={1000,10,100,1000};
for (int i=0;i<1;i++)
{
btAlignedObjectArray<btRigidBody*> bodies;
for (int k=0;k<NUM_SPHERES;k++)
{
btVector3 localInertia(0,0,0);
btScalar mass = 0.f;
curColor = 1;
switch (k)
{
case 0:
{
mass = largeMass[i];
curColor = 0;
break;
}
case NUM_SPHERES-1:
{
mass = 0.f;
curColor = 2;
break;
}
default:
{
curColor = 1;
mass = 1.f;
}
};
if (mass)
colShape ->calculateLocalInertia(mass,localInertia);
btVector4 color = colors[curColor];
startTransform.setOrigin(btVector3(
btScalar(7.5+-i*5),
btScalar(6.*scaling+2.0*scaling*k),
btScalar(0)));
m_glApp->m_instancingRenderer->registerGraphicsInstance(sphereShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
bodies.push_back(body);
body->setActivationState(DISABLE_DEACTIVATION);
m_dynamicsWorld->addRigidBody(body);
}
//add constraints
btVector3 pivotInA(0,radius,0);
btVector3 pivotInB(0,-radius,0);
for (int k=0;k<NUM_SPHERES-1;k++)
{
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*bodies[k],*bodies[k+1],pivotInA,pivotInB);
m_dynamicsWorld->addConstraint(p2p,true);
}
}
}
m_glApp->m_instancingRenderer->writeTransforms();
}
void RigidBodyFromObjExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
//if (m_dynamicsWorld->getDebugDrawer())
// m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
///create a few basic rigid bodies
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
{
btScalar mass(0.);
createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
}
//load our obj mesh
const char* fileName = "teddy.obj";//sphere8.obj";//sponza_closed.obj";//sphere8.obj";
char relativeFileName[1024];
if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
{
char pathPrefix[1024];
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, "");
printf("[INFO] Obj loaded: Extracted %d verticed from obj file [%s]\n", glmesh->m_numvertices, fileName);
const GLInstanceVertex& v = glmesh->m_vertices->at(0);
btConvexHullShape* shape = new btConvexHullShape((const btScalar*)(&(v.xyzw[0])), glmesh->m_numvertices, sizeof(GLInstanceVertex));
float scaling[4] = {0.1,0.1,0.1,1};
btVector3 localScaling(scaling[0],scaling[1],scaling[2]);
shape->setLocalScaling(localScaling);
if (m_options & OptimizeConvexObj)
{
shape->optimizeConvexHull();
}
if (m_options & ComputePolyhedralFeatures)
{
shape->initializePolyhedralFeatures();
}
//shape->setMargin(0.001);
m_collisionShapes.push_back(shape);
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
shape->calculateLocalInertia(mass,localInertia);
float color[4] = {1,1,1,1};
float orn[4] = {0,0,0,1};
float pos[4] = {0,3,0,0};
btVector3 position(pos[0],pos[1],pos[2]);
startTransform.setOrigin(position);
btRigidBody* body = createRigidBody(mass,startTransform,shape);
bool useConvexHullForRendering = ((m_options & ObjUseConvexHullForRendering)!=0);
if (!useConvexHullForRendering)
{
int shapeId = m_guiHelper->registerGraphicsShape(&glmesh->m_vertices->at(0).xyzw[0],
glmesh->m_numvertices,
&glmesh->m_indices->at(0),
glmesh->m_numIndices,
B3_GL_TRIANGLES, -1);
shape->setUserIndex(shapeId);
int renderInstance = m_guiHelper->registerGraphicsInstance(shapeId,pos,orn,color,scaling);
body->setUserIndex(renderInstance);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void TestJointTorqueSetup::initPhysics()
{
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
//create a static ground object
if (0)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-1.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
{
bool floating = false;
bool damping = true;
bool gyro = true;
int numLinks = 5;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if(baseMass)
{
btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool isMultiDof = false;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI/ 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for(int i = 0; i < numLinks; ++i)
{
if(!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
//pMultiBody->finalizeMultiDof();
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
///
world->addMultiBody(pMultiBody);
btMultiBody* mbC = pMultiBody;
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if(!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}else
{ mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
btVector3 gravity(0,0,0);
//gravity[upAxis] = -9.81;
m_dynamicsWorld->setGravity(gravity);
//////////////////////////////////////////////
if(numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
if(mbC->isMultiDof())
mbC->setJointPosMultiDof(0, &q0);
else
mbC->setJointPos(0, q0);
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
double friction = 1;
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
m_guiHelper->createCollisionShapeGraphicsObject(box);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
world->addCollisionObject(col, 2,1+2);
btVector3 color(0.0,0.0,0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
m_guiHelper->createCollisionShapeGraphicsObject(box);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
world->addCollisionObject(col,2,1+2);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
pMultiBody->getLink(i).m_collider=col;
}
}
}
