Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add picking to shared memory physics server

Browse Source
This commit is contained in:
erwin coumans
2015-09-04 11:28:08 -07:00
parent 40d07139a9
commit 2909b5fae3
4 changed files with 303 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -112,6 +112,28 @@ public:
}
}
}
void prepareControlCommand(SharedMemoryCommand& command)
{
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
}
for (int i=0;i<m_numMotors;i++)
{
btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
int uIndex = m_motorTargetVelocities[i].m_uIndex;
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
}
}
virtual void physicsDebugDraw(int debugFlags){}
virtual bool mouseMoveCallback(float x,float y){return false;};
@@ -188,28 +210,9 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
command.m_type =CMD_SEND_DESIRED_STATE;
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
cl->prepareControlCommand(command);
command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
}
for (int i=0;i<cl->m_numMotors;i++)
{
btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget;
int uIndex = cl->m_motorTargetVelocities[i].m_uIndex;
if (targetVel>1)
{
printf("testme");
}
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
}
cl->enqueueCommand(command);
break;
}
@@ -239,7 +242,7 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
m_userCommandRequests.push_back(orgCommand);
SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
//b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
}
@@ -376,7 +379,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
{
if (m_userCommandRequests.size())
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
//b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
SharedMemoryCommand command = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
@@ -396,6 +399,21 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
}
m_physicsClient.submitClientCommand(command);
} else
{
if (m_numMotors)
{
SharedMemoryCommand command;
command.m_type =CMD_SEND_DESIRED_STATE;
prepareControlCommand(command);
enqueueCommand(command);
command.m_type =CMD_STEP_FORWARD_SIMULATION;
enqueueCommand(command);
command.m_type = CMD_REQUEST_ACTUAL_STATE;
enqueueCommand(command);
}
}
}
}

135
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -103,6 +103,16 @@ struct PhysicsServerInternalData
bool m_verboseOutput;
//data for picking objects
class btRigidBody* m_pickedBody;
class btTypedConstraint* m_pickedConstraint;
class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
btVector3 m_oldPickingPos;
btVector3 m_hitPos;
btScalar m_oldPickingDist;
bool m_prevCanSleep;
PhysicsServerInternalData()
:m_sharedMemory(0),
m_testBlock1(0),
@@ -112,7 +122,10 @@ struct PhysicsServerInternalData
m_debugDrawer(0),
m_guiHelper(0),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_verboseOutput(false)
m_verboseOutput(false),
m_pickedBody(0),
m_pickedConstraint(0),
m_pickingMultiBodyPoint2Point(0)
{
m_rootLocalInertialFrame.setIdentity();
}
@@ -1160,3 +1173,123 @@ void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags)
}
#endif
}
bool PhysicsServerSharedMemory::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
if (m_data->m_dynamicsWorld==0)
return false;
btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld);
m_data->m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback);
if (rayCallback.hasHit())
{
btVector3 pickPos = rayCallback.m_hitPointWorld;
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
if (body)
{
//other exclusions?
if (!(body->isStaticObject() || body->isKinematicObject()))
{
m_data->m_pickedBody = body;
m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot);
m_data->m_dynamicsWorld->addConstraint(p2p, true);
m_data->m_pickedConstraint = p2p;
btScalar mousePickClamping = 30.f;
p2p->m_setting.m_impulseClamp = mousePickClamping;
//very weak constraint for picking
p2p->m_setting.m_tau = 0.001f;
}
} else
{
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
if (multiCol && multiCol->m_multiBody)
{
m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep();
multiCol->m_multiBody->setCanSleep(false);
btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos);
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos);
//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
btScalar scaling=1;
p2p->setMaxAppliedImpulse(2*scaling);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(p2p);
m_data->m_pickingMultiBodyPoint2Point =p2p;
}
}
// pickObject(pickPos, rayCallback.m_collisionObject);
m_data->m_oldPickingPos = rayToWorld;
m_data->m_hitPos = pickPos;
m_data->m_oldPickingDist = (pickPos - rayFromWorld).length();
// printf("hit !\n");
//add p2p
}
return false;
}
bool PhysicsServerSharedMemory::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{
if (m_data->m_pickedBody && m_data->m_pickedConstraint)
{
btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_data->m_pickedConstraint);
if (pickCon)
{
//keep it at the same picking distance
btVector3 dir = rayToWorld-rayFromWorld;
dir.normalize();
dir *= m_data->m_oldPickingDist;
btVector3 newPivotB = rayFromWorld + dir;
pickCon->setPivotB(newPivotB);
}
}
if (m_data->m_pickingMultiBodyPoint2Point)
{
//keep it at the same picking distance
btVector3 dir = rayToWorld-rayFromWorld;
dir.normalize();
dir *= m_data->m_oldPickingDist;
btVector3 newPivotB = rayFromWorld + dir;
m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
}
return false;
}
void PhysicsServerSharedMemory::removePickingConstraint()
{
if (m_data->m_pickedConstraint)
{
m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint);
delete m_data->m_pickedConstraint;
m_data->m_pickedConstraint = 0;
m_data->m_pickedBody = 0;
}
if (m_data->m_pickingMultiBodyPoint2Point)
{
m_data->m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_data->m_prevCanSleep);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->removeMultiBodyConstraint(m_data->m_pickingMultiBodyPoint2Point);
delete m_data->m_pickingMultiBodyPoint2Point;
m_data->m_pickingMultiBodyPoint2Point = 0;
}
}

