diff --git a/data/capsule.urdf b/data/capsule.urdf new file mode 100644 index 000000000..ade401a61 --- /dev/null +++ b/data/capsule.urdf @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 970613294..32281da3e 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -455,6 +455,16 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co switch (collision->m_geometry.m_type) { + case URDF_GEOM_CAPSULE: + { + btScalar radius = collision->m_geometry.m_capsuleRadius; + btScalar height = btScalar(2.f)*collision->m_geometry.m_capsuleHalfHeight; + btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius,height); + shape = capsuleShape; + shape ->setMargin(gUrdfDefaultCollisionMargin); + break; + } + case URDF_GEOM_CYLINDER: { btScalar cylRadius = collision->m_geometry.m_cylinderRadius; diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 9f06dbfa0..db8f3c498 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -380,7 +380,20 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* geom.m_cylinderLength = urdfLexicalCast(shape->Attribute("length")); } - + else if (type_name == "capsule") + { + geom.m_type = URDF_GEOM_CAPSULE; + if (!shape->Attribute("length") || + !shape->Attribute("radius")) + { + logger->reportError("Capsule shape must have both length and radius attributes"); + return false; + } + geom.m_hasFromTo = false; + geom.m_capsuleRadius = urdfLexicalCast(shape->Attribute("radius")); + geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast(shape->Attribute("length")); + + } else if (type_name == "mesh") { geom.m_type = URDF_GEOM_MESH; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1ab731551..060959330 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3358,25 +3358,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) { - b3ContactPointData pt; - pt.m_bodyUniqueIdA = m_bodyUniqueIdA; - pt.m_bodyUniqueIdB = m_bodyUniqueIdB; - const btManifoldPoint& srcPt = cp; - pt.m_contactDistance = srcPt.getDistance(); - pt.m_contactFlags = 0; - pt.m_linkIndexA = m_linkIndexA; - pt.m_linkIndexB = m_linkIndexB; - for (int j = 0; j < 3; j++) + if (cp.m_distance1<=m_closestDistanceThreshold) { - pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; - pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; - pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + b3ContactPointData pt; + pt.m_bodyUniqueIdA = m_bodyUniqueIdA; + pt.m_bodyUniqueIdB = m_bodyUniqueIdB; + const btManifoldPoint& srcPt = cp; + pt.m_contactDistance = srcPt.getDistance(); + pt.m_contactFlags = 0; + pt.m_linkIndexA = m_linkIndexA; + pt.m_linkIndexB = m_linkIndexB; + for (int j = 0; j < 3; j++) + { + pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + } + pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime; + // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; + m_cachedContactPoints.push_back(pt); } - pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime; - // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; - m_cachedContactPoints.push_back(pt); - return 1; + } }; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 4fdd9f1f2..13d616be6 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -94,9 +94,19 @@ struct TinyRendererVisualShapeConverterInternalData m_swWidth(START_WIDTH), m_swHeight(START_HEIGHT), m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB), + m_lightDirection(btVector3(-5,200,-40)), m_hasLightDirection(false), + m_lightColor(btVector3(1.0,1.0,1.0)), m_hasLightColor(false), - m_hasShadow(false) + m_lightDistance(2.0), + m_hasLightDistance(false), + m_lightAmbientCoeff(0.6), + m_hasLightAmbientCoeff(false), + m_lightDiffuseCoeff(0.35), + m_hasLightDiffuseCoeff(false), + m_lightSpecularCoeff(0.05), + m_hasLightSpecularCoeff(false), + m_hasShadow(false) { m_depthBuffer.resize(m_swWidth*m_swHeight); m_shadowBuffer.resize(m_swWidth*m_swHeight); diff --git a/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp index 864df26e9..89522d7b8 100644 --- a/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ b/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -19,8 +19,10 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" -btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape () +btCapsuleShape::btCapsuleShape(btScalar orgRadius, btScalar height) : btConvexInternalShape () { + m_collisionMargin = orgRadius; + btScalar radius = 0.f; m_shapeType = CAPSULE_SHAPE_PROXYTYPE; m_upAxis = 1; m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); @@ -133,7 +135,7 @@ void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) con btVector3 halfExtents(radius,radius,radius); halfExtents[getUpAxis()]+=getHalfHeight(); - btScalar margin = CONVEX_DISTANCE_MARGIN; + btScalar margin = m_collisionMargin; btScalar lx=btScalar(2.)*(halfExtents[0]+margin); btScalar ly=btScalar(2.)*(halfExtents[1]+margin); @@ -149,8 +151,10 @@ void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) con } -btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height) +btCapsuleShapeX::btCapsuleShapeX(btScalar orgRadius,btScalar height) { + m_collisionMargin = orgRadius; + btScalar radius = 0.f; m_upAxis = 0; m_implicitShapeDimensions.setValue(0.5f*height, radius,radius); } @@ -160,8 +164,10 @@ btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height) -btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height) +btCapsuleShapeZ::btCapsuleShapeZ(btScalar orgRadius,btScalar height) { + m_collisionMargin = orgRadius; + btScalar radius = 0.f; m_upAxis = 2; m_implicitShapeDimensions.setValue(radius,radius,0.5f*height); }