|
楼主 |
发表于 2011-3-21 15:57:08
|
显示全部楼层
- #define min(a,b) (((a)<(b))?(a):(b))
- extern int MeshCount;
- //ODE的错误消减参数,值在0-1之间,默认值为0.2
- const double ERP = 0.8;
- //ODE的约束力混合参数
- const double CFM = 0.1;
- const double MAX_STEP = 0.01;
- const int MAX_CONTACTS = 10;
- #define NUM 200 // 最大的物体数量
- dWorldID World; //ODE世界
- dJointGroupID ContactGroup; //接触点容器
- dSpaceID Space; //ODE冲突检测空间
- dBodyID testBody; //主动运动的盒子刚体体积数据句柄,用于物理交互
- dGeomID testGeom; //主动运动的盒子刚体几何体数据句柄,用于碰撞交互
- dGeomID GroundGeom[6]; //地形刚体几何体数据句柄,用于碰撞交互
- bool peng=false; //碰撞检测函数
- typedef struct MyTrimesh
- {
- float* vertices;
- unsigned int vertexcount;
- int* indices;
- unsigned int indexcount;
- } MyTrimesh; //三角形数据TriMesh
- typedef struct MyObiect
- {
- dBodyID body;
- dGeomID geom;
- dReal matrix_dblbuff[16*2];
- int last_matrix_index;
- } MyObiect; //模型节点
- static dTriMeshDataID TriData;
- dTriMeshDataID TriData1;
- dGeomID geom;
- MyTrimesh* trimesh;
- static MyObiect model;
- osg::ref_ptr<osg::Node> testnode = new osg::Node;
- osg::ref_ptr<osg::Node> testnode1 = new osg::Node;
- osg::ref_ptr<osg::PositionAttitudeTransform> testPAT = new osg::PositionAttitudeTransform;
- osg::ref_ptr<osg::PositionAttitudeTransform> testPAT1 = new osg::PositionAttitudeTransform;
- osg::ref_ptr<osg::Group> root = new osg::Group;
- osg::ref_ptr<osgViewer::Viewer> viewer = new osgViewer::Viewer;
- osg::ref_ptr<osg::Geode> sphereGeode = new osg::Geode;
- osg::ref_ptr<osg::Box> groundBox;
- osg::ref_ptr<osg::Geode> groundGeode = new osg::Geode();
- osg::ref_ptr<osgManipulator::Selection> sel= new osgManipulator::Selection; //初始化拖拽器
- osg::ref_ptr<osgManipulator::TranslateAxisDragger> dra= new osgManipulator::TranslateAxisDragger;
- osg::ref_ptr<osgManipulator::CommandManager> cmd= new osgManipulator::CommandManager;
- osg::ref_ptr<osg::Group> gr= new osg::Group;
- //GUI事件
- class ODEController: public osgGA::GUIEventHandler
- {
- public:
- ODEController()
- {
- m_isForceOn = 0;
- }
- virtual bool handle (const osgGA::GUIEventAdapter &ea,osgGA::GUIActionAdapter &aa)
- {
- switch (ea.getEventType())
- {
- case osgGA::GUIEventAdapter::KEYDOWN:
- if (ea.getKey() == 'w' || ea.getKey() == 'W')
- {
- m_isForceOn = 1;
- return true;
- }
- if (ea.getKey() == 's' || ea.getKey() == 'S')
- {
- m_isForceOn = 2;
- return true;
- }
- case osgGA::GUIEventAdapter::KEYUP:
- //if (ea.getKey() == 'f' || ea.getKey() == 'F')
- {
- m_isForceOn = 0;
- return true;
- }
- default:
- break;
- }
- return false;
- }
- int isForceOn()
- {
- return m_isForceOn;
- }
- private:
- int m_isForceOn;
- };
- //创建物理学世界
- void setCurrentTransform(dGeomID geom)
- {
- const dReal* Pos = dGeomGetPosition(geom);
- const dReal* Rot = dGeomGetRotation(geom);
- const
- dReal Transform[16] =
- {
- Rot[0], Rot[4], Rot[8], 0,
- Rot[1], Rot[5], Rot[9], 0,
- Rot[2], Rot[6], Rot[10], 0,
-
- Pos[0], Pos[1], Pos[2], 1
- };
- dGeomTriMeshSetLastTransform( geom, *(dMatrix4*)(&Transform) );
- }
- void makeModel(const char* filename)
- {
- dMass m;
- static float weigth=50.0;
- dReal x0 = 0, y0 = 0, z0 = 5.0;
- load3dsModel(filename);
- for (int i = 0; i < MeshCount; i++) {
- //memset(&model[i],0,sizeof(model[i]));
- // rigid body
- model.body = dBodyCreate(World);
- dBodySetPosition(model.body, x0, y0, z0);
- dBodySetData(model.body,&i);
- // for a trimesh object
- TriData = dGeomTriMeshDataCreate();
- dGeomTriMeshDataBuildSingle(TriData, trimesh[i].vertices, 3 * sizeof(float),
- trimesh[i].vertexcount, trimesh[i].indices, trimesh[i].indexcount * 3, 3 * sizeof(int));
- model.geom = dCreateTriMesh(Space, TriData, 0, 0, 0);
- dGeomSetPosition(model.geom,0,0,0);
- dMatrix3 RI;
- dRSetIdentity (RI);
- dGeomSetRotation(model.geom,RI);
- }
- }
- void createODEWorld()
- {
- //初始化ODE引擎
- dInitODE2(0);
- World = dWorldCreate();
- //设置地球重力,如果设置为正数,则被操作物体将漂浮
- dWorldSetGravity(World, 0.0, 0.0, -0.5);
- //设置全局ERP
- dWorldSetERP (World, 0.8);
- dWorldSetCFM (World,1e-5);
- //创建冲突世界和几何对象
- Space = dSimpleSpaceCreate(0);
- //GroundGeom[0]=dCreatePlane(Space, 0.0, 0.0, 1.0, -40.0);
- ContactGroup = dJointGroupCreate(0);
- }
- void updateOSGWorld()
- {
- //const dReal *testQuat = dBodyGetQuaternion(model.body);
- //testPAT->setAttitude(osg::Quat(testQuat[1], testQuat[2], testQuat[3], testQuat[0]));
- //const dReal *testVec = dGeomGetPosition(model.geom);
- //testPAT->setPosition(osg::Vec3(testVec[0], testVec[1], testVec[2]));
- }
- //用于ODE冲突检测子系统的回调函数
- void nearCallback (void *, dGeomID o1, dGeomID o2)
- {
- int i;
- // if (o1->body && o2->body) return;
- // exit without doing anything if the two bodies are connected by a joint
- dBodyID b1 = dGeomGetBody
- (o1);
- dBodyID b2 = dGeomGetBody(o2);
- if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
- dContact contact[MAX_CONTACTS]; // up to
- MAX_CONTACTS contacts per box-box
- for (i=0; i<MAX_CONTACTS; i++) {
- contact[i].surface.mode = dContactBounce | dContactSoftCFM;
- contact
- [i].surface.mu = dInfinity;
- contact[i].surface.mu2 = 0;
- contact[i].surface.bounce = 0.1;
- contact[i].surface.bounce_vel = 0.1;
- contact
- [i].surface.soft_cfm = 0.01;
- }
- if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
- sizeof(dContact)))
-
- {
-
- std::cout<<"peng="<<numc<<std::endl;
- peng=true;
- //dMatrix3 RI;
- //dRSetIdentity (RI);
- //const dReal ss[3] =
- {0.02,0.02,0.02};
- //for (i=0; i<numc; i++) {
- // osg::ref_ptr<osg::Box> groundBox = new osg::Box(osg::Vec3(contact
- [i].geom.pos[0],contact[i].geom.pos[1],contact[i].geom.pos[2]), 1.0, 1.0, 1.0);
- // groundGeode->addDrawable(new osg::ShapeDrawable(groundBox.get()));
- //// dJointID c = dJointCreateContact
- (World,ContactGroup,contact+i);
- //// dJointAttach (c,b1,b2);
- //// //if (show_contacts) dsDrawBox (contact
- [i].geom.pos,RI,ss);
- //}
- }
- }
- //销毁ODE世界
- void destroyODEWorld()
- {
- dSpaceDestroy(Space);
- dWorldDestroy(World);
- }
- class keybordHandle:public osgGA::GUIEventHandler
- {
- public:
- enum modes
- {
- VIEW=0,
- PICK
- };
- keybordHandle():_mx(0.0f),_my(0.0f),_activedrgger(0),_mode(VIEW),m_isForceOn(0)
- {
- }
- ~keybordHandle()
- {
- }
- bool handle(const osgGA::GUIEventAdapter& ea,osgGA::GUIActionAdapter& aa)
- {
- osgViewer::Viewer* viewer=dynamic_cast<osgViewer::Viewer*>(&aa);
- if(!viewer)
- return false;
- if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Tab &&
- ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN &&
- _activedrgger == 0)
- {
- _mode = ! _mode;
- }
- if (VIEW == _mode) return false;
- switch(ea.getEventType())
- {
- case osgGA::GUIEventAdapter::KEYDOWN:
- if(ea.getKey()==osgGA::GUIEventAdapter::KEY_Space)
- {
- int width=ea.getWindowHeight();
- int height=ea.getWindowWidth();
- viewer->requestWarpPointer(width*0.5,height*0.5);
- }
- else
- {
- osg::ref_ptr<osg::Switch> root=dynamic_cast<osg::Switch*>(viewer->getSceneData());
- if(!root)return false;
- if(ea.getKey()=='1')
- {
- root->setValue(0,false);
- root->setValue(1,true);
- }
- else if (ea.getKey()=='2')
- {
- root->setValue(0,true);
- root->setValue(1,false);
- }
- return true;
- }
- break;
- case osgGA::GUIEventAdapter::PUSH:
- //case(osgGA::GUIEventAdapter::MOVE):
- {
- _mx=ea.getX();
- _my=ea.getY();
- _pointinfo.reset();
- osgUtil::LineSegmentIntersector::Intersections inter;
- if(viewer->computeIntersections(ea.getX(),ea.getY(),inter))
- {
- _pointinfo.setCamera(viewer->getCamera());
- _pointinfo.setMousePosition(_mx,_my);
- osgUtil::LineSegmentIntersector::Intersections::iterator hitr;
- for(hitr=inter.begin();hitr!=inter.end();++hitr)
- {
- _pointinfo.addIntersection(hitr->nodePath,hitr->getLocalIntersectPoint());
- }
- osg::NodePath::iterator it;
- for(it=_pointinfo._hitList.front().first.begin();
- it!=_pointinfo._hitList.front().first.end();it++)
- {
- osgManipulator::Dragger* dragger=dynamic_cast<osgManipulator::Dragger*>(*it);
- if(dragger)
- {
- dragger->handle(_pointinfo,ea,aa);
- _activedrgger=dragger;
- break;
- }
- }
- }
- break;
- }
- case osgGA::GUIEventAdapter::DRAG:
- case osgGA::GUIEventAdapter::RELEASE:
- {
- if(_activedrgger)
- {
- m_isForceOn=1;
- osg::Matrix dra_command=_activedrgger->getMatrix();//之前的变换
- osg::Matrix sel_command=sel->getMatrix();//之前的变换
- const dReal *testVec = dGeomGetPosition(model.geom);
- printf("center at %f %f %f\n", testVec[0], testVec[1], testVec[2]);
- _pointinfo._hitIter=_pointinfo._hitList.begin();
- _pointinfo.setCamera(viewer->getCamera());
- _pointinfo.setMousePosition(ea.getX(),ea.getY());
- _activedrgger->handle(_pointinfo,ea,aa);
- osg::Vec3d vec=_activedrgger->getMatrix().getTrans();//处理临时的变换
- dGeomSetPosition(model.geom,vec[0],vec[1],vec[2]);
- dSpaceCollide(Space, 0, nearCallback);
- dJointGroupEmpty(ContactGroup);
- if (peng)
- {
- dGeomSetPosition(model.geom,testVec[0],testVec[1],testVec[2]);
- dra->setMatrix(dra_command);
- sel->setMatrix(sel_command);
- peng=false;
- //std::cout<<"改变1"<<std::endl;
- }
- }
- break;
- }
- default: break;
- }
- if(ea.getEventType()==osgGA::GUIEventAdapter::RELEASE)
- {
- m_isForceOn=0;
- _activedrgger=0;
- _pointinfo.reset();
- }
- return true;
- }
-
- int isForceOn()
- {
- return m_isForceOn;
- }
- public:
- //得到鼠标的位置
- float _mx;
- float _my;
- unsigned int _mode;
- int m_isForceOn;
- osgManipulator::Dragger* _activedrgger;
- osgManipulator::PointerInfo _pointinfo;
- };
- int main()
- {
- double x = -5.0;
- groundBox = new osg::Box(osg::Vec3(0.0,0.0,0.0), 50.0, 50.0, 0.1);
- groundGeode->addDrawable(new osg::ShapeDrawable(groundBox.get()));
- root->addChild(groundGeode.get());
- testnode = osgDB::readNodeFile("lun2.3ds");
- testnode1 = osgDB::readNodeFile("lun4.3ds");
- testPAT->addChild(testnode1.get());
- root->addChild(testPAT.get());
- sel->addChild(testnode.get());
- dra->setupDefaultGeometry();
- gr->addChild(dra.get());
- gr->addChild(sel.get());
- float scale=testnode->getBound().radius()*1.1;
- dra->setMatrix(osg::Matrix::scale(scale,scale,scale)*osg::Matrix::translate(testnode->getBound().center()));
- cmd->connect(*dra.get(),*sel.get());
- root->addChild(gr.get());
- keybordHandle* eh= new keybordHandle;
- viewer->addEventHandler(eh);
- viewer->setCameraManipulator(new osgGA::TrackballManipulator);
- viewer->setSceneData(root.get());
- osg::ref_ptr<osg::GraphicsContext::Traits> trait= new osg::GraphicsContext::Traits;
- trait->x=100;
- trait->y=100;
- trait->width=900;
- trait->height=700;
- trait->windowDecoration=true;
- trait->doubleBuffer=true;
- trait->sharedContext=0;
- osg::ref_ptr<osg::GraphicsContext> gc=osg::GraphicsContext::createGraphicsContext(trait);
- osg::ref_ptr<osg::Camera>camera=viewer->getCamera();
- camera->setGraphicsContext(gc);
- camera->setViewport(new osg::Viewport(0,0,trait->width,trait->height));
- GLenum buffer=trait->doubleBuffer?GL_BACK:GL_FRONT;
- camera->setDrawBuffer(buffer);
- camera->setReadBuffer(buffer);
- createODEWorld();
- makeModel("lun2.3ds");
-
- load3dsModel("lun4.3ds");
- TriData1 = dGeomTriMeshDataCreate();
- dGeomTriMeshDataBuildSingle(TriData1, trimesh[0].vertices, 3 * sizeof(float),
- trimesh[0].vertexcount, trimesh[0].indices, trimesh[0].indexcount * 3, 3 * sizeof(int));
- geom = dCreateTriMesh(Space, TriData1, 0, 0, 0);
- dGeomSetPosition(geom,50,0,0);
- dMatrix3 RI;
-
- dRSetIdentity (RI);
- dGeomSetRotation(geom,RI);
- testPAT->setPosition(osg::Vec3(50.0, 0.0, 0.0));
-
- updateOSGWorld();
- osg::Timer_t prevTime = osg::Timer::instance()->tick();
- while (!viewer->done())
- {
- viewer->frame();
- const osg::Timer_t now = osg::Timer::instance()->tick();
- double deltaSecs = osg::Timer::instance()->delta_s(prevTime, now);
- prevTime = now;
- while (deltaSecs > 0.0)
- {
- const double step = min(MAX_STEP, deltaSecs);
- deltaSecs -= MAX_STEP;
- dWorldStep(World, step);
- dJointGroupEmpty(ContactGroup);
- }
- updateOSGWorld();
- }
- destroyODEWorld();
- return 0;
- }
复制代码 |
|