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

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans
2016-09-20 23:19:27 -07:00
parent e1ba6e9736 0a628f06cc
commit 89eef8e091
29 changed files with 542 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@@ -505,6 +505,8 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor
void OpenGLGuiHelper::drawText3D( const char* txt, float posX, float posY, float posZ, float size)
{
B3_PROFILE("OpenGLGuiHelper::drawText3D");
btAssert(m_data->m_glApp);
m_data->m_glApp->drawText3D(txt,posX,posY,posZ,size);
}

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

@@ -14,7 +14,7 @@ subject to the following restrictions:
#include "BulletUrdfImporter.h"
#include "../../CommonInterfaces/CommonRenderInterface.h"
#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
#include "URDFImporterInterface.h"
#include "btBulletCollisionCommon.h"
#include "../ImportObjDemo/LoadMeshFromObj.h"
@@ -28,6 +28,7 @@ subject to the following restrictions:
#include "../ImportMeshUtility/b3ImportMeshUtility.h"
static btScalar gUrdfDefaultCollisionMargin = 0.001;
#include <iostream>
#include <fstream>
@@ -295,26 +296,66 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
if (linkPtr)
{
UrdfLink* link = *linkPtr;
btMatrix3x3 linkInertiaBasis;
btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ;
if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
{
mass = 0.f;
localInertiaDiagonal.setValue(0,0,0);
linkMass = 0.f;
principalInertiaX = 0.f;
principalInertiaY = 0.f;
principalInertiaZ = 0.f;
linkInertiaBasis.setIdentity();
}
else
{
mass = link->m_inertia.m_mass;
localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
link->m_inertia.m_izz);
linkMass = link->m_inertia.m_mass;
if (link->m_inertia.m_ixy == 0.0 &&
link->m_inertia.m_ixz == 0.0 &&
link->m_inertia.m_iyz == 0.0)
{
principalInertiaX = link->m_inertia.m_ixx;
principalInertiaY = link->m_inertia.m_iyy;
principalInertiaZ = link->m_inertia.m_izz;
linkInertiaBasis.setIdentity();
}
else
{
principalInertiaX = link->m_inertia.m_ixx;
btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
btScalar threshold = 1.0e-6;
int numIterations = 30;
inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations);
principalInertiaX = inertiaTensor[0][0];
principalInertiaY = inertiaTensor[1][1];
principalInertiaZ = inertiaTensor[2][2];
}
}
inertialFrame = link->m_inertia.m_linkLocalFrame;
mass = linkMass;
if (principalInertiaX < 0 ||
principalInertiaX > (principalInertiaY + principalInertiaZ) ||
principalInertiaY < 0 ||
principalInertiaY > (principalInertiaX + principalInertiaZ) ||
principalInertiaZ < 0 ||
principalInertiaZ > (principalInertiaX + principalInertiaY))
{
b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str());
principalInertiaX = 0.f;
principalInertiaY = 0.f;
principalInertiaZ = 0.f;
linkInertiaBasis.setIdentity();
}
localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ);
inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis()*linkInertiaBasis);
}
else
{
mass = 1.f;
localInertiaDiagonal.setValue(1,1,1);
inertialFrame.setIdentity();
}
{
mass = 1.f;
localInertiaDiagonal.setValue(1,1,1);
inertialFrame.setIdentity();
}
}
bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
@@ -360,7 +401,45 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
return true;
}
static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes)
{
btCompoundShape* compound = new btCompoundShape();
btTransform identity;
identity.setIdentity();
for (int s = 0; s<(int)shapes.size(); s++)
{
btConvexHullShape* convexHull = new btConvexHullShape();
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
for (int f = 0; f<faceCount; f += 3)
{
btVector3 pt;
pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
convexHull->addPoint(pt,false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
convexHull->addPoint(pt, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
convexHull->addPoint(pt, false);
}
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
compound->addChildShape(identity,convexHull);
}
return compound;
}
btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
{
@@ -386,7 +465,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(0.001);
cylZShape->setMargin(gUrdfDefaultCollisionMargin);
cylZShape->initializePolyhedralFeatures();
//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
@@ -403,7 +482,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
shape = boxShape;
shape ->setMargin(0.001);
shape ->setMargin(gUrdfDefaultCollisionMargin);
break;
}
case URDF_GEOM_SPHERE:
@@ -412,7 +491,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
btScalar radius = collision->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
shape = sphereShape;
shape ->setMargin(0.001);
shape ->setMargin(gUrdfDefaultCollisionMargin);
break;
break;
@@ -467,7 +546,18 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
{
case FILE_OBJ:
{
glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix);
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{
glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
//create a convex hull for each shape, and store it in a btCompoundShape
shape = createConvexHullFromShapes(shapes);
return shape;
}
break;
}
case FILE_STL:
@@ -600,7 +690,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
//cylZShape->initializePolyhedralFeatures();
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
cylZShape->setMargin(0.001);
cylZShape->setMargin(gUrdfDefaultCollisionMargin);
shape = cylZShape;
}
} else
@@ -659,7 +749,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(0.001);
cylZShape->setMargin(gUrdfDefaultCollisionMargin);
convexColShape = cylZShape;
break;
}
@@ -671,7 +761,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
convexColShape = boxShape;
convexColShape->setMargin(0.001);
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
break;
}
case URDF_GEOM_SPHERE:
@@ -679,7 +769,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
btScalar radius = visual->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
convexColShape = sphereShape;
convexColShape->setMargin(0.001);
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
break;
break;
@@ -1077,7 +1167,7 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index)
btCompoundShape* compoundShape = new btCompoundShape();
m_data->m_allocatedCollisionShapes.push_back(compoundShape);
compoundShape->setMargin(0.001);
compoundShape->setMargin(gUrdfDefaultCollisionMargin);
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)

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