6
examples/SharedMemory/PhysicsServer.h
View File

@@ -42,6 +42,12 @@ public:
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
//@todo(erwincoumans) Should we have shared memory commands for picking objects?
///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
virtual void removePickingConstraint();
//for physicsDebugDraw and renderScene are mainly for debugging purposes
//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
//to a physics client, over shared memory

124
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -47,8 +47,65 @@ public:
virtual void exitPhysics(){}
virtual void physicsDebugDraw(int debugFlags);
virtual bool mouseMoveCallback(float x,float y){return false;};
virtual bool mouseButtonCallback(int button, int state, float x, float y){return false;}
btVector3 getRayTo(int x,int y);
virtual bool mouseMoveCallback(float x,float y)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (!renderer)
{
btAssert(0);
return false;
}
btVector3 rayTo = getRayTo(int(x), int(y));
btVector3 rayFrom;
renderer->getActiveCamera()->getCameraPosition(rayFrom);
m_physicsServer.movePickedBody(rayFrom,rayTo);
return false;
};
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (!renderer)
{
btAssert(0);
return false;
}
CommonWindowInterface* window = m_guiHelper->getAppInterface()->m_window;
if (state==1)
{
if(button==0 && (!window->isModifierKeyPressed(B3G_ALT) && !window->isModifierKeyPressed(B3G_CONTROL) ))
{
btVector3 camPos;
renderer->getActiveCamera()->getCameraPosition(camPos);
btVector3 rayFrom = camPos;
btVector3 rayTo = getRayTo(int(x),int(y));
m_physicsServer.pickBody(rayFrom, rayTo);
}
} else
{
if (button==0)
{
m_physicsServer.removePickingConstraint();
//remove p2p
}
}
//printf("button=%d, state=%d\n",button,state);
return false;
}
virtual bool keyboardCallback(int key, int state){return false;}
virtual void setSharedMemoryKey(int key)
@@ -142,4 +199,67 @@ class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOpt
}
btVector3 PhysicsServerExample::getRayTo(int x,int y)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (!renderer)
{
btAssert(0);
return btVector3(0,0,0);
}
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = btScalar(2.0) * btAtan(tanFov);
btVector3 camPos,camTarget;
renderer->getActiveCamera()->getCameraPosition(camPos);
renderer->getActiveCamera()->getCameraTargetPosition(camTarget);
btVector3 rayFrom = camPos;
btVector3 rayForward = (camTarget-camPos);
rayForward.normalize();
float farPlane = 10000.f;
rayForward*= farPlane;
btVector3 rightOffset;
btVector3 cameraUp=btVector3(0,0,0);
cameraUp[m_guiHelper->getAppInterface()->getUpAxis()]=1;
btVector3 vertical = cameraUp;
btVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
btScalar aspect;
float width = float(renderer->getScreenWidth());
float height = float (renderer->getScreenHeight());
aspect = width / height;
hor*=aspect;
btVector3 rayToCenter = rayFrom + rayForward;
btVector3 dHor = hor * 1.f/width;
btVector3 dVert = vertical * 1.f/height;
btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += btScalar(x) * dHor;
rayTo -= btScalar(y) * dVert;
return rayTo;
}