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 );
}
