void createInvisibleHead( float* pos ) { dMatrix3 head_orientation; dRFromEulerAngles(head_orientation, 0.0, 0.0, 0.0); //position and orientation head.Body = dBodyCreate(World); dBodySetPosition(head.Body, pos[ 0 ], pos[ 1 ], pos[ 2 ]); dBodySetRotation(head.Body, head_orientation); dBodySetLinearVel(head.Body, 0, 0, 0); dBodySetData(head.Body, (void *)0); //mass dMass head_mass; dMassSetBox(&head_mass, 1.0, 1.0, 1.0, 1.0); dBodySetMass(head.Body, &head_mass); //geometry head.Geom = dCreateBox(Space, 1.0, 1.0, 1.0); dGeomSetBody(head.Geom, head.Body); //fixed joint invis_box_joint = dJointCreateFixed(World, jointgroup); dJointAttach(invis_box_joint, body.Body, head.Body); dJointSetFixed(invis_box_joint); }
/* ================================================================================= createFixedLeg Use parameters to create leg body/geom and attach to body with fixed joint ================================================================================= */ void createFixedLeg(ODEObject &leg, ODEObject &bodyAttachedTo, dJointID& joint, dReal xPos, dReal yPos, dReal zPos, dReal xRot, dReal yRot, dReal zRot, dReal radius, dReal length) { dMatrix3 legOrient; dRFromEulerAngles(legOrient, xRot, yRot, zRot); //position and orientation leg.Body = dBodyCreate(World); dBodySetPosition(leg.Body, xPos, yPos, zPos); dBodySetRotation(leg.Body, legOrient); dBodySetLinearVel(leg.Body, 0, 0, 0); dBodySetData(leg.Body, (void *)0); //mass dMass legMass; dMassSetCapsule(&legMass, 1, 3, radius, length); dBodySetMass(leg.Body, &legMass); //geometry leg.Geom = dCreateCapsule(Space, radius, length); dGeomSetBody(leg.Geom, leg.Body); //fixed joint joint = dJointCreateFixed(World, jointgroup); dJointAttach(joint, bodyAttachedTo.Body, leg.Body); dJointSetFixed(joint); }
void CollisionMesh::Finalize() { //create mesh data structure meshData = dGeomTriMeshDataCreate(); //create indices int indexlist = new int[TriangleList.size()*3]; for( int i = 0; i< TriangleList.size()*3; i++) { indexlist[i] = i; } //copy over vertex data into buffer dReal* vertices = new dReal[TriangleList.size()*9]; int vi=0; for ( int i=0; i<TriangleList.size(); i++ ) { vertices[vi+0] = TriangleList[i].v1.x; vertices[vi+1] = TriangleList[i].v1.y; vertices[vi+2] = TriangleList[i].v1.z; vertices[vi+3] = TriangleList[i].v2.x; vertices[vi+4] = TriangleList[i].v2.y; vertices[vi+5] = TriangleList[i].v2.z; vertices[vi+6] = TriangleList[i].v3.x; vertices[vi+7] = TriangleList[i].v3.y; vertices[vi+8] = TriangleList[i].v3.z; vi += 9; } dGeomTriMeshDataBuildSingle(meshData, vertices, sizeof(dReal)*3, TriangleList.size()*3, (const int*)indexlist, TriangleList.size()*3, 3*sizeof( int )); Geom = dCreateTriMesh(solver->GetSpaceID(true, Location.x, Location.y, Location.z), meshData, 0, 0, 0); dGeomSetData( Geom, &SurfaceDesc ); dGeomSetPosition( Geom, Location.x, Location.y, Location.z ); dMatrix3 R; dRFromEulerAngles (R, pitch, -yaw, roll); dGeomSetRotation( Geom, R ); Initialized = true; }
/* ================================================================================= createUniversalLeg Use parameters to create leg body/geom and attach to body with universal joint **Warning** mass is not set ================================================================================= */ void createUniversalLeg(ODEObject &leg, ODEObject &bodyAttachedTo, dJointID& joint, dReal xPos, dReal yPos, dReal zPos, dReal xRot, dReal yRot, dReal zRot, dReal radius, dReal length, dReal maxAngle, dReal minAngle, dReal anchorXPos, dReal anchorYPos, dReal anchorZPos) { dMatrix3 legOrient; dRFromEulerAngles(legOrient, xRot, yRot, zRot); //position and orientation leg.Body = dBodyCreate(World); dBodySetPosition(leg.Body, xPos, yPos, zPos); dBodySetRotation(leg.Body, legOrient); dBodySetLinearVel(leg.Body, 0, 0, 0); dBodySetData(leg.Body, (void *)0); //mass dMass legMass; dMassSetCapsule(&legMass, 1, 3, radius, length); //dBodySetMass(leg.Body, &legMass); //geometry leg.Geom = dCreateCapsule(Space, radius, length); dGeomSetBody(leg.Geom, leg.Body); //universal joint joint = dJointCreateUniversal(World, jointgroup); //attach and anchor dJointAttach(joint, bodyAttachedTo.Body, leg.Body); dJointSetUniversalAnchor(joint, anchorXPos, anchorYPos, anchorZPos); //axes dJointSetUniversalAxis1(joint, 0, 0, 1); dJointSetUniversalAxis2(joint, 0, 1, 0); //Max and min angles dJointSetUniversalParam(joint, dParamHiStop, maxAngle); dJointSetUniversalParam(joint, dParamLoStop, minAngle); dJointSetUniversalParam(joint, dParamHiStop2, maxAngle); dJointSetUniversalParam(joint, dParamLoStop2, minAngle); }
IPhysicObject* CPhysicManager::AddNode(INode* node, const char* init_string){ CPhysicAdapter* myAdapter = new CPhysicAdapter(Engine); myAdapter->myPhysManager = this; myAdapter->myNode = node; IModifierPack* nodeModPack = (IModifierPack*)node->GetPtrParam("ModifierPack"); nodeModPack->AddModifier( myAdapter ); CVector pos = node->getPos(); CVector rot = node->getRot(); CVector size = node->getSize(); CVector center = node->getCenterPoint(); CPhysicObject* myObj = new CPhysicObject(Engine); node->SetPtrParam("PhysicObject",myObj); myAdapter->myPhysObject =myObj; myObj->node = node; myObj->myBody = dBodyCreate(world); dBodySetPosition(myObj->myBody, pos.v[0], pos.v[1], pos.v[2] ); dBodySetLinearVel(myObj->myBody, 0, 0, 0); dBodySetAngularVel(myObj->myBody, 0, 0, 0); dMassSetBox (&myObj->m,DENSITY,size.v[0],size.v[1],size.v[2]); dBodySetMass(myObj->myBody, &myObj->m); float pitch=rot.v[0]*M_PI/180.0f; float yaw=rot.v[1]*M_PI/180.0f; float roll=rot.v[2]*M_PI/180.0f; //printf("%f %f %f\n", pitch,roll,yaw); dMatrix3 R; dRFromEulerAngles(R, pitch, yaw, roll ); dBodySetRotation(myObj->myBody, R); myObj->myGeom = dCreateBox(space, size.v[0], size.v[1], size.v[2] ); dGeomSetBody(myObj->myGeom, myObj->myBody); //dGeomSetPosition(myObj->myGeom, pos.v[0]+center.v[0], pos.v[1]+center.v[1], pos.v[2]+center.v[2] ); //dBodyAddTorque(myObj->myBody, 0, 0, 0); //dBodyAddTorque(myObj->myBody, 0, 1, 0); // преобразуем позицию ноды и углы поворота в матрицу //Matrix NodeMatrix; //NodeMatrix.LoadIdentity(); //NodeMatrix.m_data[12] = pos.v[0]; //NodeMatrix.m_data[13] = pos.v[1]; //NodeMatrix.m_data[14] = pos.v[2]; //CVector temp = CVector( NodeMatrix.m_data[12], NodeMatrix.m_data[13], NodeMatrix.m_data[14]); //NodeMatrix.RotateXYZ(rot); //NodeMatrix.m_data[12] = temp.v[0]; //NodeMatrix.m_data[13] = temp.v[1]; //NodeMatrix.m_data[14] = temp.v[2]; dGeomSetData(myObj->myGeom, node); //dGeomSetRotation(myObj->myGeom, R); // dBodySetLinearVel(Object.Body, tempVect.x, tempVect.y, tempVect.z); PhysicObjectList.push_back(myObj); printf("geom add node\n"); return myObj; //dBodySetLinearVel(myObj->myBody, 0.0, 0.0, 5.0 ); };
/******************************************************************************* Function to perform the simulation loop for ODE. *******************************************************************************/ void simulationLoop() { //First, determine which geoms inside the space are potentially colliding. //The nearCallBack function will be responsible for analizing those potential collisions. //The second parameter indicates that no user data is being sent the callback routine. dSpaceCollide(Space, 0, &nearCallBack); //Next, advance the simulation, based on the step size given. dWorldStep(World, simulationStep); //Then, remove the temporary contact joints. dJointGroupEmpty(ContactGroup); if (animator.active) { dMatrix3 orient; dRFromEulerAngles(orient, 0.0, 0.0, 0.0); const dReal sides[3] = { 1, 1, 1 }; animator.Move(); } for (auto i = 0; i < objectsToDestroy.size(); ++i) { const dBodyID foodParticleBodyID = objectsToDestroy[i].Body; const dGeomID foodParticleGeomID = objectsToDestroy[i].Geom; dGeomDestroy(foodParticleGeomID); dBodyDestroy(foodParticleBodyID); } for (auto i = objectsToDestroy.size(); i > 0; --i){ objectsToDestroy.erase(objectsToDestroy.begin() + i - 1); } //animator.updateEnergyLevel(); animator.dealWithBoundries(); if ( animator.starving ) { animator.seekFoodParticleWithHighestScore(); //printf( "# known food particles = %d\n", animator.known_target_counter ); //printf( "\t\ttarget = %f.3, %f.3\n\n", animator.target_position[0], animator.target_position[2] ); } // FOR DEBUGGING //for ( const auto& e : animator.foodParticles ) //{ // if ( e.known ) // { // const dReal* position = dBodyGetPosition( e.odeObject.Body ); // // //printf( "x: %f.2, z: %f.2\n", position[0], position[2] ); // } //} //At this point, all geometries have been updated, so they can be drawn from display(). }
/******************************************************************************* Function to initialize ODE. *******************************************************************************/ void initODE() { ///////////////// Initializing the ODE general features //////////////////// dInitODE(); //Initialize library. World = dWorldCreate(); //Crate a new dynamics, empty world. Space = dSimpleSpaceCreate(0); //Create a new space for collision (independent). ContactGroup = dJointGroupCreate(0); //Create a joints container, without specifying size. dWorldSetGravity( World, 0.0, -9.81, 0 ); //Add gravity to this World. //Define error conrrection constants. dWorldSetERP( World, 0.2 ); dWorldSetCFM( World, 1e-5 ); //Set the velocity that interpenetrating objects will separate at. dWorldSetContactMaxCorrectingVel( World, 0.9 ); //Set the safety area for contacts to be considered at rest. dWorldSetContactSurfaceLayer( World, 0.001 ); //Automatically disable objects that have come to a rest. dWorldSetAutoDisableFlag( World, false ); /////////////// Initializing the rigid bodies in the world ///////////////// //Create a collision plane and add it to space. The next parameters are the //literal components of ax + by + cz = d. dCreatePlane( Space, 0.0, 1.0, 0.0, 0.0 ); const dReal xPos = 0; const dReal yPos = 5; const dReal zPos = 0; const dReal xRot = 0; const dReal yRot = 0; const dReal zRot = 0; const dReal radius = .75; const dReal length = 4; const dReal sides[3] = { 2, 2, 2 }; //Create body body.Body = dBodyCreate(World); dBodySetPosition(body.Body, xPos, yPos, zPos); dMatrix3 Orient3; //dRFromAxisAndAngle(Orient3, 0, 0, 1, 3.14/4); dRFromEulerAngles(Orient3, xRot, yRot, zRot); //dRFromEulerAngles(Orient3, 0, 0, 0); dBodySetRotation(body.Body, Orient3); dBodySetLinearVel(body.Body, 0, 0, 0); dBodySetData(body.Body, (void *)0); dMass bodyMass; dMassSetCapsule(&bodyMass, 1.0, 3, radius, length); //dMassSetBox(&bodyMass, 10, sides[0], sides[1], sides[2]); dBodySetMass(body.Body, &bodyMass); body.Geom = dCreateCapsule(Space, radius, length); //body.Geom = dCreateBox(Space, sides[0], sides[1], sides[2]); dGeomSetBody(body.Geom, body.Body); float head_pos[ 3 ] = { 0.0f, 5.0f, -1.0f }; createInvisibleHead( head_pos ); createFrontLegs(); createMiddleLegs(); createBackLegs(); rng_engine.seed( std::chrono::duration_cast< std::chrono::microseconds >( std::chrono::system_clock::now().time_since_epoch() ).count() ); const int foodPrize = 10; dReal position[3] = { 10.0f, 0.0f, -20.0f }; addFoodParticle( position, foodPrize, &World, &Space ); }