add quadruped.py script to load and initialize the a Minitaur-like quadruped
pybullet removeConstraint, createConstraint rename b3CreateJoint to b3InitCreateUserConstraintCommand add int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle); b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
This commit is contained in:
@@ -960,7 +960,9 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd
|
||||
return cl->getJointInfo(bodyIndex, jointIndex, *info);
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
|
||||
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
@@ -968,22 +970,52 @@ b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
|
||||
command->m_type = CMD_CREATE_JOINT;
|
||||
command->m_createJointArguments.m_parentBodyIndex = parentBodyIndex;
|
||||
command->m_createJointArguments.m_parentJointIndex = parentJointIndex;
|
||||
command->m_createJointArguments.m_childBodyIndex = childBodyIndex;
|
||||
command->m_createJointArguments.m_childJointIndex = childJointIndex;
|
||||
command->m_type = CMD_USER_CONSTRAINT;
|
||||
command->m_updateFlags = USER_CONSTRAINT_ADD_CONSTRAINT;
|
||||
|
||||
command->m_userConstraintArguments.m_parentBodyIndex = parentBodyIndex;
|
||||
command->m_userConstraintArguments.m_parentJointIndex = parentJointIndex;
|
||||
command->m_userConstraintArguments.m_childBodyIndex = childBodyIndex;
|
||||
command->m_userConstraintArguments.m_childJointIndex = childJointIndex;
|
||||
for (int i = 0; i < 7; ++i) {
|
||||
command->m_createJointArguments.m_parentFrame[i] = info->m_parentFrame[i];
|
||||
command->m_createJointArguments.m_childFrame[i] = info->m_childFrame[i];
|
||||
command->m_userConstraintArguments.m_parentFrame[i] = info->m_parentFrame[i];
|
||||
command->m_userConstraintArguments.m_childFrame[i] = info->m_childFrame[i];
|
||||
}
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
command->m_createJointArguments.m_jointAxis[i] = info->m_jointAxis[i];
|
||||
command->m_userConstraintArguments.m_jointAxis[i] = info->m_jointAxis[i];
|
||||
}
|
||||
command->m_createJointArguments.m_jointType = info->m_jointType;
|
||||
command->m_userConstraintArguments.m_jointType = info->m_jointType;
|
||||
return (b3SharedMemoryCommandHandle)command;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
|
||||
command->m_type = CMD_USER_CONSTRAINT;
|
||||
command->m_updateFlags = USER_CONSTRAINT_REMOVE_CONSTRAINT;
|
||||
command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId;
|
||||
return (b3SharedMemoryCommandHandle)command;
|
||||
}
|
||||
int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle)
|
||||
{
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
|
||||
b3Assert(status);
|
||||
b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED);
|
||||
if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED)
|
||||
{
|
||||
return status->m_userConstraintResultArgs.m_userConstraintUniqueId;
|
||||
}
|
||||
|
||||
return -1;
|
||||
|
||||
}
|
||||
|
||||
|
||||
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
|
||||
double rayFromWorldY, double rayFromWorldZ,
|
||||
double rayToWorldX, double rayToWorldY, double rayToWorldZ)
|
||||
|
||||
@@ -72,7 +72,9 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
|
||||
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
|
||||
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
|
||||
|
||||
b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
|
||||
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
|
||||
int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
|
||||
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
|
||||
|
||||
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
|
||||
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
|
||||
|
||||
@@ -731,7 +731,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
b3Warning("User debug draw failed");
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_USER_CONSTRAINT_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case CMD_USER_CONSTRAINT_FAILED:
|
||||
{
|
||||
b3Warning("createConstraint failed");
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
b3Error("Unknown server status %d\n", serverCmd.m_type);
|
||||
btAssert(0);
|
||||
|
||||
@@ -671,10 +671,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_USER_CONSTRAINT_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case CMD_USER_CONSTRAINT_FAILED:
|
||||
{
|
||||
b3Warning("createConstraint failed");
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
b3Warning("Unknown server status type");
|
||||
//b3Warning("Unknown server status type");
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -123,6 +123,17 @@ struct InteralBodyData
|
||||
}
|
||||
};
|
||||
|
||||
struct InteralUserConstraintData
|
||||
{
|
||||
btTypedConstraint* m_rbConstraint;
|
||||
btMultiBodyConstraint* m_mbConstraint;
|
||||
InteralUserConstraintData()
|
||||
:m_rbConstraint(0),
|
||||
m_mbConstraint(0)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
///todo: templatize this
|
||||
struct InternalBodyHandle : public InteralBodyData
|
||||
{
|
||||
@@ -468,6 +479,10 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
|
||||
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
|
||||
btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
|
||||
|
||||
int m_userConstraintUIDGenerator;
|
||||
btHashMap<btHashInt, InteralUserConstraintData> m_userConstraints;
|
||||
|
||||
b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
|
||||
|
||||
|
||||
@@ -532,6 +547,7 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
m_logPlayback(0),
|
||||
m_physicsDeltaTime(1./240.),
|
||||
m_numSimulationSubSteps(0),
|
||||
m_userConstraintUIDGenerator(1),
|
||||
m_dynamicsWorld(0),
|
||||
m_remoteDebugDrawer(0),
|
||||
m_guiHelper(0),
|
||||
@@ -3097,72 +3113,144 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
case CMD_CREATE_JOINT:
|
||||
case CMD_USER_CONSTRAINT:
|
||||
{
|
||||
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_createJointArguments.m_parentBodyIndex);
|
||||
if (parentBody && parentBody->m_multiBody)
|
||||
{
|
||||
InteralBodyData* childBody = m_data->getHandle(clientCmd.m_createJointArguments.m_childBodyIndex);
|
||||
if (childBody)
|
||||
{
|
||||
btVector3 pivotInParent(clientCmd.m_createJointArguments.m_parentFrame[0], clientCmd.m_createJointArguments.m_parentFrame[1], clientCmd.m_createJointArguments.m_parentFrame[2]);
|
||||
btVector3 pivotInChild(clientCmd.m_createJointArguments.m_childFrame[0], clientCmd.m_createJointArguments.m_childFrame[1], clientCmd.m_createJointArguments.m_childFrame[2]);
|
||||
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_createJointArguments.m_parentFrame[3], clientCmd.m_createJointArguments.m_parentFrame[4], clientCmd.m_createJointArguments.m_parentFrame[5], clientCmd.m_createJointArguments.m_parentFrame[6]));
|
||||
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_createJointArguments.m_childFrame[3], clientCmd.m_createJointArguments.m_childFrame[4], clientCmd.m_createJointArguments.m_childFrame[5], clientCmd.m_createJointArguments.m_childFrame[6]));
|
||||
btVector3 jointAxis(clientCmd.m_createJointArguments.m_jointAxis[0], clientCmd.m_createJointArguments.m_jointAxis[1], clientCmd.m_createJointArguments.m_jointAxis[2]);
|
||||
if (clientCmd.m_createJointArguments.m_jointType == eFixedType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
|
||||
multibodyFixed->setMaxAppliedImpulse(500.0);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild);
|
||||
rigidbodyFixed->setMaxAppliedImpulse(500.0);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(rigidbodyFixed);
|
||||
}
|
||||
}
|
||||
else if (clientCmd.m_createJointArguments.m_jointType == ePrismaticType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
|
||||
multibodySlider->setMaxAppliedImpulse(500.0);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
|
||||
rigidbodySlider->setMaxAppliedImpulse(500.0);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(rigidbodySlider);
|
||||
}
|
||||
} else if (clientCmd.m_createJointArguments.m_jointType == ePoint2PointType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild);
|
||||
p2p->setMaxAppliedImpulse(500);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild);
|
||||
p2p->setMaxAppliedImpulse(500);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(p2p);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
SharedMemoryStatus& serverCmd =serverStatusOut;
|
||||
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
|
||||
hasStatus = true;
|
||||
|
||||
if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT)
|
||||
{
|
||||
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
|
||||
if (parentBody && parentBody->m_multiBody)
|
||||
{
|
||||
InteralBodyData* childBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex);
|
||||
if (childBody)
|
||||
{
|
||||
btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
|
||||
btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
|
||||
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6]));
|
||||
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6]));
|
||||
btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]);
|
||||
if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
|
||||
multibodyFixed->setMaxAppliedImpulse(500.0);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = multibodyFixed;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild);
|
||||
rigidbodyFixed->setMaxAppliedImpulse(500.0);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(rigidbodyFixed);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = rigidbodyFixed;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
|
||||
}
|
||||
else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
|
||||
multibodySlider->setMaxAppliedImpulse(500.0);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = multibodySlider;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
|
||||
rigidbodySlider->setMaxAppliedImpulse(500.0);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(rigidbodySlider);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = rigidbodySlider;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
|
||||
} else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild);
|
||||
p2p->setMaxAppliedImpulse(500);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = p2p;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild);
|
||||
p2p->setMaxAppliedImpulse(500);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(p2p);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = p2p;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
b3Warning("unknown constraint type");
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT)
|
||||
{
|
||||
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
|
||||
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove);
|
||||
if (userConstraintPtr)
|
||||
{
|
||||
if (userConstraintPtr->m_mbConstraint)
|
||||
{
|
||||
m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint);
|
||||
delete userConstraintPtr->m_mbConstraint;
|
||||
m_data->m_userConstraints.remove(userConstraintUidRemove);
|
||||
}
|
||||
if (userConstraintPtr->m_rbConstraint)
|
||||
{
|
||||
|
||||
}
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case CMD_CALCULATE_INVERSE_KINEMATICS:
|
||||
|
||||
@@ -488,7 +488,14 @@ struct CalculateInverseKinematicsResultArgs
|
||||
double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
|
||||
};
|
||||
|
||||
struct CreateJointArgs
|
||||
enum EnumUserConstraintFlags
|
||||
{
|
||||
USER_CONSTRAINT_ADD_CONSTRAINT=1,
|
||||
USER_CONSTRAINT_REMOVE_CONSTRAINT=2,
|
||||
USER_CONSTRAINT_CHANGE_CONSTRAINT=4
|
||||
};
|
||||
|
||||
struct UserConstraintArgs
|
||||
{
|
||||
int m_parentBodyIndex;
|
||||
int m_parentJointIndex;
|
||||
@@ -498,9 +505,13 @@ struct CreateJointArgs
|
||||
double m_childFrame[7];
|
||||
double m_jointAxis[3];
|
||||
int m_jointType;
|
||||
int m_userConstraintUniqueId;
|
||||
};
|
||||
|
||||
|
||||
struct UserConstraintResultArgs
|
||||
{
|
||||
int m_userConstraintUniqueId;
|
||||
};
|
||||
|
||||
enum EnumUserDebugDrawFlags
|
||||
{
|
||||
@@ -563,7 +574,7 @@ struct SharedMemoryCommand
|
||||
struct ExternalForceArgs m_externalForceArguments;
|
||||
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
|
||||
struct CalculateJacobianArgs m_calculateJacobianArguments;
|
||||
struct CreateJointArgs m_createJointArguments;
|
||||
struct UserConstraintArgs m_userConstraintArguments;
|
||||
struct RequestContactDataArgs m_requestContactPointArguments;
|
||||
struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
|
||||
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
|
||||
@@ -620,6 +631,7 @@ struct SharedMemoryStatus
|
||||
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
|
||||
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
|
||||
struct UserDebugDrawResultArgs m_userDebugDrawArgs;
|
||||
struct UserConstraintResultArgs m_userConstraintResultArgs;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
@@ -33,7 +33,7 @@ enum EnumSharedMemoryClientCommand
|
||||
CMD_CALCULATE_INVERSE_DYNAMICS,
|
||||
CMD_CALCULATE_INVERSE_KINEMATICS,
|
||||
CMD_CALCULATE_JACOBIAN,
|
||||
CMD_CREATE_JOINT,
|
||||
CMD_USER_CONSTRAINT,
|
||||
CMD_REQUEST_CONTACT_POINT_INFORMATION,
|
||||
CMD_REQUEST_AABB_OVERLAP,
|
||||
CMD_SAVE_WORLD,
|
||||
@@ -104,6 +104,8 @@ enum EnumSharedMemoryServerStatus
|
||||
CMD_LOAD_TEXTURE_FAILED,
|
||||
CMD_USER_DEBUG_DRAW_COMPLETED,
|
||||
CMD_USER_DEBUG_DRAW_FAILED,
|
||||
CMD_USER_CONSTRAINT_COMPLETED,
|
||||
CMD_USER_CONSTRAINT_FAILED,
|
||||
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
|
||||
CMD_MAX_SERVER_COMMANDS
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user