@@ -162,8 +162,6 @@ void ConvertURDF2BulletInternal(
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
btRigidBody* parentRigidBody = 0;
//std::string name = u2b.getLinkName(urdfLinkIndex);
//b3Printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
//b3Printf("mb link index = %d\n",mbLinkIndex);
btTransform parentLocalInertialFrame;
@@ -322,6 +320,14 @@ void ConvertURDF2BulletInternal(
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
if (jointLowerLimit <= jointUpperLimit)
{
//std::string name = u2b.getLinkName(urdfLinkIndex);
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
world1->addMultiBodyConstraint(con);
}
} else
{
@@ -343,8 +349,14 @@ void ConvertURDF2BulletInternal(
-offsetInB.getOrigin(),
disableParentCollision);
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit);
world1->addMultiBodyConstraint(con);
if (jointLowerLimit <= jointUpperLimit)
{
//std::string name = u2b.getLinkName(urdfLinkIndex);
//printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit);
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
world1->addMultiBodyConstraint(con);
}
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
} else
@@ -386,8 +398,9 @@ void ConvertURDF2BulletInternal(
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
col->setWorldTransform(tr);
bool isDynamic = true;
//base and fixed? -> static, otherwise flag as dynamic
bool isDynamic = (mbLinkIndex<0 && cache.m_bulletMultiBody->hasFixedBase())? false : true;
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);

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

