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

1
data/cube_small.urdf
View File

@@ -3,7 +3,6 @@
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<spinning_friction value="0.1"/>
<inertia_scaling value="3.0"/>
</contact>
<inertial>

BIN
data/gripper/meshes/l_gripper_tip_scaled.stl
View File

Binary file not shown.

16
data/jenga/jenga.mtl Normal file
View File

@@ -0,0 +1,16 @@
newmtl jenga
Ns 10.0000
Ni 1.5000
d 1.0000
Tr 0.0000
Tf 1.0000 1.0000 1.0000
illum 2
Ka 0.0000 0.0000 0.0000
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
map_Ka jenga.tga
map_Kd jenga.png

113
data/jenga/jenga.obj Normal file
View File

@@ -0,0 +1,113 @@
# jenga.obj
#
o jenga
mtllib jenga.mtl
v -0.5 -0.5 0.5
v 0.5 -0.5 0.5
v 0.5 0.5 0.5
v -0.5 0.5 0.5
v -0.5 -0.5 -0.5
v 0.5 -0.5 -0.5
v 0.5 0.5 -0.5
v -0.5 0.5 -0.5
v -0.5 -0.5 -0.5
v -0.5 0.5 -0.5
v -0.5 0.5 0.5
v -0.5 -0.5 0.5
v 0.5 -0.5 -0.5
v 0.5 0.5 -0.5
v 0.5 0.5 0.5
v 0.5 -0.5 0.5
v -0.5 -0.5 -0.5
v -0.5 -0.5 0.5
v 0.5 -0.5 0.5
v 0.5 -0.5 -0.5
v -0.5 0.5 -0.5
v -0.5 0.5 0.5
v 0.5 0.5 0.5
v 0.5 0.5 -0.5
vt 0 1
vt 0 0.75
vt 0.25 0.75
vt 0.25 1
vt 0.25 0.5
vt 0.25 0.75
vt 0.5 0.75
vt 0.5 0.5
vt 1 0.75
vt 0.75 0.75
vt 0.75 1
vt 1 1
vt 0.25 0.75
vt 0.5 0.75
vt 0.5 1
vt 0.25 1
vt 0 0.5
vt 0 0.75
vt 0.25 0.75
vt 0.25 0.5
vt 0.75 0.75
vt 0.75 1
vt 0.5 1
vt 0.5 0.75
vn 0 0 1
vn 0 0 1
vn 0 0 1
vn 0 0 1
vn 0 0 -1
vn 0 0 -1
vn 0 0 -1
vn 0 0 -1
vn -1 0 0
vn -1 0 0
vn -1 0 0
vn -1 0 0
vn 1 0 0
vn 1 0 0
vn 1 0 0
vn 1 0 0
vn 0 -1 0
vn 0 -1 0
vn 0 -1 0
vn 0 -1 0
vn 0 1 0
vn 0 1 0
vn 0 1 0
vn 0 1 0
g jenga
usemtl jenga
s 1
f 1/1/1 2/2/2 3/3/3
f 1/1/1 3/3/3 4/4/4
s 2
f 7/7/7 6/6/6 5/5/5
f 8/8/8 7/7/7 5/5/5
s 3
f 11/11/11 10/10/10 9/9/9
f 12/12/12 11/11/11 9/9/9
s 4
f 13/13/13 14/14/14 15/15/15
f 13/13/13 15/15/15 16/16/16
s 5
f 19/19/19 18/18/18 17/17/17
f 20/20/20 19/19/19 17/17/17
s 6
f 21/21/21 22/22/22 23/23/23
f 21/21/21 23/23/23 24/24/24

BIN
data/jenga/jenga.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 280 KiB

31
data/jenga/jenga.urdf Normal file
View File

@@ -0,0 +1,31 @@
<?xml version="0.0" ?>
<robot name="jenga.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<spinning_friction value="0.5"/>
<inertia_scaling value="3.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="jenga.obj" scale="0.15 0.05 0.03"/>
</geometry>
<material name="blue">
<color rgba="0.5 0.5 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.15 0.05 0.03"/>
</geometry>
</collision>
</link>
</robot>

BIN
data/l_finger_collision.stl Normal file
View File

Binary file not shown.

2
data/plane.urdf
View File

@@ -1,6 +1,6 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<link name="planeLink">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>

47
data/pr2_gripper.urdf
View File

