diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 0cb31279d..7aa58f54b 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -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;iprepareControlCommand(command); - command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode; - - for (int i=0;im_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); + } } } } diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index eb413ceda..5ced12f12 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -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(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; + } +} diff --git a/examples/SharedMemory/PhysicsServer.h b/examples/SharedMemory/PhysicsServer.h index 5981c6adb..80d8422ea 100644 --- a/examples/SharedMemory/PhysicsServer.h +++ b/examples/SharedMemory/PhysicsServer.h @@ -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 diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 0b07e7190..87bbb8392 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -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; +}