@@ -816,7 +816,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger)
{
joint.m_lowerLimit = 0.f;
joint.m_upperLimit = 0.f;
joint.m_upperLimit = -1.f;
joint.m_effortLimit = 0.f;
joint.m_velocityLimit = 0.f;
joint.m_jointDamping = 0.f;

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

@@ -138,7 +138,7 @@ struct UrdfJoint
double m_jointFriction;
UrdfJoint()
:m_lowerLimit(0),
m_upperLimit(0),
m_upperLimit(-1),
m_effortLimit(0),
m_velocityLimit(0),
m_jointDamping(0),

5
examples/OpenGLWindow/GLPrimitiveRenderer.cpp
View File

@@ -1,7 +1,7 @@
#ifndef NO_OPENGL3
#include "GLPrimitiveRenderer.h"
#include "GLPrimInternalData.h"
//#include "Bullet3Common/b3Logging.h"
#include "LoadShader.h"
#include <assert.h>
@@ -205,7 +205,8 @@ void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float
void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVertex& v1,const PrimVertex& v2,const PrimVertex& v3,float viewMat[16],float projMat[16], bool useRGBA)
{
//B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D");
assert(glGetError()==GL_NO_ERROR);

3
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -230,7 +230,7 @@ struct sth_stash* SimpleOpenGL3App::getFontStash()
void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float worldPosY, float worldPosZ, float size1)
{
B3_PROFILE("SimpleOpenGL3App::drawText3D");
float viewMat[16];
float projMat[16];
CommonCameraInterface* cam = m_instancingRenderer->getActiveCamera();
@@ -273,6 +273,7 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world
bool measureOnly = false;
float fontSize= 32;//64;//512;//128;
sth_draw_text(m_data->m_fontStash,
m_data->m_droidRegular,fontSize,posX,posY,
txt,&dx, this->m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),measureOnly,m_window->getRetinaScale());

