Merge branch 'master' of https://github.com/erwincoumans/bullet3
This commit is contained in:
@@ -1285,7 +1285,41 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu
|
||||
}
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_LOAD_TEXTURE;
|
||||
int len = strlen(filename);
|
||||
if (len<MAX_FILENAME_LENGTH)
|
||||
{
|
||||
strcpy(command->m_loadTextureArguments.m_textureFileName,filename);
|
||||
} else
|
||||
{
|
||||
command->m_loadTextureArguments.m_textureFileName[0] = 0;
|
||||
}
|
||||
command->m_updateFlags = 0;
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_UPDATE_VISUAL_SHAPE;
|
||||
command->m_updateVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueId;
|
||||
command->m_updateVisualShapeDataArguments.m_jointIndex = jointIndex;
|
||||
command->m_updateVisualShapeDataArguments.m_shapeIndex = shapeIndex;
|
||||
command->m_updateVisualShapeDataArguments.m_textureUniqueId = textureUniqueId;
|
||||
command->m_updateFlags = 0;
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
|
||||
{
|
||||
|
||||
@@ -103,6 +103,8 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con
|
||||
b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
|
||||
void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename);
|
||||
b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId);
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
|
||||
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
|
||||
|
||||
@@ -997,14 +997,20 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
|
||||
{
|
||||
b3Warning("Cannot get visual shape information");
|
||||
}
|
||||
|
||||
if (statusType == CMD_VISUAL_SHAPE_UPDATE_FAILED)
|
||||
{
|
||||
b3Warning("Cannot update visual shape");
|
||||
}
|
||||
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
|
||||
{
|
||||
b3VisualShapeInformation shapeInfo;
|
||||
b3GetVisualShapeInformation(m_physicsClientHandle, &shapeInfo);
|
||||
b3Printf("Num visual shapes: %d", shapeInfo.m_numVisualShapes);
|
||||
}
|
||||
|
||||
if (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED)
|
||||
{
|
||||
b3Printf("Visual shape update completed.");
|
||||
}
|
||||
if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
|
||||
{
|
||||
b3ContactInformation contactPointData;
|
||||
|
||||
@@ -661,6 +661,24 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
b3Warning("Visual Shape Info Request failed");
|
||||
break;
|
||||
}
|
||||
case CMD_VISUAL_SHAPE_UPDATE_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case CMD_VISUAL_SHAPE_UPDATE_FAILED:
|
||||
{
|
||||
b3Warning("Visual Shape Update failed");
|
||||
break;
|
||||
}
|
||||
case CMD_LOAD_TEXTURE_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case CMD_LOAD_TEXTURE_FAILED:
|
||||
{
|
||||
b3Warning("Load texture failed");
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
b3Error("Unknown server status %d\n", serverCmd.m_type);
|
||||
btAssert(0);
|
||||
|
||||
@@ -3018,6 +3018,31 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
case CMD_UPDATE_VISUAL_SHAPE:
|
||||
{
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED;
|
||||
|
||||
m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId);
|
||||
|
||||
serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED;
|
||||
hasStatus = true;
|
||||
|
||||
break;
|
||||
}
|
||||
case CMD_LOAD_TEXTURE:
|
||||
{
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED;
|
||||
|
||||
m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName);
|
||||
|
||||
serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED;
|
||||
hasStatus = true;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
b3Error("Unknown command encountered");
|
||||
|
||||
@@ -156,6 +156,19 @@ struct RequestVisualShapeDataArgs
|
||||
int m_startingVisualShapeIndex;
|
||||
};
|
||||
|
||||
struct UpdateVisualShapeDataArgs
|
||||
{
|
||||
int m_bodyUniqueId;
|
||||
int m_jointIndex;
|
||||
int m_shapeIndex;
|
||||
int m_textureUniqueId;
|
||||
};
|
||||
|
||||
struct LoadTextureArgs
|
||||
{
|
||||
char m_textureFileName[MAX_FILENAME_LENGTH];
|
||||
};
|
||||
|
||||
struct SendVisualShapeDataArgs
|
||||
{
|
||||
int m_bodyUniqueId;
|
||||
@@ -484,6 +497,8 @@ struct SharedMemoryCommand
|
||||
struct CreateJointArgs m_createJointArguments;
|
||||
struct RequestContactDataArgs m_requestContactPointArguments;
|
||||
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
|
||||
struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments;
|
||||
struct LoadTextureArgs m_loadTextureArguments;
|
||||
struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
|
||||
};
|
||||
};
|
||||
|
||||
@@ -35,6 +35,8 @@ enum EnumSharedMemoryClientCommand
|
||||
CMD_REQUEST_CONTACT_POINT_INFORMATION,
|
||||
CMD_SAVE_WORLD,
|
||||
CMD_REQUEST_VISUAL_SHAPE_INFO,
|
||||
CMD_UPDATE_VISUAL_SHAPE,
|
||||
CMD_LOAD_TEXTURE,
|
||||
//don't go beyond this command!
|
||||
CMD_MAX_CLIENT_COMMANDS,
|
||||
|
||||
@@ -81,6 +83,10 @@ enum EnumSharedMemoryServerStatus
|
||||
CMD_SAVE_WORLD_FAILED,
|
||||
CMD_VISUAL_SHAPE_INFO_COMPLETED,
|
||||
CMD_VISUAL_SHAPE_INFO_FAILED,
|
||||
CMD_VISUAL_SHAPE_UPDATE_COMPLETED,
|
||||
CMD_VISUAL_SHAPE_UPDATE_FAILED,
|
||||
CMD_LOAD_TEXTURE_COMPLETED,
|
||||
CMD_LOAD_TEXTURE_FAILED,
|
||||
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
|
||||
CMD_MAX_SERVER_COMMANDS
|
||||
};
|
||||
|
||||
@@ -33,6 +33,8 @@ subject to the following restrictions:
|
||||
#include <fstream>
|
||||
#include "../Importers/ImportURDFDemo/UrdfParser.h"
|
||||
#include "../SharedMemory/SharedMemoryPublic.h"//for b3VisualShapeData
|
||||
#include "../TinyRenderer/model.h"
|
||||
#include "../ThirdPartyLibs/stb_image/stb_image.h"
|
||||
|
||||
|
||||
enum MyFileType
|
||||
@@ -68,6 +70,7 @@ struct TinyRendererVisualShapeConverterInternalData
|
||||
int m_swWidth;
|
||||
int m_swHeight;
|
||||
TGAImage m_rgbColorBuffer;
|
||||
b3AlignedObjectArray<MyTexture2> m_textures;
|
||||
b3AlignedObjectArray<float> m_depthBuffer;
|
||||
b3AlignedObjectArray<int> m_segmentationMaskBuffer;
|
||||
|
||||
@@ -827,3 +830,52 @@ void TinyRendererVisualShapeConverter::resetAll()
|
||||
m_data->m_swRenderInstances.clear();
|
||||
}
|
||||
|
||||
void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, int textureUniqueId)
|
||||
{
|
||||
btAssert(textureUniqueId < m_data->m_textures.size());
|
||||
TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(shapeUniqueId);
|
||||
if (ptrptr && *ptrptr)
|
||||
{
|
||||
TinyRendererObjectArray* ptr = *ptrptr;
|
||||
ptr->m_renderObjects[0]->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData,m_data->m_textures[textureUniqueId].m_width,m_data->m_textures[textureUniqueId].m_height);
|
||||
}
|
||||
}
|
||||
|
||||
void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId)
|
||||
{
|
||||
int start = -1;
|
||||
for (int i = 0; i < m_data->m_visualShapes.size(); i++)
|
||||
{
|
||||
if (m_data->m_visualShapes[i].m_objectUniqueId == objectUniqueId && m_data->m_visualShapes[i].m_linkIndex == jointIndex)
|
||||
{
|
||||
start = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (start >= 0)
|
||||
{
|
||||
if (start + shapeIndex < m_data->m_visualShapes.size())
|
||||
{
|
||||
activateShapeTexture(start + shapeIndex, textureUniqueId);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int width, int height)
|
||||
{
|
||||
MyTexture2 texData;
|
||||
texData.m_width = width;
|
||||
texData.m_height = height;
|
||||
texData.textureData = texels;
|
||||
m_data->m_textures.push_back(texData);
|
||||
return m_data->m_textures.size()-1;
|
||||
}
|
||||
|
||||
void TinyRendererVisualShapeConverter::loadTextureFile(const char* filename)
|
||||
{
|
||||
int width,height,n;
|
||||
unsigned char* image=0;
|
||||
image = stbi_load(filename, &width, &height, &n, 3);
|
||||
registerTexture(image, width, height);
|
||||
}
|
||||
@@ -37,6 +37,11 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
|
||||
|
||||
void render();
|
||||
void render(const float viewMat[16], const float projMat[16]);
|
||||
|
||||
void loadTextureFile(const char* filename);
|
||||
int registerTexture(unsigned char* texels, int width, int height);
|
||||
void activateShapeTexture(int shapeUniqueId, int textureUniqueId);
|
||||
void activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId);
|
||||
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user