@@ -17,7 +17,7 @@
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<mass value="0.5"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
@@ -32,23 +32,23 @@
<link name="left_gripper">
<contact>
<lateral_friction value="50.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
@@ -60,7 +60,8 @@
<link name="left_tip">
<contact>
<lateral_friction value="50.0"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
@@ -72,10 +73,10 @@
<geometry>
<box size=".03 .03 .02"/>
</geometry>
<origin rpy="0.0 0 0" xyz="0.109137 0.00495 0"/>
<origin rpy="0.0 0 0" xyz="0.105 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
@@ -90,22 +91,23 @@
<link name="right_gripper">
<contact>
<lateral_friction value="50.0"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="l_finger.stl"/>
</geometry>
<collision>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
@@ -117,7 +119,8 @@
<link name="right_tip">
<contact>
<lateral_friction value="50.0"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
@@ -127,12 +130,12 @@
</visual>
<collision>
<geometry>
<box size=".03 .03 .02"/>
<box size=".03 .03 .02"/>
</geometry>
<origin rpy="-3.1415 0 0" xyz="0.109137 0.00495 0"/>
<origin rpy="-3.1415 0 0" xyz="0.105 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>

3
data/samurai.urdf
View File

@@ -1,9 +1,6 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link concave="yes" name="baseLink">
<contact>
<rolling_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>

8
data/samurai_monastry.obj
View File

@@ -331416,11 +331416,3 @@ f 30335 30336
f 1329 1330
f 77715 77717
f 20619 20622
o Cube
v -0.500000 -0.200000 -1.000000
v 1.000000 -0.000000 0.000000
v -0.500000 0.200000 1.000000
usemtl Material
s off
f 86188 86189 86190
f 86189 86188 86190

5
data/sphere2.urdf
View File

@@ -1,6 +1,11 @@
<?xml version="0.0" ?>
<robot name="urdf_robot">
<link name="baseLink">
<contact>
<rolling_friction value="0.03"/>
<spinning_friction value="0.03"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="10.0"/>

32
data/teddy_vhacd.urdf Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0.7 0.5 0.3"/>
<mass value="1.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="teddy2_VHACD_CHs.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="teddy2_VHACD_CHs.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

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

4
src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
View File

@@ -372,10 +372,10 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
return userData;
}
//#include <stdio.h>
#include "LinearMath/btQuickprof.h"
void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
{
BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
int i;
// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());

5
src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
View File

@@ -16,7 +16,7 @@ subject to the following restrictions:
#include "btCollisionDispatcher.h"
#include "LinearMath/btQuickprof.h"
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
@@ -227,6 +227,8 @@ public:
virtual bool processOverlap(btBroadphasePair& pair)
{
BT_PROFILE("btCollisionDispatcher::processOverlap");
(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
return false;
@@ -249,7 +251,6 @@ void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pa
//by default, Bullet will use this near callback
void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
{

2
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
View File

@@ -15,6 +15,7 @@ subject to the following restrictions:
*/
#include "btCompoundCompoundCollisionAlgorithm.h"
#include "LinearMath/btQuickprof.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
@@ -124,6 +125,7 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
{
BT_PROFILE("btCompoundCompoundLeafCallback::Process");
m_numOverlapPairs++;

5
src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
View File

@@ -15,6 +15,7 @@ subject to the following restrictions:
#include "btConvexConcaveCollisionAlgorithm.h"
#include "LinearMath/btQuickprof.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
@@ -79,6 +80,7 @@ void btConvexTriangleCallback::clearCache()
void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
partId, int triangleIndex)
{
BT_PROFILE("btConvexTriangleCallback::processTriangle");
if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
{
@@ -184,7 +186,7 @@ void btConvexConcaveCollisionAlgorithm::clearCache()
void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
@@ -265,6 +267,7 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
{
BT_PROFILE("processTriangle");
(void)partId;
(void)triangleIndex;
//do a swept sphere for now

9
src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
View File

@@ -245,16 +245,18 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
btStridingMeshInterface* m_meshInterface;
btTriangleCallback* m_callback;
btVector3 m_triangle[3];
int m_numOverlap;
MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
:m_meshInterface(meshInterface),
m_callback(callback)
m_callback(callback),
m_numOverlap(0)
{
}
virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
{
m_numOverlap++;
const unsigned char *vertexbase;
int numverts;
PHY_ScalarType type;
@@ -321,8 +323,7 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface);
m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
#endif//DISABLE_BVH

4
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
View File

@@ -39,7 +39,7 @@ subject to the following restrictions:
//temp globals, to improve GJK/EPA/penetration calculations
int gNumDeepPenetrationChecks = 0;
int gNumGjkChecks = 0;
btScalar gGjkEpaPenetrationTolerance = 0.001;
btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
@@ -304,7 +304,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
}
bool catchDegeneratePenetrationCase =
(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
//if (checkPenetration && !isValid)
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))