Merge pull request #1619 from erwincoumans/master
PyBullet fix createVisualShape (inertial frame related), minor backward compatibility fix
This commit is contained in:
erwincoumans
GitHub
@@ -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;
|
||||
|
||||
|
||||
@@ -150,16 +150,16 @@ struct InternalVisualShapeData
|
||||
int m_tinyRendererVisualShapeIndex;
|
||||
int m_OpenGLGraphicsIndex;
|
||||
|
||||
UrdfVisual m_visualShape;
|
||||
btTransform m_localInertiaFrame;
|
||||
std::string m_pathPrefix;
|
||||
b3AlignedObjectArray<UrdfVisual> m_visualShapes;
|
||||
|
||||
b3AlignedObjectArray<std::string> 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;i<visHandle->m_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<GLInstanceVertex> vertices;
|
||||
btAlignedObjectArray<int> indices;
|
||||
btTransform startTrans; startTrans.setIdentity();
|
||||
btAlignedObjectArray<BulletURDFTexture> 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;v<visHandle->m_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;i<handle->m_urdfCollisionObjects.size();i++)
|
||||
for (int i=0;i<visHandle->m_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;i<handle->m_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<GLInstanceVertex> vertices;
|
||||
btAlignedObjectArray<int> indices;
|
||||
btTransform startTrans; startTrans.setIdentity();
|
||||
btAlignedObjectArray<BulletURDFTexture> 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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -537,7 +537,7 @@ void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersist
|
||||
BT_PROFILE( "allocAllContactConstraints" );
|
||||
btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray; // = m_manifoldCachedInfoArray;
|
||||
cachedInfoArray.resizeNoInitialize( numManifolds );
|
||||
if (false)
|
||||
if (/* DISABLES CODE */ (false))
|
||||
{
|
||||
// sequential
|
||||
internalCollectContactManifoldCachedInfo(&cachedInfoArray[ 0 ], manifoldPtr, numManifolds, infoGlobal);
|
||||
|
||||
@@ -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<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
|
||||
btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& 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);
|
||||
|
||||
Reference in New Issue
Block a user