diff --git a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h index d7ae1d699..6e7afe6f4 100644 --- a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h +++ b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h @@ -18,7 +18,7 @@ struct UrdfRenderingInterface ///use the visualShapeUniqueId as a unique identifier to synchronize the world transform and to remove the visual shape. virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int visualShapeUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0; - virtual int registerShapeAndInstance(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex)=0; + virtual int registerShapeAndInstance(const struct b3VisualShapeData& visualShape, const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex)=0; virtual void updateShape(int shapeUniqueId, const btVector3* vertices, int numVertices) = 0; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 663d1f64a..6c933b190 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1640,6 +1640,7 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_collisionShapes; btAlignedObjectArray m_heightfieldDatas; btAlignedObjectArray m_allocatedTextures; + btAlignedObjectArray m_allocatedTexturesRequireFree; btHashMap m_bulletCollisionShape2UrdfCollision; btAlignedObjectArray m_meshInterfaces; @@ -2530,7 +2531,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface B3_PROFILE("free textureData"); if (!textures[i].m_isCached) { - free(textures[i].textureData1); + m_data->m_allocatedTexturesRequireFree.push_back(textures[i].textureData1); } } @@ -2978,8 +2979,15 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() m_data->m_guiHelper->removeTexture(texId); } } + + for (int i = 0; i < m_data->m_allocatedTexturesRequireFree.size(); i++) + { + //we can't free them right away, due to caching based on memory pointer in PhysicsServerExample + free(m_data->m_allocatedTexturesRequireFree[i]); + } m_data->m_heightfieldDatas.clear(); m_data->m_allocatedTextures.clear(); + m_data->m_allocatedTexturesRequireFree.clear(); m_data->m_meshInterfaces.clear(); m_data->m_collisionShapes.clear(); m_data->m_bulletCollisionShape2UrdfCollision.clear(); @@ -8839,13 +8847,47 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo bodyHandle->m_softBody = psb; psb->setUserIndex2(*bodyUniqueId); + b3VisualShapeData visualShape; + + visualShape.m_objectUniqueId = *bodyUniqueId; + visualShape.m_linkIndex = -1; + visualShape.m_visualGeometryType = URDF_GEOM_MESH; + //dimensions just contains the scale + visualShape.m_dimensions[0] = 1; + visualShape.m_dimensions[1] = 1; + visualShape.m_dimensions[2] = 1; + //filename + strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN); + visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0; + //position and orientation + visualShape.m_localVisualFrame[0] = 0; + visualShape.m_localVisualFrame[1] = 0; + visualShape.m_localVisualFrame[2] = 0; + visualShape.m_localVisualFrame[3] = 0; + visualShape.m_localVisualFrame[4] = 0; + visualShape.m_localVisualFrame[5] = 0; + visualShape.m_localVisualFrame[6] = 1; + //color and ids to be set by the renderer + visualShape.m_rgbaColor[0] = 1; + visualShape.m_rgbaColor[1] = 1; + visualShape.m_rgbaColor[2] = 1; + visualShape.m_rgbaColor[3] = 1; + visualShape.m_tinyRendererTextureId = -1; + visualShape.m_textureUniqueId = -1; + visualShape.m_openglTextureId = -1; + if (meshData.m_gfxShape) { - int texUid1 = m_data->m_guiHelper->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight); + int texUid1 = -1; + if (meshData.m_textureHeight > 0 && meshData.m_textureWidth > 0 && meshData.m_textureImage1) + { + texUid1 = m_data->m_guiHelper->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight); + } + visualShape.m_openglTextureId = texUid1; int shapeUid1 = m_data->m_guiHelper->registerGraphicsShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], meshData.m_gfxShape->m_numvertices, &meshData.m_gfxShape->m_indices->at(0), meshData.m_gfxShape->m_numIndices, B3_GL_TRIANGLES, texUid1); psb->getCollisionShape()->setUserIndex(shapeUid1); - float position[4] = { 0,0,0,0 }; - float orientation [4] = { 0,0,0,1 }; + float position[4] = { 0,0,0,1 }; + float orientation[4] = { 0,0,0,1 }; float color[4] = { 1,1,1,1 }; float scaling[4] = { 1,1,1,1 }; int instanceUid = m_data->m_guiHelper->registerGraphicsInstance(shapeUid1, position, orientation, color, scaling); @@ -8854,8 +8896,10 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo if (m_data->m_enableTinyRenderer) { int texUid2 = m_data->m_pluginManager.getRenderInterface()->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight); + visualShape.m_tinyRendererTextureId = texUid2; int linkIndex = -1; int softBodyGraphicsShapeUid = m_data->m_pluginManager.getRenderInterface()->registerShapeAndInstance( + visualShape, &meshData.m_gfxShape->m_vertices->at(0).xyzw[0], meshData.m_gfxShape->m_numvertices, &meshData.m_gfxShape->m_indices->at(0), @@ -8928,6 +8972,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo } int texId = m_data->m_guiHelper->registerTexture(&texels[0], texWidth, texHeight); + visualShape.m_openglTextureId = texId; int shapeId = m_data->m_guiHelper->registerGraphicsShape(&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, texId); b3Assert(shapeId >= 0); psb->getCollisionShape()->setUserIndex(shapeId); @@ -8935,8 +8980,10 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo { int texUid2 = m_data->m_pluginManager.getRenderInterface()->registerTexture(&texels[0], texWidth, texHeight); + visualShape.m_tinyRendererTextureId = texUid2; int linkIndex = -1; int softBodyGraphicsShapeUid = m_data->m_pluginManager.getRenderInterface()->registerShapeAndInstance( + visualShape, &gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, texUid2, psb->getBroadphaseHandle()->getUid(), *bodyUniqueId, @@ -9410,456 +9457,456 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc clientCmd.m_changeDynamicsInfoArgs.m_anisotropicFriction[1], clientCmd.m_changeDynamicsInfoArgs.m_anisotropicFriction[2]); - btAssert(bodyUniqueId >= 0); - - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - if (body && body->m_multiBody) + if (bodyUniqueId >= 0) { - btMultiBody* mb = body->m_multiBody; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) + if (body && body->m_multiBody) { - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) - { - mb->wakeUp(); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) - { - mb->goToSleep(); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) - { - mb->setCanSleep(true); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) - { - mb->setCanSleep(false); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableWakeup) - { - mb->setCanWakeup(true); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableWakeup) - { - mb->setCanWakeup(false); - } - } + btMultiBody* mb = body->m_multiBody; - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) - { - mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) - { - mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); - } - - if (linkIndex == -1) - { - if (mb->getBaseCollider()) + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) { - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) { - mb->getBaseCollider()->setRestitution(restitution); + mb->wakeUp(); } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) { - mb->getBaseCollider()->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + mb->goToSleep(); } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) { - mb->getBaseCollider()->setFriction(lateralFriction); + mb->setCanSleep(true); } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) { - mb->getBaseCollider()->setSpinningFriction(spinningFriction); + mb->setCanSleep(false); } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableWakeup) { - mb->getBaseCollider()->setRollingFriction(rollingFriction); + mb->setCanWakeup(true); } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableWakeup) { - if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) - { - mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } - else - { - mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } + mb->setCanWakeup(false); } } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) - { - mb->setBaseMass(mass); - if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape()) - { - btVector3 localInertia; - mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass, localInertia); - mb->setBaseInertia(localInertia); - } - //handle switch from static/fixedBase to dynamic and vise-versa - if (mass > 0) - { - bool isDynamic = true; - if (mb->hasFixedBase()) - { - int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); - int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider()); - int oldFlags = mb->getBaseCollider()->getCollisionFlags(); - mb->getBaseCollider()->setCollisionFlags(oldFlags & ~btCollisionObject::CF_STATIC_OBJECT); - mb->setFixedBase(false); - m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask); - } - } - else - { - if (!mb->hasFixedBase()) - { - bool isDynamic = false; - int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); - int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - int oldFlags = mb->getBaseCollider()->getCollisionFlags(); - mb->getBaseCollider()->setCollisionFlags(oldFlags | btCollisionObject::CF_STATIC_OBJECT); - m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider()); - mb->setFixedBase(true); - m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask); - } - } - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) { - mb->setBaseInertia(newLocalInertiaDiagonal); + mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping); } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION) + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) { - mb->getBaseCollider()->setAnisotropicFriction(anisotropicFriction); + mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY) + if (linkIndex == -1) { - mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN) - { - mb->getBaseCollider()->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); - if (mb->getBaseCollider()->getCollisionShape()->isCompound()) - { - btCompoundShape* compound = (btCompoundShape*)mb->getBaseCollider()->getCollisionShape(); - for (int s = 0; s < compound->getNumChildShapes(); s++) - { - compound->getChildShape(s)->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); - } - } - } - } - else - { - if (linkIndex >= 0 && linkIndex < mb->getNumLinks()) - { - - if ((clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE) || - (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS)) - { - - btMultiBodyJointLimitConstraint* limC = 0; - - int numConstraints = m_data->m_dynamicsWorld->getNumMultiBodyConstraints(); - for (int c = 0; c < numConstraints; c++) - { - btMultiBodyConstraint* mbc = m_data->m_dynamicsWorld->getMultiBodyConstraint(c); - if (mbc->getConstraintType() == MULTIBODY_CONSTRAINT_LIMIT) - { - if (((mbc->getMultiBodyA() == mb) && (mbc->getLinkA() == linkIndex)) - || - ((mbc->getMultiBodyB() == mb) && ((mbc->getLinkB() == linkIndex))) - ) - { - limC = (btMultiBodyJointLimitConstraint*)mbc; - } - } - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS) - { - //find a joint limit - btScalar prevUpper = mb->getLink(linkIndex).m_jointUpperLimit; - btScalar prevLower = mb->getLink(linkIndex).m_jointLowerLimit; - btScalar lower = clientCmd.m_changeDynamicsInfoArgs.m_jointLowerLimit; - btScalar upper = clientCmd.m_changeDynamicsInfoArgs.m_jointUpperLimit; - bool enableLimit = lower <= upper; - - if (enableLimit) - { - if (limC == 0) - { - limC = new btMultiBodyJointLimitConstraint(mb, linkIndex, lower, upper); - m_data->m_dynamicsWorld->addMultiBodyConstraint(limC); - } - else - { - limC->setLowerBound(lower); - limC->setUpperBound(upper); - } - mb->getLink(linkIndex).m_jointLowerLimit = lower; - mb->getLink(linkIndex).m_jointUpperLimit = upper; - } - else - { - if (limC) - { - m_data->m_dynamicsWorld->removeMultiBodyConstraint(limC); - delete limC; - limC = 0; - } - mb->getLink(linkIndex).m_jointLowerLimit = 1; - mb->getLink(linkIndex).m_jointUpperLimit = -1; - } - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE) - { - btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime; - btScalar maxImpulse = clientCmd.m_changeDynamicsInfoArgs.m_jointLimitForce * fixedTimeSubStep; - if (limC) - { - //convert from force to impulse - limC->setMaxAppliedImpulse(maxImpulse); - } - } - } - - if (mb->getLinkCollider(linkIndex)) + if (mb->getBaseCollider()) { if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) { - mb->getLinkCollider(linkIndex)->setRestitution(restitution); + mb->getBaseCollider()->setRestitution(restitution); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + { + mb->getBaseCollider()->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + mb->getBaseCollider()->setFriction(lateralFriction); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) { - mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction); + mb->getBaseCollider()->setSpinningFriction(spinningFriction); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) { - mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction); + mb->getBaseCollider()->setRollingFriction(rollingFriction); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) { if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) { - mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); } else { - mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); } } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) - { - mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) - { - mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN) - { - mb->getLinkCollider(linkIndex)->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); - } } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING) - { - mb->getLink(linkIndex).m_jointDamping = clientCmd.m_changeDynamicsInfoArgs.m_jointDamping; - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) { - mb->getLink(linkIndex).m_mass = mass; - if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape()) + mb->setBaseMass(mass); + if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape()) { btVector3 localInertia; - mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass, localInertia); - mb->getLink(linkIndex).m_inertiaLocal = localInertia; + mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass, localInertia); + mb->setBaseInertia(localInertia); + } + + //handle switch from static/fixedBase to dynamic and vise-versa + if (mass > 0) + { + bool isDynamic = true; + if (mb->hasFixedBase()) + { + int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); + int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider()); + int oldFlags = mb->getBaseCollider()->getCollisionFlags(); + mb->getBaseCollider()->setCollisionFlags(oldFlags & ~btCollisionObject::CF_STATIC_OBJECT); + mb->setFixedBase(false); + m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask); + } + } + else + { + if (!mb->hasFixedBase()) + { + bool isDynamic = false; + int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); + int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + int oldFlags = mb->getBaseCollider()->getCollisionFlags(); + mb->getBaseCollider()->setCollisionFlags(oldFlags | btCollisionObject::CF_STATIC_OBJECT); + m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider()); + mb->setFixedBase(true); + m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask); + } } } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) { - mb->getLink(linkIndex).m_inertiaLocal = newLocalInertiaDiagonal; + mb->setBaseInertia(newLocalInertiaDiagonal); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION) { - mb->getLinkCollider(linkIndex)->setAnisotropicFriction(anisotropicFriction); + mb->getBaseCollider()->setAnisotropicFriction(anisotropicFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY) + { + mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN) + { + mb->getBaseCollider()->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); + if (mb->getBaseCollider()->getCollisionShape()->isCompound()) + { + btCompoundShape* compound = (btCompoundShape*)mb->getBaseCollider()->getCollisionShape(); + for (int s = 0; s < compound->getNumChildShapes(); s++) + { + compound->getChildShape(s)->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); + } + } } - } - } - } - else - { - btRigidBody* rb = 0; - if (body && body->m_rigidBody) - { - if (linkIndex == -1) - { - rb = body->m_rigidBody; } else { - if (linkIndex >= 0 && linkIndex < body->m_rigidBodyJoints.size()) + if (linkIndex >= 0 && linkIndex < mb->getNumLinks()) { - btRigidBody* parentRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyA(); - btRigidBody* childRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyB(); - rb = childRb; + + if ((clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE) || + (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS)) + { + + btMultiBodyJointLimitConstraint* limC = 0; + + int numConstraints = m_data->m_dynamicsWorld->getNumMultiBodyConstraints(); + for (int c = 0; c < numConstraints; c++) + { + btMultiBodyConstraint* mbc = m_data->m_dynamicsWorld->getMultiBodyConstraint(c); + if (mbc->getConstraintType() == MULTIBODY_CONSTRAINT_LIMIT) + { + if (((mbc->getMultiBodyA() == mb) && (mbc->getLinkA() == linkIndex)) + || + ((mbc->getMultiBodyB() == mb) && ((mbc->getLinkB() == linkIndex))) + ) + { + limC = (btMultiBodyJointLimitConstraint*)mbc; + } + } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS) + { + //find a joint limit + btScalar prevUpper = mb->getLink(linkIndex).m_jointUpperLimit; + btScalar prevLower = mb->getLink(linkIndex).m_jointLowerLimit; + btScalar lower = clientCmd.m_changeDynamicsInfoArgs.m_jointLowerLimit; + btScalar upper = clientCmd.m_changeDynamicsInfoArgs.m_jointUpperLimit; + bool enableLimit = lower <= upper; + + if (enableLimit) + { + if (limC == 0) + { + limC = new btMultiBodyJointLimitConstraint(mb, linkIndex, lower, upper); + m_data->m_dynamicsWorld->addMultiBodyConstraint(limC); + } + else + { + limC->setLowerBound(lower); + limC->setUpperBound(upper); + } + mb->getLink(linkIndex).m_jointLowerLimit = lower; + mb->getLink(linkIndex).m_jointUpperLimit = upper; + } + else + { + if (limC) + { + m_data->m_dynamicsWorld->removeMultiBodyConstraint(limC); + delete limC; + limC = 0; + } + mb->getLink(linkIndex).m_jointLowerLimit = 1; + mb->getLink(linkIndex).m_jointUpperLimit = -1; + } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE) + { + btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime; + btScalar maxImpulse = clientCmd.m_changeDynamicsInfoArgs.m_jointLimitForce * fixedTimeSubStep; + if (limC) + { + //convert from force to impulse + limC->setMaxAppliedImpulse(maxImpulse); + } + } + } + + if (mb->getLinkCollider(linkIndex)) + { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + { + mb->getLinkCollider(linkIndex)->setRestitution(restitution); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + { + mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + { + mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) + { + mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + else + { + mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + { + mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN) + { + mb->getLinkCollider(linkIndex)->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); + } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING) + { + mb->getLink(linkIndex).m_jointDamping = clientCmd.m_changeDynamicsInfoArgs.m_jointDamping; + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) + { + mb->getLink(linkIndex).m_mass = mass; + if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape()) + { + btVector3 localInertia; + mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass, localInertia); + mb->getLink(linkIndex).m_inertiaLocal = localInertia; + } + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) + { + mb->getLink(linkIndex).m_inertiaLocal = newLocalInertiaDiagonal; + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION) + { + mb->getLinkCollider(linkIndex)->setAnisotropicFriction(anisotropicFriction); + } } } } - - if (rb) + else { - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) + btRigidBody* rb = 0; + if (body && body->m_rigidBody) { - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) + if (linkIndex == -1) { - rb->forceActivationState(ACTIVE_TAG); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) - { - rb->forceActivationState(DISABLE_DEACTIVATION); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) - { - rb->forceActivationState(ACTIVE_TAG); - rb->setDeactivationTime(0.0); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) - { - rb->forceActivationState(ISLAND_SLEEPING); - } - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) - { - btScalar angDamping = rb->getAngularDamping(); - rb->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) - { - btScalar linDamping = rb->getLinearDamping(); - rb->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) - { - rb->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) - { - rb->setRestitution(restitution); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) - { - rb->setFriction(lateralFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) - { - rb->setSpinningFriction(spinningFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) - { - rb->setRollingFriction(rollingFriction); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) - { - if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) - { - rb->setCollisionFlags(rb->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + rb = body->m_rigidBody; } else { - rb->setCollisionFlags(rb->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + if (linkIndex >= 0 && linkIndex < body->m_rigidBodyJoints.size()) + { + btRigidBody* parentRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyA(); + btRigidBody* childRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyB(); + rb = childRb; + } } } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) + if (rb) { - btVector3 localInertia; - if (rb->getCollisionShape()) + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) { - rb->getCollisionShape()->calculateLocalInertia(mass, localInertia); + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) + { + rb->forceActivationState(ACTIVE_TAG); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) + { + rb->forceActivationState(DISABLE_DEACTIVATION); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) + { + rb->forceActivationState(ACTIVE_TAG); + rb->setDeactivationTime(0.0); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) + { + rb->forceActivationState(ISLAND_SLEEPING); + } } - rb->setMassProps(mass, localInertia); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) - { - btScalar orgMass = rb->getInvMass(); - if (orgMass > 0) + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) { - rb->setMassProps(mass, newLocalInertiaDiagonal); + btScalar angDamping = rb->getAngularDamping(); + rb->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) + { + btScalar linDamping = rb->getLinearDamping(); + rb->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); } - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION) - { - rb->setAnisotropicFriction(anisotropicFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD) - { - rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); - } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + { + rb->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + { + rb->setRestitution(restitution); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + rb->setFriction(lateralFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + { + rb->setSpinningFriction(spinningFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + { + rb->setRollingFriction(rollingFriction); + } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS) - { - rb->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius); - //for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled - rb->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN) - { - rb->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) + { + rb->setCollisionFlags(rb->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + else + { + rb->setCollisionFlags(rb->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) + { + btVector3 localInertia; + if (rb->getCollisionShape()) + { + rb->getCollisionShape()->calculateLocalInertia(mass, localInertia); + } + rb->setMassProps(mass, localInertia); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) + { + btScalar orgMass = rb->getInvMass(); + if (orgMass > 0) + { + rb->setMassProps(mass, newLocalInertiaDiagonal); + } + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION) + { + rb->setAnisotropicFriction(anisotropicFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD) + { + rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS) + { + rb->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius); + //for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled + rb->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN) + { + rb->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin); + } } } - } #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - if (body && body->m_softBody) - { - btSoftBody* psb = body->m_softBody; - if (psb) + if (body && body->m_softBody) { - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) + btSoftBody* psb = body->m_softBody; + if (psb) { - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) { - psb->forceActivationState(ACTIVE_TAG); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) - { - psb->forceActivationState(DISABLE_DEACTIVATION); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) - { - psb->forceActivationState(ACTIVE_TAG); - psb->setDeactivationTime(0.0); - } - if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) - { - psb->forceActivationState(ISLAND_SLEEPING); + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) + { + psb->forceActivationState(ACTIVE_TAG); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) + { + psb->forceActivationState(DISABLE_DEACTIVATION); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) + { + psb->forceActivationState(ACTIVE_TAG); + psb->setDeactivationTime(0.0); + } + if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) + { + psb->forceActivationState(ISLAND_SLEEPING); + } } } } - } #endif - + } SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; @@ -13376,7 +13423,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share if (imageData) { texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData, width, height); - free(imageData); + m_data->m_allocatedTexturesRequireFree.push_back(imageData); } else { diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp index a23f551e1..17d86c8eb 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp @@ -915,10 +915,10 @@ static btVector4 sColors[4] = btVector4(72. / 256., 133. / 256., 237. / 256., 1), }; -int EGLRendererVisualShapeConverter::registerShapeAndInstance(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex) +int EGLRendererVisualShapeConverter::registerShapeAndInstance(const b3VisualShapeData& visualShape, const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex) { int uniqueId = orgGraphicsUniqueId; - float rgbaColor[4] = { 1,1,1,1 }; + float rgbaColor[4] = { visualShape.m_rgbaColor[0],visualShape.m_rgbaColor[1],visualShape.m_rgbaColor[2],visualShape.m_rgbaColor[3] }; EGLRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[uniqueId]; if (visualsPtr == 0) @@ -945,16 +945,15 @@ int EGLRendererVisualShapeConverter::registerShapeAndInstance(const float* verti int shapeIndex = m_data->m_instancingRenderer->registerShape(&vertices[0], numvertices, &indices[0], numIndices, B3_GL_TRIANGLES, innerTexId); - double scaling[3] = { 1, 1, 1 }; - double color[4] = { 1,1,1,1 }; - double position[4] = { 0,0,0,1 }; - double orn[4] = { 0,0,0,1 }; + double scaling[3] = { visualShape.m_dimensions[0], visualShape.m_dimensions[1],visualShape.m_dimensions[2] }; + double position[4] = { visualShape.m_localVisualFrame[0],visualShape.m_localVisualFrame[1],visualShape.m_localVisualFrame[2],1 }; + double orn[4] = { visualShape.m_localVisualFrame[3],visualShape.m_localVisualFrame[4],visualShape.m_localVisualFrame[5],visualShape.m_localVisualFrame[6]}; int graphicsIndex = m_data->m_instancingRenderer->registerGraphicsInstance( shapeIndex, position, orn, - color, + visualShape.m_rgbaColor, scaling); int segmentationMask = bodyUniqueId + ((linkIndex + 1) << 24); @@ -980,6 +979,8 @@ int EGLRendererVisualShapeConverter::registerShapeAndInstance(const float* verti visuals->m_vertices[v] = orgVertices[v]; } } + + m_data->m_visualShapes.push_back(visualShape); return uniqueId; } diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h index 19216fede..0414c1d20 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h @@ -17,7 +17,7 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData); - virtual int registerShapeAndInstance(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex); + virtual int registerShapeAndInstance(const b3VisualShapeData& visualShape, const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex); virtual void updateShape(int shapeUniqueId, const btVector3* vertices, int numVertices); diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index b2ea2bab6..ef68b9613 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -2,8 +2,8 @@ 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. @@ -68,7 +68,7 @@ struct TinyRendererVisualShapeConverterInternalData btAlignedObjectArray m_checkeredTexels; - + int m_upAxis; int m_swWidth; int m_swHeight; @@ -95,23 +95,23 @@ struct TinyRendererVisualShapeConverterInternalData TinyRendererVisualShapeConverterInternalData() : m_upAxis(2), - 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_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_flags(0) + 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_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_flags(0) { m_depthBuffer.resize(m_swWidth * m_swHeight); m_shadowBuffer.resize(m_swWidth * m_swHeight); @@ -130,7 +130,7 @@ TinyRendererVisualShapeConverter::TinyRendererVisualShapeConverter() float dist = 1.5; float pitch = -10; float yaw = -80; - float targetPos[3] = {0, 0, 0}; + float targetPos[3] = { 0, 0, 0 }; m_data->m_camera.setCameraUpAxis(m_data->m_upAxis); resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } @@ -208,418 +208,419 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa switch (visual->m_geometry.m_type) { - case URDF_GEOM_CYLINDER: - case URDF_GEOM_CAPSULE: + case URDF_GEOM_CYLINDER: + case URDF_GEOM_CAPSULE: + { + btVector3 p1 = visual->m_geometry.m_capsuleFrom; + btVector3 p2 = visual->m_geometry.m_capsuleTo; + btTransform tr; + tr.setIdentity(); + btScalar rad, len; + btVector3 center(0, 0, 0); + btVector3 axis(0, 0, 1); + btAlignedObjectArray vertices; + int numSteps = 32; + + if (visual->m_geometry.m_hasFromTo) { - btVector3 p1 = visual->m_geometry.m_capsuleFrom; - btVector3 p2 = visual->m_geometry.m_capsuleTo; - btTransform tr; - tr.setIdentity(); - btScalar rad, len; - btVector3 center(0, 0, 0); - btVector3 axis(0, 0, 1); - btAlignedObjectArray vertices; - int numSteps = 32; + btVector3 v = p2 - p1; + btVector3 dir = v.normalized(); + tr = visual->m_linkLocalFrame; + len = v.length(); + rad = visual->m_geometry.m_capsuleRadius; + btVector3 ax1, ax2; + btPlaneSpace1(dir, ax1, ax2); - if (visual->m_geometry.m_hasFromTo) + for (int i = 0; i < numSteps; i++) { - btVector3 v = p2 - p1; - btVector3 dir = v.normalized(); - tr = visual->m_linkLocalFrame; - len = v.length(); - rad = visual->m_geometry.m_capsuleRadius; - btVector3 ax1, ax2; - btPlaneSpace1(dir, ax1, ax2); - - for (int i = 0; i < numSteps; i++) { - { - btVector3 vert = p1 + ax1 * rad * btSin(SIMD_2_PI * (float(i) / numSteps)) + ax2 * rad * btCos(SIMD_2_PI * (float(i) / numSteps)); - vertices.push_back(vert); - } - { - btVector3 vert = p2 + ax1 * rad * btSin(SIMD_2_PI * (float(i) / numSteps)) + ax2 * rad * btCos(SIMD_2_PI * (float(i) / numSteps)); - vertices.push_back(vert); - } - } - if (visual->m_geometry.m_type == URDF_GEOM_CAPSULE) - { - btVector3 pole1 = p1 - dir * rad; - btVector3 pole2 = p2 + dir * rad; - vertices.push_back(pole1); - vertices.push_back(pole2); - } - } - else - { - //assume a capsule along the Z-axis, centered at the origin - tr = visual->m_linkLocalFrame; - len = visual->m_geometry.m_capsuleHeight; - rad = visual->m_geometry.m_capsuleRadius; - for (int i = 0; i < numSteps; i++) - { - btVector3 vert(rad * btSin(SIMD_2_PI * (float(i) / numSteps)), rad * btCos(SIMD_2_PI * (float(i) / numSteps)), len / 2.); - vertices.push_back(vert); - vert[2] = -len / 2.; + btVector3 vert = p1 + ax1 * rad * btSin(SIMD_2_PI * (float(i) / numSteps)) + ax2 * rad * btCos(SIMD_2_PI * (float(i) / numSteps)); vertices.push_back(vert); } - if (visual->m_geometry.m_type == URDF_GEOM_CAPSULE) { - btVector3 pole1(0, 0, +len / 2. + rad); - btVector3 pole2(0, 0, -len / 2. - rad); - vertices.push_back(pole1); - vertices.push_back(pole2); + btVector3 vert = p2 + ax1 * rad * btSin(SIMD_2_PI * (float(i) / numSteps)) + ax2 * rad * btCos(SIMD_2_PI * (float(i) / numSteps)); + vertices.push_back(vert); } } - visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0]; - visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1]; - visualShapeOut.m_localVisualFrame[2] = tr.getOrigin()[2]; - visualShapeOut.m_localVisualFrame[3] = tr.getRotation()[0]; - visualShapeOut.m_localVisualFrame[4] = tr.getRotation()[1]; - visualShapeOut.m_localVisualFrame[5] = tr.getRotation()[2]; - visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3]; - visualShapeOut.m_dimensions[0] = len; - visualShapeOut.m_dimensions[1] = rad; - - btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - //btCapsuleShape* cylZShape = new btCapsuleShape(rad,len);//btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - - cylZShape->setMargin(0.001); - convexColShape = cylZShape; - break; + if (visual->m_geometry.m_type == URDF_GEOM_CAPSULE) + { + btVector3 pole1 = p1 - dir * rad; + btVector3 pole2 = p2 + dir * rad; + vertices.push_back(pole1); + vertices.push_back(pole2); + } } - case URDF_GEOM_BOX: + else { - visualShapeOut.m_dimensions[0] = visual->m_geometry.m_boxSize[0]; - visualShapeOut.m_dimensions[1] = visual->m_geometry.m_boxSize[1]; - visualShapeOut.m_dimensions[2] = visual->m_geometry.m_boxSize[2]; + //assume a capsule along the Z-axis, centered at the origin + tr = visual->m_linkLocalFrame; + len = visual->m_geometry.m_capsuleHeight; + rad = visual->m_geometry.m_capsuleRadius; + for (int i = 0; i < numSteps; i++) + { + btVector3 vert(rad * btSin(SIMD_2_PI * (float(i) / numSteps)), rad * btCos(SIMD_2_PI * (float(i) / numSteps)), len / 2.); + vertices.push_back(vert); + vert[2] = -len / 2.; + vertices.push_back(vert); + } + if (visual->m_geometry.m_type == URDF_GEOM_CAPSULE) + { + btVector3 pole1(0, 0, +len / 2. + rad); + btVector3 pole2(0, 0, -len / 2. - rad); + vertices.push_back(pole1); + vertices.push_back(pole2); + } + } + visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0]; + visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1]; + visualShapeOut.m_localVisualFrame[2] = tr.getOrigin()[2]; + visualShapeOut.m_localVisualFrame[3] = tr.getRotation()[0]; + visualShapeOut.m_localVisualFrame[4] = tr.getRotation()[1]; + visualShapeOut.m_localVisualFrame[5] = tr.getRotation()[2]; + visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3]; + visualShapeOut.m_dimensions[0] = len; + visualShapeOut.m_dimensions[1] = rad; - btVector3 extents = visual->m_geometry.m_boxSize; + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + //btCapsuleShape* cylZShape = new btCapsuleShape(rad,len);//btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - - int strideInBytes = 9 * sizeof(float); - int numVertices = sizeof(cube_vertices_textured) / strideInBytes; - int numIndices = sizeof(cube_indices) / sizeof(int); + cylZShape->setMargin(0.001); + convexColShape = cylZShape; + break; + } + case URDF_GEOM_BOX: + { + visualShapeOut.m_dimensions[0] = visual->m_geometry.m_boxSize[0]; + visualShapeOut.m_dimensions[1] = visual->m_geometry.m_boxSize[1]; + visualShapeOut.m_dimensions[2] = visual->m_geometry.m_boxSize[2]; + btVector3 extents = visual->m_geometry.m_boxSize; + + + int strideInBytes = 9 * sizeof(float); + int numVertices = sizeof(cube_vertices_textured) / strideInBytes; + int numIndices = sizeof(cube_indices) / sizeof(int); + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + glmesh->m_indices->resize(numIndices); + for (int k = 0; k < numIndices; k++) + { + glmesh->m_indices->at(k) = cube_indices[k]; + } + glmesh->m_vertices->resize(numVertices); + + btScalar halfExtentsX = extents[0] * 0.5; + btScalar halfExtentsY = extents[1] * 0.5; + btScalar halfExtentsZ = extents[2] * 0.5; + GLInstanceVertex* verts = &glmesh->m_vertices->at(0); + btScalar textureScaling = 1; + + for (int i = 0; i < numVertices; i++) + { + + verts[i].xyzw[0] = halfExtentsX * cube_vertices_textured[i * 9]; + verts[i].xyzw[1] = halfExtentsY * cube_vertices_textured[i * 9 + 1]; + verts[i].xyzw[2] = halfExtentsZ * cube_vertices_textured[i * 9 + 2]; + verts[i].xyzw[3] = cube_vertices_textured[i * 9 + 3]; + verts[i].normal[0] = cube_vertices_textured[i * 9 + 4]; + verts[i].normal[1] = cube_vertices_textured[i * 9 + 5]; + verts[i].normal[2] = cube_vertices_textured[i * 9 + 6]; + verts[i].uv[0] = cube_vertices_textured[i * 9 + 7] * textureScaling; + verts[i].uv[1] = cube_vertices_textured[i * 9 + 8] * textureScaling; + } + + glmesh->m_numIndices = numIndices; + glmesh->m_numvertices = numVertices; + + + break; + } + case URDF_GEOM_SPHERE: + { + visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius; + + btScalar radius = visual->m_geometry.m_sphereRadius; + btSphereShape* sphereShape = new btSphereShape(radius); + convexColShape = sphereShape; + convexColShape->setMargin(0.001); + break; + } + case URDF_GEOM_MESH: + { + strncpy(visualShapeOut.m_meshAssetFileName, visual->m_geometry.m_meshFileName.c_str(), VISUAL_SHAPE_MAX_PATH_LEN); + visualShapeOut.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0; + + visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0]; + visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1]; + visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2]; + + switch (visual->m_geometry.m_meshFileType) + { + case UrdfGeometry::MEMORY_VERTICES: + { glmesh = new GLInstanceGraphicsShape; // int index = 0; glmesh->m_indices = new b3AlignedObjectArray(); glmesh->m_vertices = new b3AlignedObjectArray(); + glmesh->m_vertices->resize(visual->m_geometry.m_vertices.size()); + glmesh->m_indices->resize(visual->m_geometry.m_indices.size()); - glmesh->m_indices->resize(numIndices); - for (int k = 0; k < numIndices; k++) + for (int i = 0; i < visual->m_geometry.m_vertices.size(); i++) { - glmesh->m_indices->at(k) = cube_indices[k]; - } - glmesh->m_vertices->resize(numVertices); - - btScalar halfExtentsX = extents[0] * 0.5; - btScalar halfExtentsY = extents[1] * 0.5; - btScalar halfExtentsZ = extents[2] * 0.5; - GLInstanceVertex* verts = &glmesh->m_vertices->at(0); - btScalar textureScaling = 1; - - for (int i = 0; i < numVertices; i++) - { - - verts[i].xyzw[0] = halfExtentsX * cube_vertices_textured[i * 9]; - verts[i].xyzw[1] = halfExtentsY * cube_vertices_textured[i * 9 + 1]; - verts[i].xyzw[2] = halfExtentsZ * cube_vertices_textured[i * 9 + 2]; - verts[i].xyzw[3] = cube_vertices_textured[i * 9 + 3]; - verts[i].normal[0] = cube_vertices_textured[i * 9 + 4]; - verts[i].normal[1] = cube_vertices_textured[i * 9 + 5]; - verts[i].normal[2] = cube_vertices_textured[i * 9 + 6]; - verts[i].uv[0] = cube_vertices_textured[i * 9 + 7] * textureScaling; - verts[i].uv[1] = cube_vertices_textured[i * 9 + 8] * textureScaling; - } - - glmesh->m_numIndices = numIndices; - glmesh->m_numvertices = numVertices; - - - break; - } - case URDF_GEOM_SPHERE: - { - visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius; - - btScalar radius = visual->m_geometry.m_sphereRadius; - btSphereShape* sphereShape = new btSphereShape(radius); - convexColShape = sphereShape; - convexColShape->setMargin(0.001); - break; - } - case URDF_GEOM_MESH: - { - strncpy(visualShapeOut.m_meshAssetFileName, visual->m_geometry.m_meshFileName.c_str(), VISUAL_SHAPE_MAX_PATH_LEN); - visualShapeOut.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0; - - visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0]; - visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1]; - visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2]; - - switch (visual->m_geometry.m_meshFileType) - { - case UrdfGeometry::MEMORY_VERTICES: + glmesh->m_vertices->at(i).xyzw[0] = visual->m_geometry.m_vertices[i].x(); + glmesh->m_vertices->at(i).xyzw[1] = visual->m_geometry.m_vertices[i].y(); + glmesh->m_vertices->at(i).xyzw[2] = visual->m_geometry.m_vertices[i].z(); + glmesh->m_vertices->at(i).xyzw[3] = 1; + btVector3 normal(visual->m_geometry.m_vertices[i]); + if (visual->m_geometry.m_normals.size() == visual->m_geometry.m_vertices.size()) { - glmesh = new GLInstanceGraphicsShape; - // int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - glmesh->m_vertices->resize(visual->m_geometry.m_vertices.size()); - glmesh->m_indices->resize(visual->m_geometry.m_indices.size()); + normal = visual->m_geometry.m_normals[i]; + } + else + { + normal.safeNormalize(); + } - for (int i = 0; i < visual->m_geometry.m_vertices.size(); i++) + btVector3 uv(0.5, 0.5, 0); + if (visual->m_geometry.m_uvs.size() == visual->m_geometry.m_vertices.size()) + { + uv = visual->m_geometry.m_uvs[i]; + } + glmesh->m_vertices->at(i).normal[0] = normal[0]; + glmesh->m_vertices->at(i).normal[1] = normal[1]; + glmesh->m_vertices->at(i).normal[2] = normal[2]; + glmesh->m_vertices->at(i).uv[0] = uv[0]; + glmesh->m_vertices->at(i).uv[1] = uv[1]; + + } + for (int i = 0; i < visual->m_geometry.m_indices.size(); i++) + { + glmesh->m_indices->at(i) = visual->m_geometry.m_indices[i]; + + } + glmesh->m_numIndices = visual->m_geometry.m_indices.size(); + glmesh->m_numvertices = visual->m_geometry.m_vertices.size(); + + break; + } + case UrdfGeometry::FILE_OBJ: + { + //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + b3ImportMeshData meshData; + + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, fileIO)) + { + if (flags & URDF_USE_MATERIAL_COLORS_FROM_MTL) + { + if (meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) { - glmesh->m_vertices->at(i).xyzw[0] = visual->m_geometry.m_vertices[i].x(); - glmesh->m_vertices->at(i).xyzw[1] = visual->m_geometry.m_vertices[i].y(); - glmesh->m_vertices->at(i).xyzw[2] = visual->m_geometry.m_vertices[i].z(); - glmesh->m_vertices->at(i).xyzw[3] = 1; - btVector3 normal(visual->m_geometry.m_vertices[i]); - if (visual->m_geometry.m_normals.size() == visual->m_geometry.m_vertices.size()) + visualShapeOut.m_rgbaColor[0] = meshData.m_rgbaColor[0]; + visualShapeOut.m_rgbaColor[1] = meshData.m_rgbaColor[1]; + visualShapeOut.m_rgbaColor[2] = meshData.m_rgbaColor[2]; + + if (flags & URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) { - normal = visual->m_geometry.m_normals[i]; + visualShapeOut.m_rgbaColor[3] = meshData.m_rgbaColor[3]; } else { - normal.safeNormalize(); + visualShapeOut.m_rgbaColor[3] = 1; } - - btVector3 uv(0.5, 0.5, 0); - if (visual->m_geometry.m_uvs.size() == visual->m_geometry.m_vertices.size()) - { - uv = visual->m_geometry.m_uvs[i]; - } - glmesh->m_vertices->at(i).normal[0] = normal[0]; - glmesh->m_vertices->at(i).normal[1] = normal[1]; - glmesh->m_vertices->at(i).normal[2] = normal[2]; - glmesh->m_vertices->at(i).uv[0] = uv[0]; - glmesh->m_vertices->at(i).uv[1] = uv[1]; - } - for (int i = 0; i < visual->m_geometry.m_indices.size(); i++) - { - glmesh->m_indices->at(i) = visual->m_geometry.m_indices[i]; - - } - glmesh->m_numIndices = visual->m_geometry.m_indices.size(); - glmesh->m_numvertices = visual->m_geometry.m_vertices.size(); - - break; } - case UrdfGeometry::FILE_OBJ: + if (meshData.m_textureImage1) { - //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); - b3ImportMeshData meshData; - - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, fileIO)) - { - if (flags&URDF_USE_MATERIAL_COLORS_FROM_MTL) - { - if (meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) - { - visualShapeOut.m_rgbaColor[0] = meshData.m_rgbaColor[0]; - visualShapeOut.m_rgbaColor[1] = meshData.m_rgbaColor[1]; - visualShapeOut.m_rgbaColor[2] = meshData.m_rgbaColor[2]; - - if (flags&URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) - { - visualShapeOut.m_rgbaColor[3] = meshData.m_rgbaColor[3]; - } else - { - visualShapeOut.m_rgbaColor[3] = 1; - } - } - } - if (meshData.m_textureImage1) - { - MyTexture2 texData; - texData.m_width = meshData.m_textureWidth; - texData.m_height = meshData.m_textureHeight; - texData.textureData1 = meshData.m_textureImage1; - texData.m_isCached = meshData.m_isCached; - texturesOut.push_back(texData); - } - glmesh = meshData.m_gfxShape; - } - break; - } - case UrdfGeometry::FILE_STL: - - char relativeFileName[1024]; - if (fileIO->findResourcePath(visual->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024)) - { - glmesh = LoadMeshFromSTL(relativeFileName, fileIO); - } - break; - case UrdfGeometry::FILE_COLLADA: - { - btAlignedObjectArray visualShapes; - btAlignedObjectArray visualShapeInstances; - btTransform upAxisTrans; - upAxisTrans.setIdentity(); - float unitMeterScaling = 1; - int upAxis = 2; - - LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(), - visualShapes, - visualShapeInstances, - upAxisTrans, - unitMeterScaling, - upAxis, - fileIO); - - glmesh = new GLInstanceGraphicsShape; - // int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i = 0; i < visualShapeInstances.size(); i++) - { - ColladaGraphicsInstance* instance = &visualShapeInstances[i]; - GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i = 0; i < gfxShape->m_vertices->size(); i++) - { - verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; - verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; - verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; - verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; - verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; - verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; - verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; - verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; - verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; - } - - int curNumIndices = glmesh->m_indices->size(); - int additionalIndices = gfxShape->m_indices->size(); - glmesh->m_indices->resize(curNumIndices + additionalIndices); - for (int k = 0; k < additionalIndices; k++) - { - glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; - } - - //compensate upAxisTrans and unitMeterScaling here - btMatrix4x4 upAxisMat; - upAxisMat.setIdentity(); - // upAxisMat.setPureRotation(upAxisTrans.getRotation()); - btMatrix4x4 unitMeterScalingMat; - unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); - btMatrix4x4 worldMat = unitMeterScalingMat * upAxisMat * instance->m_worldTransform; - //btMatrix4x4 worldMat = instance->m_worldTransform; - int curNumVertices = glmesh->m_vertices->size(); - int additionalVertices = verts.size(); - glmesh->m_vertices->reserve(curNumVertices + additionalVertices); - - for (int v = 0; v < verts.size(); v++) - { - btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]); - pos = worldMat * pos; - verts[v].xyzw[0] = float(pos[0]); - verts[v].xyzw[1] = float(pos[1]); - verts[v].xyzw[2] = float(pos[2]); - glmesh->m_vertices->push_back(verts[v]); - } - } - glmesh->m_numIndices = glmesh->m_indices->size(); - glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str()); - break; - } - - default: - { - // should never get here (findExistingMeshFile returns false if it doesn't recognize extension) - btAssert(0); - } - } - - if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices > 0)) - { - //apply the geometry scaling - for (int i = 0; i < glmesh->m_vertices->size(); i++) - { - glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; - glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; - glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; - } - } - else - { - if (visual->m_geometry.m_meshFileType !=UrdfGeometry::MEMORY_VERTICES) - { - b3Warning("issue extracting mesh from COLLADA/STL file %s\n", visual->m_geometry.m_meshFileName.c_str()); + MyTexture2 texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData1 = meshData.m_textureImage1; + texData.m_isCached = meshData.m_isCached; + texturesOut.push_back(texData); } + glmesh = meshData.m_gfxShape; } break; - } // case mesh + } + case UrdfGeometry::FILE_STL: - case URDF_GEOM_PLANE: + char relativeFileName[1024]; + if (fileIO->findResourcePath(visual->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024)) + { + glmesh = LoadMeshFromSTL(relativeFileName, fileIO); + } + break; + case UrdfGeometry::FILE_COLLADA: { + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans; + upAxisTrans.setIdentity(); + float unitMeterScaling = 1; + int upAxis = 2; + + LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(), + visualShapes, + visualShapeInstances, + upAxisTrans, + unitMeterScaling, + upAxis, + fileIO); + glmesh = new GLInstanceGraphicsShape; // int index = 0; glmesh->m_indices = new b3AlignedObjectArray(); glmesh->m_vertices = new b3AlignedObjectArray(); - glmesh->m_indices->push_back(0); - glmesh->m_indices->push_back(1); - glmesh->m_indices->push_back(2); - glmesh->m_indices->push_back(0); - glmesh->m_indices->push_back(2); - glmesh->m_indices->push_back(3); - glmesh->m_scaling[0] = 1; - glmesh->m_scaling[1] = 1; - glmesh->m_scaling[2] = 1; - glmesh->m_scaling[3] = 1; - btScalar planeConst = 0; - btVector3 planeNormal = visual->m_geometry.m_planeNormal; - btVector3 planeOrigin = planeNormal * planeConst; - btVector3 vec0, vec1; - btPlaneSpace1(planeNormal, vec0, vec1); + for (int i = 0; i < visualShapeInstances.size(); i++) + { + ColladaGraphicsInstance* instance = &visualShapeInstances[i]; + GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex]; - btScalar vecLen = 128; - btVector3 verts[4]; + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); - verts[0] = planeOrigin + vec0 * vecLen + vec1 * vecLen; - verts[1] = planeOrigin - vec0 * vecLen + vec1 * vecLen; - verts[2] = planeOrigin - vec0 * vecLen - vec1 * vecLen; - verts[3] = planeOrigin + vec0 * vecLen - vec1 * vecLen; + int baseIndex = glmesh->m_vertices->size(); - GLInstanceVertex vtx; - vtx.xyzw[0] = verts[0][0]; vtx.xyzw[1] = verts[0][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; - vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; - vtx.uv[0] = vecLen; vtx.uv[1] = vecLen; - glmesh->m_vertices->push_back(vtx); + for (int i = 0; i < gfxShape->m_vertices->size(); i++) + { + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; + } - vtx.xyzw[0] = verts[1][0]; vtx.xyzw[1] = verts[1][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; - vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; - vtx.uv[0] = 0; vtx.uv[1] = vecLen; - glmesh->m_vertices->push_back(vtx); + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices + additionalIndices); + for (int k = 0; k < additionalIndices; k++) + { + glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; + } - vtx.xyzw[0] = verts[2][0]; vtx.xyzw[1] = verts[2][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; - vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; - vtx.uv[0] = 0; vtx.uv[1] = 0; - glmesh->m_vertices->push_back(vtx); - - vtx.xyzw[0] = verts[3][0]; vtx.xyzw[1] = verts[3][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; - vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; - vtx.uv[0] = vecLen; vtx.uv[1] = 0; - glmesh->m_vertices->push_back(vtx); + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; + upAxisMat.setIdentity(); + // upAxisMat.setPureRotation(upAxisTrans.getRotation()); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat * upAxisMat * instance->m_worldTransform; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices + additionalVertices); + for (int v = 0; v < verts.size(); v++) + { + btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]); + pos = worldMat * pos; + verts[v].xyzw[0] = float(pos[0]); + verts[v].xyzw[1] = float(pos[1]); + verts[v].xyzw[2] = float(pos[2]); + glmesh->m_vertices->push_back(verts[v]); + } + } glmesh->m_numIndices = glmesh->m_indices->size(); glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str()); break; } + default: { - b3Warning("TinyRenderer: unknown visual geometry type %i\n", visual->m_geometry.m_type); + // should never get here (findExistingMeshFile returns false if it doesn't recognize extension) + btAssert(0); } + } + + if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices > 0)) + { + //apply the geometry scaling + for (int i = 0; i < glmesh->m_vertices->size(); i++) + { + glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; + glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; + glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; + } + } + else + { + if (visual->m_geometry.m_meshFileType != UrdfGeometry::MEMORY_VERTICES) + { + b3Warning("issue extracting mesh from COLLADA/STL file %s\n", visual->m_geometry.m_meshFileName.c_str()); + } + } + break; + } // case mesh + + case URDF_GEOM_PLANE: + { + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + glmesh->m_indices->push_back(0); + glmesh->m_indices->push_back(1); + glmesh->m_indices->push_back(2); + glmesh->m_indices->push_back(0); + glmesh->m_indices->push_back(2); + glmesh->m_indices->push_back(3); + glmesh->m_scaling[0] = 1; + glmesh->m_scaling[1] = 1; + glmesh->m_scaling[2] = 1; + glmesh->m_scaling[3] = 1; + + btScalar planeConst = 0; + btVector3 planeNormal = visual->m_geometry.m_planeNormal; + btVector3 planeOrigin = planeNormal * planeConst; + btVector3 vec0, vec1; + btPlaneSpace1(planeNormal, vec0, vec1); + + btScalar vecLen = 128; + btVector3 verts[4]; + + verts[0] = planeOrigin + vec0 * vecLen + vec1 * vecLen; + verts[1] = planeOrigin - vec0 * vecLen + vec1 * vecLen; + verts[2] = planeOrigin - vec0 * vecLen - vec1 * vecLen; + verts[3] = planeOrigin + vec0 * vecLen - vec1 * vecLen; + + GLInstanceVertex vtx; + vtx.xyzw[0] = verts[0][0]; vtx.xyzw[1] = verts[0][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = vecLen; vtx.uv[1] = vecLen; + glmesh->m_vertices->push_back(vtx); + + vtx.xyzw[0] = verts[1][0]; vtx.xyzw[1] = verts[1][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = 0; vtx.uv[1] = vecLen; + glmesh->m_vertices->push_back(vtx); + + vtx.xyzw[0] = verts[2][0]; vtx.xyzw[1] = verts[2][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = 0; vtx.uv[1] = 0; + glmesh->m_vertices->push_back(vtx); + + vtx.xyzw[0] = verts[3][0]; vtx.xyzw[1] = verts[3][1]; vtx.xyzw[2] = 0; vtx.xyzw[3] = 0; + vtx.normal[0] = 0; vtx.normal[1] = 0; vtx.normal[2] = 1; + vtx.uv[0] = vecLen; vtx.uv[1] = 0; + glmesh->m_vertices->push_back(vtx); + + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + break; + } + default: + { + b3Warning("TinyRenderer: unknown visual geometry type %i\n", visual->m_geometry.m_type); + } } //if we have a convex, tesselate into localVertices/localIndices @@ -699,22 +700,22 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa } static btVector4 sGoogleyColors[4] = - { - btVector4(60. / 256., 186. / 256., 84. / 256., 1), - btVector4(244. / 256., 194. / 256., 13. / 256., 1), - btVector4(219. / 256., 50. / 256., 54. / 256., 1), - btVector4(72. / 256., 133. / 256., 237. / 256., 1), +{ + btVector4(60. / 256., 186. / 256., 84. / 256., 1), + btVector4(244. / 256., 194. / 256., 13. / 256., 1), + btVector4(219. / 256., 50. / 256., 54. / 256., 1), + btVector4(72. / 256., 133. / 256., 237. / 256., 1), - //btVector4(1,1,0,1), + //btVector4(1,1,0,1), }; int TinyRendererVisualShapeConverter::convertVisualShapes( - int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, + int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int orgGraphicsUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) { - int uniqueId = orgGraphicsUniqueId; + int uniqueId = orgGraphicsUniqueId; btAssert(orgGraphicsUniqueId >= 0); btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines) if (linkPtr) @@ -759,7 +760,7 @@ int TinyRendererVisualShapeConverter::convertVisualShapes( colorIndex &= 3; btVector4 color; color = (m_data->m_flags & URDF_GOOGLEY_UNDEFINED_COLORS) ? sGoogleyColors[colorIndex] : btVector4(1, 1, 1, 1); - float rgbaColor[4] = {(float)color[0], (float)color[1], (float)color[2], (float)color[3]}; + float rgbaColor[4] = { (float)color[0], (float)color[1], (float)color[2], (float)color[3] }; //if (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE) //{ // color.setValue(1,1,1,1); @@ -847,7 +848,7 @@ int TinyRendererVisualShapeConverter::convertVisualShapes( int green = 199; int blue = 255; //create a textured surface - + m_data->m_checkeredTexels.resize(texWidth * texHeight * 3); for (int i = 0; i < texWidth * texHeight * 3; i++) m_data->m_checkeredTexels[i] = 255; @@ -881,7 +882,7 @@ int TinyRendererVisualShapeConverter::convertVisualShapes( rgbaColor[1] = visualShape.m_rgbaColor[1]; rgbaColor[2] = visualShape.m_rgbaColor[2]; rgbaColor[3] = visualShape.m_rgbaColor[3]; - + if (vertices.size() && indices.size()) { TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer, m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex); @@ -901,7 +902,7 @@ int TinyRendererVisualShapeConverter::convertVisualShapes( B3_PROFILE("registerMeshShape"); tinyObj->registerMeshShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), rgbaColor, - textureImage1, textureWidth, textureHeight); + textureImage1, textureWidth, textureHeight); } visuals->m_renderObjects.push_back(tinyObj); } @@ -925,25 +926,27 @@ int TinyRendererVisualShapeConverter::convertVisualShapes( return uniqueId; } -int TinyRendererVisualShapeConverter::registerShapeAndInstance(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex) +int TinyRendererVisualShapeConverter::registerShapeAndInstance( const b3VisualShapeData& visualShape, const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex) { - btAlignedObjectArray* shapes = + btAlignedObjectArray* shapes1 = m_data->m_visualShapesMap[bodyUniqueId]; - if (!shapes) + if (!shapes1) { m_data->m_visualShapesMap.insert(bodyUniqueId, btAlignedObjectArray()); - shapes = m_data->m_visualShapesMap[bodyUniqueId]; + shapes1 = m_data->m_visualShapesMap[bodyUniqueId]; } if (numvertices && numIndices) { TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer, m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex); //those are primary soft bodies, possibly cloth, make them double-sided by default tinyObj->m_doubleSided = true; + + float rgbaColor[4] = { visualShape.m_rgbaColor[0],visualShape.m_rgbaColor[1],visualShape.m_rgbaColor[2],visualShape.m_rgbaColor[3] }; { B3_PROFILE("registerMeshShape"); - float rgbaColor[4] = { 1,1,1,1 }; + tinyObj->registerMeshShape(vertices, numvertices, indices, @@ -968,8 +971,11 @@ int TinyRendererVisualShapeConverter::registerShapeAndInstance(const float* vert visuals->m_objectUniqueId = bodyUniqueId; visuals->m_renderObjects.push_back(tinyObj); } + + shapes1->push_back(visualShape); } } + return orgGraphicsUniqueId; } @@ -978,12 +984,12 @@ void TinyRendererVisualShapeConverter::updateShape(int shapeUniqueId, const btVe TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[shapeUniqueId]; if (visualsPtr != 0) { - + TinyRendererObjectArray* visuals = *visualsPtr; if (visuals->m_renderObjects.size() == 1) { TinyRenderObjectData* renderObj = visuals->m_renderObjects[0]; - + if (renderObj->m_model->nverts() == numVertices) { TinyRender::Vec3f* verts = renderObj->m_model->readWriteVertices(); @@ -1033,7 +1039,7 @@ void TinyRendererVisualShapeConverter::changeInstanceFlags(int bodyUniqueId, int return; } int start = -1; - + for (int i = 0; i < m_data->m_swRenderInstances.size(); i++) { TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i); @@ -1079,7 +1085,7 @@ void TinyRendererVisualShapeConverter::changeRGBAColor(int bodyUniqueId, int lin TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i); if (ptrptr && *ptrptr) { - float rgba[4] = {(float)rgbaColor[0], (float)rgbaColor[1], (float)rgbaColor[2], (float)rgbaColor[3]}; + float rgba[4] = { (float)rgbaColor[0], (float)rgbaColor[1], (float)rgbaColor[2], (float)rgbaColor[3] }; TinyRendererObjectArray* visuals = *ptrptr; if ((bodyUniqueId == visuals->m_objectUniqueId) && (linkIndex == visuals->m_linkIndex)) { @@ -1176,15 +1182,15 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo { switch (m_data->m_upAxis) { - case 1: - lightDirWorld = btVector3(-50.f, 100, 30); - break; - case 2: - lightDirWorld = btVector3(-50.f, 30, 100); - break; - default: - { - } + case 1: + lightDirWorld = btVector3(-50.f, 100, 30); + break; + case 2: + lightDirWorld = btVector3(-50.f, 30, 100); + break; + default: + { + } }; } @@ -1332,9 +1338,9 @@ void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height) } void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, - float* depthBuffer, int depthBufferSizeInPixels, - int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, - int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied) + float* depthBuffer, int depthBufferSizeInPixels, + int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, + int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied) { int w = m_data->m_rgbColorBuffer.get_width(); int h = m_data->m_rgbColorBuffer.get_height(); @@ -1423,7 +1429,7 @@ void TinyRendererVisualShapeConverter::removeVisualShape(int shapeUniqueId) void TinyRendererVisualShapeConverter::resetAll() { - + for (int i = 0; i < m_data->m_swRenderInstances.size(); i++) { @@ -1474,12 +1480,13 @@ void TinyRendererVisualShapeConverter::changeShapeTexture(int bodyUniqueId, int if ((shapeIndex < 0) || (shapeIndex == v)) { - if (textureUniqueId>=0) + if (textureUniqueId >= 0) { renderObj->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData1, m_data->m_textures[textureUniqueId].m_width, m_data->m_textures[textureUniqueId].m_height); - } else + } + else { - renderObj->m_model->setDiffuseTextureFromData(0,0,0); + renderObj->m_model->setDiffuseTextureFromData(0, 0, 0); } } } @@ -1508,14 +1515,14 @@ int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename, stru { b3AlignedObjectArray buffer; buffer.reserve(1024); - int fileId = fileIO->fileOpen(filename,"rb"); - if (fileId>=0) + int fileId = fileIO->fileOpen(filename, "rb"); + if (fileId >= 0) { int size = fileIO->getFileSize(fileId); - if (size>0) + if (size > 0) { buffer.resize(size); - int actual = fileIO->fileRead(fileId,&buffer[0],size); + int actual = fileIO->fileRead(fileId, &buffer[0], size); if (actual != size) { b3Warning("image filesize mismatch!\n"); @@ -1528,7 +1535,8 @@ int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename, stru { image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3); } - } else + } + else { image = stbi_load(filename, &width, &height, &n, 3); } diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h index c32fbe5d2..68882b457 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h @@ -14,7 +14,7 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int orgGraphicsUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO); //returns a shapeUniqueId - virtual int registerShapeAndInstance(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex); + virtual int registerShapeAndInstance(const b3VisualShapeData& visualShape, const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex); virtual void updateShape(int shapeUniqueId, const btVector3* vertices, int numVertices);