114
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -28,6 +28,7 @@
#include "SharedMemoryCommands.h"
btVector3 gLastPickPos(0, 0, 0);
bool gEnableRealTimeSimVR=false;
struct UrdfLinkNameMapUtil
{
@@ -556,7 +557,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
m_data = new PhysicsServerCommandProcessorInternalData();
createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0;
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001;
}
@@ -597,6 +598,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.05;
}
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@@ -2838,7 +2840,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
m_data->m_logPlayback = pb;
}
btVector3 gVRGripperPos(0,0,0);
btVector3 gVRGripperPos(0,0,0.2);
btQuaternion gVRGripperOrn(0,0,0,1);
btScalar gVRGripperAnalog = 0;
bool gVRGripperClosed = false;
@@ -2850,10 +2852,11 @@ double gDtInSec = 0.f;
double gSubStep = 0.f;
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
if (m_data->m_allowRealTimeSimulation && m_data->m_guiHelper)
if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
{
static btAlignedObjectArray<char> gBufferServerToClient;
gBufferServerToClient.resize(32768);
gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
if (!m_data->m_hasGround)
@@ -2864,7 +2867,8 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
if (m_data->m_gripperRigidbodyFixed == 0)
{
int bodyId = 0;
@@ -2874,11 +2878,11 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setHasSelfCollision(1);
btVector3 pivotInParent(0, 0, 0);
parentBody->m_multiBody->setHasSelfCollision(0);
btVector3 pivotInParent(0.2, 0, 0);
btMatrix3x3 frameInParent;
frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
//frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
frameInParent.setIdentity();
btVector3 pivotInChild(0, 0, 0);
btMatrix3x3 frameInChild;
frameInChild.setIdentity();
@@ -2890,62 +2894,110 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
m_data->m_gripperMultiBody->setJointPos(0, 0);
m_data->m_gripperMultiBody->setJointPos(2, 0);
}
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(1.);
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(10000);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
}
}
#if 1
for (int i = 0; i < 10; i++)
{
loadUrdf("cube.urdf", btVector3(3, -2, 0.5+i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
loadUrdf("sphere2.urdf", btVector3(-2, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-2, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-2, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("kuka_iiwa/model.urdf", btVector3(3, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("husky/husky.urdf", btVector3(1, 1, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// loadUrdf("cube_small.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
#endif
#if 0
int jengaHeight = 10;
for (int j = 0; j < jengaHeight; j++)
{
for (int i = 0; i < 3; i++)
{
if (j & 1)
{
loadUrdf("jenga/jenga.urdf", btVector3(-0.5, 0.05*i, .03*0.5 + .03*j), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
else
{
btQuaternion orn(btVector3(0, 0, 1), SIMD_HALF_PI);
loadUrdf("jenga/jenga.urdf", btVector3(-0.5 -1 / 3.*0.15 + 0.05*i, +1 / 3.*0.15,0.03*0.5 + .03*j), orn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
}
}
#endif
//loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("husky/husky.urdf", btVector3(5, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_huskyId = bodyId;
}
loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
loadUrdf("teddy_vhacd.urdf", btVector3(1, 1, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
}
if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)
{
m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
if (m_data->m_gripperMultiBody->getNumLinks() > 2)
for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++)
{
for (int i = 0; i < 2; i++)
if (supportsJointMotor(m_data->m_gripperMultiBody, i))
{
if (supportsJointMotor(m_data->m_gripperMultiBody, i * 2))
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr;
if (motor)
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr;
if (motor)
motor->setErp(0.2);
btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29;
btScalar maxPosTarget = 0.55;
if (m_data->m_gripperMultiBody->getJointPos(i) < 0)
{
motor->setErp(0.01);
motor->setPositionTarget(0.1+(1-gVRGripperAnalog)*SIMD_HALF_PI*0.5, 1);
motor->setVelocityTarget(0, 0.1);
btScalar maxImp = 1550.*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
m_data->m_gripperMultiBody->setJointPos(i,0);
}
if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget)
{
m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget);
}
motor->setPositionTarget(posTarget, 1);
motor->setVelocityTarget(0, 0.5);
btScalar maxImp = 1*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
}
}
}
}
int maxSteps = 3;
int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime);
int maxSteps = m_data->m_numSimulationSubSteps+3;
if (m_data->m_numSimulationSubSteps)
{
gSubStep = m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps;
}
else
{
gSubStep = m_data->m_physicsDeltaTime;
}
int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps, gSubStep);
gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
if (numSteps)
{
gNumSteps = numSteps;
gDtInSec = dtInSec;
gSubStep = m_data->m_physicsDeltaTime;
}
}
}

14
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -19,6 +19,10 @@
extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos(0,0,0);
btQuaternion gVRTeleportOrn(0, 0, 0,1);
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
extern btScalar gVRGripperAnalog;
extern bool gEnableRealTimeSimVR;
extern bool gVRGripperClosed;
@@ -893,6 +897,8 @@ extern double gSubStep;
void PhysicsServerExample::renderScene()
{
B3_PROFILE("PhysicsServerExample::RenderScene");
///debug rendering
//m_args[0].m_cs->lock();
@@ -934,6 +940,8 @@ void PhysicsServerExample::renderScene()
if (gDebugRenderToggle)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
gEnableRealTimeSimVR = true;
B3_PROFILE("Draw Debug HUD");
//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
static int frameCount=0;
@@ -1154,16 +1162,12 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
}
}
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
extern btScalar gVRGripperAnalog;
void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
{
gEnableRealTimeSimVR = true;
if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
{

96
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -1,4 +1,5 @@
#ifdef BT_ENABLE_VR
//#define BT_USE_CUSTOM_PROFILER
//========= Copyright Valve Corporation ============//
#include "../OpenGLWindow/SimpleOpenGL3App.h"
@@ -701,11 +702,12 @@ bool CMainApplication::HandleInput()
{
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync
//so it can (and likely will) cause crashes
//add a special debug drawer that deals with this
//gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe;// :DBG_DrawContactPoints
//gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints+
//btIDebugDraw::DBG_DrawConstraintLimits+
//btIDebugDraw::DBG_DrawConstraints
;
//;
}
sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn);
@@ -751,7 +753,7 @@ bool CMainApplication::HandleInput()
}
}
m_rbShowTrackedDevice[ unDevice ] = state.ulButtonPressed == 0;
// m_rbShowTrackedDevice[ unDevice ] = state.ulButtonPressed == 0;
}
sPrevStates[unDevice] = state;
}
@@ -769,6 +771,8 @@ void CMainApplication::RunMainLoop()
while ( !bQuit && !m_app->m_window->requestedExit())
{
B3_PROFILE("main");
bQuit = HandleInput();
RenderFrame();
@@ -812,9 +816,15 @@ void CMainApplication::RenderFrame()
// for now as fast as possible
if ( m_pHMD )
{
DrawControllers();
{
B3_PROFILE("DrawControllers");
DrawControllers();
}
RenderStereoTargets();
RenderDistortion();
{
B3_PROFILE("RenderDistortion");
RenderDistortion();
}
vr::Texture_t leftEyeTexture = {(void*)leftEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma };
vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture );
@@ -824,6 +834,7 @@ void CMainApplication::RenderFrame()
if ( m_bVblank && m_bGlFinishHack )
{
B3_PROFILE("bGlFinishHack");
//$ HACKHACK. From gpuview profiling, it looks like there is a bug where two renders and a present
// happen right before and after the vsync causing all kinds of jittering issues. This glFinish()
// appears to clear that up. Temporary fix while I try to get nvidia to investigate this problem.
@@ -833,6 +844,7 @@ void CMainApplication::RenderFrame()
// SwapWindow
{
B3_PROFILE("m_app->swapBuffer");
m_app->swapBuffer();
//SDL_GL_SwapWindow( m_pWindow );
@@ -840,6 +852,7 @@ void CMainApplication::RenderFrame()
// Clear
{
B3_PROFILE("glClearColor");
// We want to make sure the glFinish waits for the entire present to complete, not just the submission
// of the command. So, we do a clear here right here so the glFinish will wait fully for the swap.
glClearColor( 0, 0, 0, 1 );
@@ -849,6 +862,8 @@ void CMainApplication::RenderFrame()
// Flush and wait for swap.
if ( m_bVblank )
{
B3_PROFILE("glFlushglFinish");
glFlush();
glFinish();
}
@@ -856,13 +871,18 @@ void CMainApplication::RenderFrame()
// Spew out the controller and pose count whenever they change.
if ( m_iTrackedControllerCount != m_iTrackedControllerCount_Last || m_iValidPoseCount != m_iValidPoseCount_Last )
{
B3_PROFILE("debug pose");
m_iValidPoseCount_Last = m_iValidPoseCount;
m_iTrackedControllerCount_Last = m_iTrackedControllerCount;
b3Printf( "PoseCount:%d(%s) Controllers:%d\n", m_iValidPoseCount, m_strPoseClasses.c_str(), m_iTrackedControllerCount );
}
UpdateHMDMatrixPose();
{
B3_PROFILE("UpdateHMDMatrixPose");
UpdateHMDMatrixPose();
}
}
@@ -1565,6 +1585,8 @@ void CMainApplication::SetupDistortion()
//-----------------------------------------------------------------------------
void CMainApplication::RenderStereoTargets()
{
B3_PROFILE("CMainApplication::RenderStereoTargets");
sExample->stepSimulation(1./60.);
glClearColor( 0.15f, 0.15f, 0.18f, 1.0f ); // nice background color, but not black
@@ -1641,7 +1663,7 @@ void CMainApplication::RenderStereoTargets()
{
sExample->physicsDebugDraw(gDebugDrawFlags);
}
else
{
sExample->renderScene();
}
@@ -1692,7 +1714,7 @@ void CMainApplication::RenderStereoTargets()
{
sExample->physicsDebugDraw(gDebugDrawFlags);
}
else
{
sExample->renderScene();
}
@@ -1720,6 +1742,8 @@ void CMainApplication::RenderStereoTargets()
//-----------------------------------------------------------------------------
void CMainApplication::RenderScene( vr::Hmd_Eye nEye )
{
B3_PROFILE("RenderScene");
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
@@ -1866,38 +1890,47 @@ Matrix4 CMainApplication::GetCurrentViewProjectionMatrix( vr::Hmd_Eye nEye )
//-----------------------------------------------------------------------------
void CMainApplication::UpdateHMDMatrixPose()
{
if ( !m_pHMD )
if (!m_pHMD)
return;
vr::VRCompositor()->WaitGetPoses(m_rTrackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0 );
{
B3_PROFILE("WaitGetPoses");
vr::VRCompositor()->WaitGetPoses(m_rTrackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0);
}
m_iValidPoseCount = 0;
m_strPoseClasses = "";
for ( int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice )
{
if ( m_rTrackedDevicePose[nDevice].bPoseIsValid )
B3_PROFILE("for loop");
for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
{
m_iValidPoseCount++;
m_rmat4DevicePose[nDevice] = ConvertSteamVRMatrixToMatrix4( m_rTrackedDevicePose[nDevice].mDeviceToAbsoluteTracking );
if (m_rDevClassChar[nDevice]==0)
if (m_rTrackedDevicePose[nDevice].bPoseIsValid)
{
switch (m_pHMD->GetTrackedDeviceClass(nDevice))
m_iValidPoseCount++;
m_rmat4DevicePose[nDevice] = ConvertSteamVRMatrixToMatrix4(m_rTrackedDevicePose[nDevice].mDeviceToAbsoluteTracking);
if (m_rDevClassChar[nDevice] == 0)
{
case vr::TrackedDeviceClass_Controller: m_rDevClassChar[nDevice] = 'C'; break;
case vr::TrackedDeviceClass_HMD: m_rDevClassChar[nDevice] = 'H'; break;
case vr::TrackedDeviceClass_Invalid: m_rDevClassChar[nDevice] = 'I'; break;
case vr::TrackedDeviceClass_Other: m_rDevClassChar[nDevice] = 'O'; break;
case vr::TrackedDeviceClass_TrackingReference: m_rDevClassChar[nDevice] = 'T'; break;
default: m_rDevClassChar[nDevice] = '?'; break;
switch (m_pHMD->GetTrackedDeviceClass(nDevice))
{
case vr::TrackedDeviceClass_Controller: m_rDevClassChar[nDevice] = 'C'; break;
case vr::TrackedDeviceClass_HMD: m_rDevClassChar[nDevice] = 'H'; break;
case vr::TrackedDeviceClass_Invalid: m_rDevClassChar[nDevice] = 'I'; break;
case vr::TrackedDeviceClass_Other: m_rDevClassChar[nDevice] = 'O'; break;
case vr::TrackedDeviceClass_TrackingReference: m_rDevClassChar[nDevice] = 'T'; break;
default: m_rDevClassChar[nDevice] = '?'; break;
}
}
m_strPoseClasses += m_rDevClassChar[nDevice];
}
m_strPoseClasses += m_rDevClassChar[nDevice];
}
}
if ( m_rTrackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid )
{
m_mat4HMDPose = m_rmat4DevicePose[vr::k_unTrackedDeviceIndex_Hmd].invert();
B3_PROFILE("m_mat4HMDPose invert");
if (m_rTrackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid)
{
m_mat4HMDPose = m_rmat4DevicePose[vr::k_unTrackedDeviceIndex_Hmd].invert();
}
}
}
@@ -2145,6 +2178,11 @@ void CGLRenderModel::Draw()
//-----------------------------------------------------------------------------
int main(int argc, char *argv[])
{
#ifdef BT_USE_CUSTOM_PROFILER
//b3SetCustomEnterProfileZoneFunc(...);
//b3SetCustomLeaveProfileZoneFunc(...);
#endif
CMainApplication *pMainApplication = new CMainApplication( argc, argv );
if (!pMainApplication->BInit())
@@ -2157,6 +2195,10 @@ int main(int argc, char *argv[])
pMainApplication->Shutdown();
#ifdef BT_USE_CUSTOM_PROFILER
//...
#endif
return 0;
}
#endif //BT_ENABLE_VR