diff --git a/Demos3/bullet2/BasicDemo/main.cpp b/Demos3/bullet2/BasicDemo/main.cpp index 8d90b8416..c9265f958 100644 --- a/Demos3/bullet2/BasicDemo/main.cpp +++ b/Demos3/bullet2/BasicDemo/main.cpp @@ -66,9 +66,19 @@ class BasicDemo : public Bullet2RigidBodyDemo { SimpleOpenGL3App* m_glApp; + btRigidBody* m_pickedBody; + btTypedConstraint* m_pickedConstraint; + btVector3 m_oldPickingPos; + btVector3 m_hitPos; + btScalar m_oldPickingDist; + + + public: BasicDemo(SimpleOpenGL3App* app) - :m_glApp(app) + :m_glApp(app), + m_pickedBody(0), + m_pickedConstraint(0) { } virtual ~BasicDemo() @@ -141,7 +151,7 @@ public: curColor&=3; startTransform.setOrigin(btVector3( btScalar(2.0*i), - btScalar(1+2.0*k), + btScalar(20+2.0*k), btScalar(2.0*j))); m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents); @@ -179,12 +189,193 @@ public: m_glApp->m_instancingRenderer->renderScene(); } + + btVector3 getRayTo(int x,int y) + { + if (!m_glApp->m_instancingRenderer) + { + 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 = b3Scalar(2.0) * b3Atan(tanFov); + + btVector3 camPos,camTarget; + m_glApp->m_instancingRenderer->getCameraPosition(camPos); + m_glApp->m_instancingRenderer->getCameraTargetPosition(camTarget); + + btVector3 rayFrom = camPos; + btVector3 rayForward = (camTarget-camPos); + rayForward.normalize(); + float farPlane = 10000.f; + rayForward*= farPlane; + + btVector3 rightOffset; + btVector3 m_cameraUp=btVector3(0,1,0); + btVector3 vertical = m_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; + + b3Scalar aspect; + float width = m_glApp->m_instancingRenderer->getScreenWidth(); + float height = m_glApp->m_instancingRenderer->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; + } + + + bool mouseMoveCallback(float x,float y) + { +// if (m_data->m_altPressed!=0 || m_data->m_controlPressed!=0) + // return false; + + if (m_pickedBody && m_pickedConstraint) + { + btPoint2PointConstraint* pickCon = static_cast(m_pickedConstraint); + if (pickCon) + { + //keep it at the same picking distance + btVector3 newRayTo = getRayTo(x,y); + btVector3 rayFrom; + btVector3 oldPivotInB = pickCon->getPivotInB(); + btVector3 newPivotB; + m_glApp->m_instancingRenderer->getCameraPosition(rayFrom); + btVector3 dir = newRayTo-rayFrom; + dir.normalize(); + dir *= m_oldPickingDist; + + newPivotB = rayFrom + dir; + pickCon->setPivotB(newPivotB); + } + } + + return false; + } + bool mouseButtonCallback(int button, int state, float x, float y) + { + + if (state==1) + { + if(button==0)// && (m_data->m_altPressed==0 && m_data->m_controlPressed==0)) + { + btVector3 camPos; + m_glApp->m_instancingRenderer->getCameraPosition(camPos); + + btVector3 rayFrom = camPos; + btVector3 rayTo = getRayTo(x,y); + + btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo); + m_dynamicsWorld->rayTest(rayFrom,rayTo,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_pickedBody = body; + 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_dynamicsWorld->addConstraint(p2p,true); + 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; + } + } + + +// pickObject(pickPos, rayCallback.m_collisionObject); + m_oldPickingPos = rayTo; + m_hitPos = pickPos; + m_oldPickingDist = (pickPos-rayFrom).length(); +// printf("hit !\n"); + //add p2p + } + + } + } else + { + if (button==0) + { + if (m_pickedConstraint) + { + m_dynamicsWorld->removeConstraint(m_pickedConstraint); + delete m_pickedConstraint; + m_pickedConstraint=0; + m_pickedBody = 0; + } + //remove p2p + } + } + + //printf("button=%d, state=%d\n",button,state); + return false; + } + void stepSimulation() { m_dynamicsWorld->stepSimulation(1./60,0); } }; + + +BasicDemo* sDemo = 0; + +static void MyMouseMoveCallback( float x, float y) +{ + bool handled = false; + if (sDemo) + handled = sDemo->mouseMoveCallback(x,y); + if (!handled) + b3DefaultMouseMoveCallback(x,y); +} +static void MyMouseButtonCallback(int button, int state, float x, float y) +{ + bool handled = false; + //try picking first + if (sDemo) + handled = sDemo->mouseButtonCallback(button,state,x,y); + + if (!handled) + b3DefaultMouseButtonCallback(button,state,x,y); +} + + int main(int argc, char* argv[]) { @@ -201,8 +392,12 @@ int main(int argc, char* argv[]) app->m_instancingRenderer->setCameraPitch(0); app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0)); + app->m_window->setMouseMoveCallback(MyMouseMoveCallback); + app->m_window->setMouseButtonCallback(MyMouseButtonCallback); + BasicDemo* demo = new BasicDemo(app); demo->initPhysics(); + sDemo = demo; GLint err = glGetError(); assert(err==GL_NO_ERROR);