diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index 916a82343..a9e273f02 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -94,8 +94,8 @@ struct GUIHelperInterface int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied){} - virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])=0; - virtual void setProjectiveTexture(bool useProjectiveTexture)=0; + virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]){} + virtual void setProjectiveTexture(bool useProjectiveTexture){} virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4352fd5ba..f1e1b057e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -150,16 +150,16 @@ struct InternalVisualShapeData int m_tinyRendererVisualShapeIndex; int m_OpenGLGraphicsIndex; - UrdfVisual m_visualShape; - btTransform m_localInertiaFrame; - std::string m_pathPrefix; + b3AlignedObjectArray m_visualShapes; + + b3AlignedObjectArray m_pathPrefixes; void clear() { - m_tinyRendererVisualShapeIndex = 0; - m_OpenGLGraphicsIndex = 0; - m_localInertiaFrame.setIdentity(); - m_pathPrefix = ""; + m_tinyRendererVisualShapeIndex = -1; + m_OpenGLGraphicsIndex = -1; + m_visualShapes.clear(); + m_pathPrefixes.clear(); } }; @@ -1921,10 +1921,13 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]); if (visHandle) { - if (visHandle->m_visualShape.m_geometry.m_hasLocalMaterial) + for (int i=0;im_visualShapes.size();i++) { - matCol = visHandle->m_visualShape.m_geometry.m_localMaterial.m_matColor; - return true; + if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial) + { + matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor; + return true; + } } } } @@ -2092,39 +2095,91 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface b3Assert(0); } - ///quick hack: need to rethink the API/dependencies of this - virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const + virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const { + int graphicsIndex = -1; + double globalScaling = 1.f;//todo! + int flags=0; + BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags); + u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); + + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + btTransform startTrans; startTrans.setIdentity(); + btAlignedObjectArray textures; + if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0) { - const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]); + InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]); if (visHandle) { - - return visHandle->m_OpenGLGraphicsIndex; + if (visHandle->m_OpenGLGraphicsIndex>=0) + { + //instancing. assume the inertial frame is identical + graphicsIndex = visHandle->m_OpenGLGraphicsIndex; + } else + { + for (int v = 0;vm_visualShapes.size();v++) + { + u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse()*visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures); + } + + if (vertices.size() && indices.size()) + { + if (1) + { + int textureIndex = -1; + if (textures.size()) + { + + textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height); + } + + { + B3_PROFILE("registerGraphicsShape"); + graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); + visHandle->m_OpenGLGraphicsIndex = graphicsIndex; + } + } + } + } } + } - return -1; + return graphicsIndex; } virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const { //if there is a visual, use it, otherwise convert collision shape back into UrdfCollision... - - - UrdfModel model;// = m_data->m_urdfParser.getModel(); UrdfLink link; - int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex]; - if (colShapeUniqueId>=0) + + if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0) { - InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId); - if (handle) + const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]); + if (visHandle) { - for (int i=0;im_urdfCollisionObjects.size();i++) + for (int i=0;im_visualShapes.size();i++) { - link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]); + link.m_visualArray.push_back(visHandle->m_visualShapes[i]); + } + } + } + + if (link.m_visualArray.size()==0) + { + int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex]; + if (colShapeUniqueId>=0) + { + InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId); + if (handle) + { + for (int i=0;im_urdfCollisionObjects.size();i++) + { + link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]); + } } } } @@ -3968,19 +4023,16 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); btTransform localInertiaFrame; localInertiaFrame.setIdentity(); - btTransform childTrans; - childTrans.setIdentity(); + const char* pathPrefix = ""; - - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - btTransform startTrans; startTrans.setIdentity(); - btAlignedObjectArray textures; + int visualShapeUniqueId = -1; + UrdfVisual visualShape; for (int userShapeIndex = 0; userShapeIndex< clientCmd.m_createUserShapeArgs.m_numUserShapes; userShapeIndex++) { - + btTransform childTrans; + childTrans.setIdentity(); visualShape.m_geometry.m_type = (UrdfGeomTypes)clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_type; char relativeFileName[1024]; char pathPrefix[1024]; @@ -4106,41 +4158,23 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct } - u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices, textures); - - } - - if (vertices.size() && indices.size()) - { - if (1) + if (visualShapeUniqueId<0) { - int textureIndex = -1; - if (textures.size()) - { - - textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height); - } - int graphicsIndex = -1; - { - B3_PROFILE("registerGraphicsShape"); - graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); - if (graphicsIndex >= 0) - { - int visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle(); - InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId); - visualHandle->m_OpenGLGraphicsIndex = graphicsIndex; - visualHandle->m_tinyRendererVisualShapeIndex = -1; - //tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance - //store needed info for tinyrenderer - visualHandle->m_localInertiaFrame = localInertiaFrame; - visualHandle->m_visualShape = visualShape; - visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : ""; - - serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId; - serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED; - } - } + visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle(); } + InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId); + visualHandle->m_OpenGLGraphicsIndex = -1; + visualHandle->m_tinyRendererVisualShapeIndex = -1; + //tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance + //store needed info for tinyrenderer + + visualShape.m_linkLocalFrame = childTrans; + visualHandle->m_visualShapes.push_back(visualShape); + visualHandle->m_pathPrefixes.push_back(pathPrefix[0] ? pathPrefix : ""); + + serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId; + serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED; + } return hasStatus; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 8c9c67f85..c853aed77 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -64,31 +64,31 @@ protected: void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.); void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.); btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f); - - void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, + + void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2); static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode); - void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, + void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction unsigned long m_btSeed2; - + btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold); virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); @@ -96,7 +96,7 @@ protected: void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal); - void convertJoint(btSolverConstraint* destConstraintRow, btTypedConstraint* srcConstraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal); + void convertJoint(btSolverConstraint* currentConstraintRow, btTypedConstraint* constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal); virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); @@ -122,9 +122,9 @@ protected: { return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); } - + protected: - + void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); @@ -139,15 +139,15 @@ protected: public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btSequentialImpulseConstraintSolver(); virtual ~btSequentialImpulseConstraintSolver(); virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); - + ///clear internal cached data and reset random seed virtual void reset(); - + unsigned long btRand2(); int btRandInt2 (int n); @@ -161,7 +161,7 @@ public: return m_btSeed2; } - + virtual btConstraintSolverType getSolverType() const { return BT_SEQUENTIAL_IMPULSE_SOLVER; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp index 4ccf7b247..82a719a3e 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp @@ -537,7 +537,7 @@ void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersist BT_PROFILE( "allocAllContactConstraints" ); btAlignedObjectArray cachedInfoArray; // = m_manifoldCachedInfoArray; cachedInfoArray.resizeNoInitialize( numManifolds ); - if (false) + if (/* DISABLES CODE */ (false)) { // sequential internalCollectContactManifoldCachedInfo(&cachedInfoArray[ 0 ], manifoldPtr, numManifolds, infoGlobal); diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h index 0577d8d2d..55d53474c 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -127,7 +127,7 @@ protected: public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btSequentialImpulseConstraintSolverMt(); virtual ~btSequentialImpulseConstraintSolverMt(); @@ -136,11 +136,11 @@ public: btScalar resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ); btScalar resolveMultipleContactFrictionConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ); btScalar resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ); - btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ); + btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray& contactIndices, int batchBegin, int batchEnd ); - void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifold, int numManifolds, const btContactSolverInfo& infoGlobal); + void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds); - void internalSetupContactConstraints(int iContact, const btContactSolverInfo& infoGlobal); + void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal); void internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);