// initialize server (supply world dimensions)
bool PhysicsServer::init(int groundSizeX, int groundSizeY)
{
if (groundSizeX <= 20 || groundSizeY <= 20)
return false;
// create global ODE world
_world = dWorldCreate();
// create global space
_space = dHashSpaceCreate(0);
// create group for handling collisions
_contactgroup = dJointGroupCreate (0);
// make ground a bit slippery :)
_envData.slip = 0.15;
// TODO: let user specify ground surface properties
// set world properties
dWorldSetGravity (_world,0,0,-(PHYS_GRAVITY*PHYS_MASS_SCALE));
dWorldSetERP (_world, 0.6);
// create ground and walls (0,0,0 is the ground's center)
// and put them in their own space
_envSpace = dSimpleSpaceCreate(_space);
_ground = dCreatePlane (_envSpace,0, 0, 1, 0);
dGeomSetData(_ground, (void*)&_envData);
_wall[0] = dCreatePlane(_envSpace, 1, 0, 0, -(groundSizeX/2.0) ); // a,b,c,d
_wall[1] = dCreatePlane(_envSpace, -1, 0, 0, -(groundSizeX/2.0) ); // a,b,c,d
_wall[2] = dCreatePlane(_envSpace, 0, 1, 0, -(groundSizeY/2.0) ); // a,b,c,d
_wall[3] = dCreatePlane(_envSpace, 0, -1, 0, -(groundSizeY/2.0) ); // a,b,c,d
return true;
}
/* ------------------------
* メイン関数
------------------------ */
int main(int argc, char *argv[])
{
/* txtデータ読み込み */
LoadTxt("route.txt", routeX, routeY, routeZ, &lineRoute);
LoadTxt("obstacle.txt", obstX, obstY, obstZ, &lineObst);
/* ODEの初期化 */
dInitODE();
/* 描画関数の設定 */
setDrawStuff();
/* ワールド, スペース, 接触点グループの生成 */
world = dWorldCreate();
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
/* 地面, 重力の生成 */
ground = dCreatePlane(space,0,0,1,0);
dWorldSetGravity(world, 0.0, 0.0, -9.8);
/* CFM, ERPの設定 */
dWorldSetCFM(world,1e-3);
dWorldSetERP(world,0.8);
/* 全方向移動ロボットの生成 */
t1 = clock();
MakeBox();
MakeOmni();
/* シミュレーションループ */
dsSimulationLoop(argc,argv,640,480,&fn);
/* 接触点グループ, スペース, ワールドの破壊, ODEの終了 */
dJointGroupDestroy(contactgroup);
dSpaceDestroy(space);
dWorldDestroy(world);
dCloseODE();
return 0;
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
// create world
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dWorldSetAutoDisableFlag (world,1);
dWorldSetContactMaxCorrectingVel (world,0.1);
dWorldSetContactSurfaceLayer (world,0.001);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
return 0;
}
int main( int argc, char **argv )
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2( 0 );
world = dWorldCreate();
space = dSimpleSpaceCreate( 0 );
contactgroup = dJointGroupCreate( 0 );
dWorldSetGravity( world,0,0,-0.5 );
dWorldSetCFM( world,1e-5 );
dCreatePlane( space,0,0,1,0 );
memset( obj,0,sizeof( obj ) );
// run simulation
dsSimulationLoop( argc,argv,352,288,&fn );
dJointGroupDestroy( contactgroup );
dSpaceDestroy( space );
dWorldDestroy( world );
dCloseODE();
return 0;
}
dGeomID ColPlane::CreateGeom( dSpaceID spaceID )
{
float scale = PhysicsServer::s_pInstance->GetWorldScale();
Plane pl( m_Plane );
pl.Normal().normalize();
return dCreatePlane( spaceID, pl.a, pl.b, pl.c, -pl.d*scale );
} // ColPlane::CreateGeom
CPhysicManager::CPhysicManager(IEngine* _Engine):CBaseObject(_Engine){
SetParam("BaseClass","PhysicManager");
SetParam("Type","CPhysicManager");
SetParam("Name","-");
dSetErrorHandler (SilenceMessage); //затыкаем рот идиотским ошибкам
dSetDebugHandler (SilenceMessage); //затыкаем рот идиотским ошибкам
dSetMessageHandler (SilenceMessage); //затыкаем рот идиотским ошибкам
calc_time= 0.0f;
world = dWorldCreate();
dWorldSetGravity(world, 0, 0, -0.01 );
space=dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
LOGGER->LogMsg("+CPhysicManager");
ptr_world = world;
ptr_contactgroup = contactgroup;
dWorldSetCFM(world, 1e-5);
dWorldSetERP(world, 0.2);
dWorldSetContactMaxCorrectingVel(world, 0.9);
dWorldSetContactSurfaceLayer(world, 0.001);
dWorldSetAutoDisableFlag(world, 1);
// ground =
#define SHIRINA 1000
//dGeomID tmp = dCreateBox( space, SHIRINA, SHIRINA, 1);
//dGeomSetPosition(tmp, SHIRINA/2, SHIRINA/2, 0); // двигаем центр координат
//tmp = dCreateBox( space, 1, SHIRINA, SHIRINA);
//dGeomSetPosition(tmp, 0, SHIRINA/2, SHIRINA/2); // двигаем центр координат
//tmp = dCreateBox( space, SHIRINA, 1, SHIRINA);
//dGeomSetPosition(tmp, SHIRINA/2, 0, SHIRINA/2); // двигаем центр координат
dCreatePlane (space,0,0,1,0);
//dCreatePlane (space,1,1,0,0);
//dCreatePlane (space,0,1,0,0);
//dCreatePlane (space,1,0,0,0);
//dCreatePlane (space,0,0,20,0);
//dCreatePlane (space,0,10,0,0);
//dCreatePlane (space,10,0,0,0);
//dCreatePlane (space,0,0,1,0);
//dCreatePlane (space,0,0,1,0);
//dCreatePlane (space,0,0,1,0);
//dCreatePlane (space,0,0,1,0);
//dCreatePlane (space,0,0,1,0);
};
void HarrierSim::start2()
{
// setDrawStuff();
world = dWorldCreate();
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
ground = dCreatePlane(space, 0, 0, 1, 0);
dWorldSetGravity(world,0,0,-0.5);
dWorldSetCFM (world,1e-6);
dWorldSetERP (world,1);
//dWorldSetAutoDisableFlag (world,1);
dWorldSetContactMaxCorrectingVel (world,0.1);
//set the contact penetration
dWorldSetContactSurfaceLayer(world, 0.001);
makeHarrier();
itsHarrierObject = vp->load3DSObject("./etc/spaceship.3ds", "./etc/textures/spaceshiptexture.ppm");
itsHarrierObject.scale = 0.1;
//set the viewpoint
double xyz[3] = {0 , 25.0, 20};
double hpr[3] = {-90.0,-35,0.0};
vp->dsSetViewpoint (xyz,hpr);
vp->setZoom(1.5);
}
void CDynamics3DEngine::Init(TConfigurationNode& t_tree) {
/* Init parent */
CPhysicsEngine::Init(t_tree);
/* Parse the XML */
GetNodeAttributeOrDefault(t_tree, "gravity", m_cGravity, CVector3(0.0f, 0.0f, -9.81f));
GetNodeAttributeOrDefault<Real>(t_tree, "erp", m_fERP, 0.8);
GetNodeAttributeOrDefault<Real>(t_tree, "cfm", m_fCFM, 0.01);
GetNodeAttributeOrDefault<UInt32>(t_tree, "iterations", m_unIterations, 20);
GetNodeAttributeOrDefault<size_t>(t_tree, "max_contacts", m_unMaxContacts, 32);
/* Init ODE stuff */
m_tWorldID = dWorldCreate();
m_tSpaceID = dHashSpaceCreate(0);
dSpaceSetSublevel(m_tSpaceID, 0);
m_tContactJointGroupID = dJointGroupCreate(0);
dWorldSetGravity(m_tWorldID, 0.0f, 0.0f, -9.81f);
dWorldSetERP(m_tWorldID, m_fERP);
dWorldSetCFM(m_tWorldID, m_fCFM);
dWorldSetQuickStepNumIterations(m_tWorldID, m_unIterations);
dInitODE();
/* Initialize contact information */
m_ptContacts = new dContact[m_unMaxContacts];
for(UInt32 i = 0; i < m_unMaxContacts; ++i) {
::memset(&(m_ptContacts[i].surface), 0, sizeof(dSurfaceParameters));
m_ptContacts[i].surface.mode = dContactMu2;
m_ptContacts[i].surface.mu = dInfinity;
m_ptContacts[i].surface.mu2 = dInfinity;
}
/* Add a planar floor */
m_tFloor = dCreatePlane(m_tSpaceID, 0, 0, 1.0f, 0.0f);
/* Set the random seed from a random number taken from ARGoS RNG */
m_pcRNG = CARGoSRandom::CreateRNG("argos");
dRandSetSeed(m_pcRNG->Uniform(CRange<UInt32>(1,65535)));
}
/***************************************************************************\
* Instance methods *
\***************************************************************************/
void PhysicsPlaneGeom::onCreate(const PhysicsPlaneGeom *)
{
PhysicsPlaneGeomPtr tmpPtr(*this);
tmpPtr->id = dCreatePlane(0, 0, 0, 1, 0);
PhysicsPlaneGeomBase::setParams(tmpPtr->getParams());
PhysicsGeomBase::setCategoryBits(dGeomGetCategoryBits(id));
PhysicsGeomBase::setCollideBits(dGeomGetCollideBits(id));
}
//==============================================================================
OdePlane::OdePlane(
const OdeCollisionObject* parent,
const Eigen::Vector3d& normal,
double offset)
: OdeGeom(parent)
{
mGeomId = dCreatePlane(0, normal.x(), normal.y(), normal.z(), offset);
}
void Simulator_Initialize(void) {
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
ground = dCreatePlane (space,0,0,1,0);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE();
world = dWorldCreate();
#if 1
space = dHashSpaceCreate (0);
#elif 0
dVector3 center = {0,0,0}, extents = { 100, 100, 100};
space = dQuadTreeSpaceCreate(0, center, extents, 5);
#elif 0
space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
#else
space = dSimpleSpaceCreate(0);
#endif
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dWorldSetLinearDamping(world, 0.00001);
dWorldSetAngularDamping(world, 0.005);
dWorldSetMaxAngularSpeed(world, 200);
dWorldSetContactSurfaceLayer (world,0.001);
ground = dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// create lift platform
platform = dCreateBox(space, 4, 4, 1);
dGeomSetCategoryBits(ground, 1ul);
dGeomSetCategoryBits(platform, 2ul);
dGeomSetCollideBits(ground, ~2ul);
dGeomSetCollideBits(platform, ~1ul);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE2(0);
world = dWorldCreate();
space = dSimpleSpaceCreate(0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// note: can't share tridata if intending to trimesh-trimesh collide
TriData1 = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
TriData2 = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0);
TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0);
dGeomSetData(TriMesh1, TriData1);
dGeomSetData(TriMesh2, TriData2);
{dGeomSetPosition(TriMesh1, 0, 0, 0.9);
dMatrix3 Rotation;
dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
dGeomSetRotation(TriMesh1, Rotation);}
{dGeomSetPosition(TriMesh2, 1, 0, 0.9);
dMatrix3 Rotation;
dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
dGeomSetRotation(TriMesh2, Rotation);}
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
/*** シミュレーションの初期化 ***/
void dmInit()
{
dInitODE(); // ODEの初期化
world = dWorldCreate(); // 世界の創造
dWorldSetGravity(world, 0.0,0.0,-9.8); // 重力設定
space = dHashSpaceCreate(0); // 衝突用空間の創造
contactgroup = dJointGroupCreate(0); // ジョイントグループの生成
ground = dCreatePlane(space,0,0,1,0); // 地面（平面ジオメトリ）の生成
dsSetSphereQuality(3);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.stop = 0;
fn.command = 0;
fn.path_to_textures = "../../drawstuff/textures";
dInitODE ();
// create world
world = dWorldCreate ();
space = dHashSpaceCreate (0);
dWorldSetGravity (world,0,0,0); //Original Gravity = -0.2
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
contactgroup = dJointGroupCreate (0);
// create object
sphere0 = dBodyCreate (world);
sphere0_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere0,&m);
dGeomSetBody (sphere0_geom,sphere0);
sphere1 = dBodyCreate (world);
sphere1_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere1,&m);
dGeomSetBody (sphere1_geom,sphere1);
sphere2 = dBodyCreate (world);
sphere2_geom = dCreateSphere (space,0.5);
dMassSetSphere (&m,1,0.5);
dBodySetMass (sphere2,&m);
dGeomSetBody (sphere2_geom,sphere2);
// set initial position
dBodySetPosition (sphere0,0,0,4);
dBodySetPosition (sphere1,5,0,4);
dBodySetPosition (sphere2,-2,0,4);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
// clean up
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
int main (int argc, char **argv)
{
int i;
dReal k;
dMass m;
/* setup pointers to drawstuff callback functions */
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
if(argc==2)
{
fn.path_to_textures = argv[1];
}
/* create world */
dInitODE();
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (1000000);
dWorldSetGravity (world,0,0,-0.5);
dCreatePlane (space,0,0,1,0);
for (i=0; i<NUM; i++) {
body[i] = dBodyCreate (world);
k = i*SIDE;
dBodySetPosition (body[i],k,k,k+0.4);
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dBodySetMass (body[i],&m);
sphere[i] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[i],body[i]);
}
for (i=0; i<(NUM-1); i++) {
joint[i] = dJointCreateBall (world,0);
dJointAttach (joint[i],body[i],body[i+1]);
k = (i+0.5)*SIDE;
dJointSetBallAnchor (joint[i],k,k,k+0.4);
}
/* run simulation */
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void ode::reset()
{
world = dWorldCreate();
space = dSweepAndPruneSpaceCreate(0, dSAP_AXES_XYZ );
dWorldSetGravity (world,0,300,0);
dWorldSetCFM (world, 1e-5);
dWorldSetERP (world, 0.8);
//dWorldSetQuickStepNumIterations (world,ITERS);
ground = dCreatePlane (space,0,0,100,0);
}
void initWorldModelling(int testcase)
{
/* create world */
dRandSetSeed(1);
dInitODE();
//dInitODE2(dInitFlagManualThreadCleanup);
//dAllocateODEDataForThread(dAllocateMaskAll);
world = dWorldCreate();
space = dHashSpaceCreate (0);
//dWorldSetAutoDisableFlag(World, 1);
// The parameter needs to be zero.
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,-9.81f,0);
dWorldSetCFM (world,1e-2f); //1e-5f was the originally value used.
dWorldSetERP(world,1.0f); // 1 is Error Correction is applied.
// Set Damping
dWorldSetLinearDamping(world, 0.01f); // 0.00001
dWorldSetAngularDamping(world, 0.005f); // 0.005
dWorldSetMaxAngularSpeed(world, 200);
// Set and get the depth of the surface layer around all geometry objects. Contacts are allowed to sink into the surface layer up to the given depth before coming to rest. The default value is zero. Increasing this to some small value (e.g. 0.001) can help prevent jittering problems due to contacts being repeatedly made and broken.
dWorldSetContactSurfaceLayer (world,0.001f);
ground = dCreatePlane (space,0,1,0,0);
switch(testcase)
{
case 1:initIslands();test1();break;// Carrier stability
case 2:initIslands();test1();test2();break;// Manta landing on island.
case 3:initIslands();test1();test2();test3();break;// Manta crashing on structure
case 4:initIslands();test1();test2();test3();test4();break;// Manta landing on runway
case 5:initIslands();test1();test2();break;// Manta landing on aircraft
case 6:initIslands();test1();test6();break;// Carrier stranded on island
case 7:test1();test2();test7();break; //Manta crashing on water.
case 8:initIslands();test1();test8();break; // Walrus reaching island.
case 9:test1();test9();break; // Walrus stability.
case 10:initIslands();test1();test10();break; // Walrus arrive to island and build the command center.
case 11:initIslands();test11();break; // Carrier stability far away.
case 12:initIslands();test1();test12();break; // Bullets
case 13:initIslands();test13();break;
default:initIslands();test1();break;
}
testing = testcase;
}
int main (int argc, char **argv)
{
static dMass m;
dReal mass = 1.0;
// set for drawing
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = NULL;
fn.stop = NULL;
fn.path_to_textures = "../textures";
dInitODE(); // init ODE
world = dWorldCreate(); // create a dynamic world
space = dSimpleSpaceCreate (0);
//@a box
capsule = dBodyCreate (world);
geom = dCreateCapsule (space,radius,length); //create geometry.
dMassSetCapsule(&m,DENSITY,3,radius,length);
dBodySetMass (capsule,&m);
dGeomSetBody(geom,capsule);
dBodySetPosition (capsule,0,4,1);
//Gravedad y cosas de simulacion
dWorldSetGravity(world,0,0,-9.81);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
contactgroup = dJointGroupCreate (0);
// the simulation
dsSimulationLoop (argc,argv,960,480,&fn);
//-- Destroy the world!!!
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void construirMundo()
{
world = dWorldCreate(); // Inicializa o mundo (responsável pela dinâmica)
space = dHashSpaceCreate(0); // Inicializa o espaço (responsável pela colisão de geometrias)
contactgroup = dJointGroupCreate(0); // Inicializa o grupo de juntas de contato (responsáveis pelas forças reativas durante colisões)
dWorldSetGravity(world,0,0,GRAVITY); // Define a gravidade do mundo
ground = dCreatePlane(space,0,0,1,0); // Cria a geometria do chão (plano xy)
construirRobos(); // Cria os robôs
construirBola(); // Cria a bola
construirCampo(); // Cria o campo
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-GRAVITY);
dWorldSetCFM (world,1e-5);
dWorldSetAutoDisableFlag (world,1);
#if 1
dWorldSetAutoDisableAverageSamplesCount( world, 10 );
#endif
dWorldSetLinearDamping(world, 0.00001);
dWorldSetAngularDamping(world, 0.005);
dWorldSetMaxAngularSpeed(world, 200);
dWorldSetContactMaxCorrectingVel (world,0.1);
dWorldSetContactSurfaceLayer (world,0.001);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
int main(int argc, char **argv)
{
dInitODE(); // ODEの初期化
setDrawStuff();
world = dWorldCreate(); // ワールドの生成
space = dHashSpaceCreate(0); // スペースの生成
contactgroup = dJointGroupCreate(0); // 接触グループの生成
ground = dCreatePlane(space, 0, 0, 1, 0); // 地面の生成
dWorldSetGravity(world, 0, 0, -9.8); // 重力の設定
makeArm(); // アームの生成
dsSimulationLoop(argc, argv, 640, 480, &fn); // シミュレーションループ
dSpaceDestroy(space); // スペースの破壊
dWorldDestroy(world); // ワールドの破壊
dCloseODE(); // ODEの終了
return 0;
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
world = dWorldCreate();
dVector3 Center = {0, 0, 0, 0};
dVector3 Extents = {WORLD_SIZE * 0.55, WORLD_SIZE * 0.55, WORLD_SIZE * 0.55, 0};
//space = dSimpleSpaceCreate(0);
//space = dHashSpaceCreate (0);
space = dQuadTreeSpaceCreate (0, Center, Extents, 6);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
for (int i = 0; i < NUM; i++){
command('s');
}
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
return 0;
}
dGeomID set_phys_geom_type(dGeomID geom, dBodyID body,
int i, int t, const float *v)
{
/* Destroy the old geom and its data. */
if (geom)
{
free(dGeomGetData(geom));
dGeomDestroy(geom);
geom = 0;
}
/* Create a new geom of the required type. */
switch (t)
{
case dSphereClass:
geom = dCreateSphere(space, v[0]);
break;
case dCapsuleClass:
geom = dCreateCapsule(space, v[0], v[1]);
break;
case dBoxClass:
geom = dCreateBox(space, v[0], v[1], v[2]);
break;
case dPlaneClass:
geom = dCreatePlane(space, v[0], v[1], v[2], v[3]);
break;
case dRayClass:
geom = dCreateRay(space, (dReal) sqrt(v[3] * v[3] +
v[4] * v[4] +
v[5] * v[5]));
dGeomRaySet(geom, v[0], v[1], v[2], v[3], v[4], v[5]);
break;
}
/* Assign geom data and attach it to the body. */
if (geom)
{
dGeomSetData(geom, create_data(i));
dGeomSetBody(geom, body);
}
return geom;
}
int test_plane_point_depth()
{
int j;
dVector3 n,p,q,a,b; // n = plane normal
dReal d;
dSimpleSpace space(0);
dGeomID plane = dCreatePlane (0,0,0,1,0);
dSpaceAdd (space,plane);
// ********** make a random plane
for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
dNormalize3 (n);
d = dRandReal() - 0.5;
dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
dPlaneSpace (n,p,q);
// ********** test point on plane has depth 0
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = 0;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED();
// ********** test arbitrary depth point
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = dRandReal() - 0.5;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol)
FAILED();
// ********** test depth-1 point
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = -1;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED();
PASSED();
}
int main (int argc, char *argv[]) {
dInitODE();
setDrawStuff();
world = dWorldCreate();
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
dWorldSetGravity(world,0,0,0);
dWorldSetERP(world,1.0); // ERPの設定
dWorldSetCFM(world,0.0); // CFMの設定
ground = dCreatePlane(space,0,0,1,0);
create();
dsSimulationLoop (argc,argv,500,600,&fn);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void Plane::init(const OdeHandle& odeHandle, double mass, const OsgHandle& osgHandle,
char mode) {
assert(mode & Body || mode & Geom);
this->mode=mode;
QMP_CRITICAL(0);
if (mode & Body){
body = dBodyCreate (odeHandle.world);
setMass(mass, mode & Density);
}
if(mode & Geom){
geom = dCreatePlane ( odeHandle.space , 0 , 0 , 1 , 0 );
attachGeomAndSetColliderFlags();
}
if(mode & Draw){
osgplane->init(osgHandle);
}
QMP_END_CRITICAL(0);
}
void CPHWorld::Create()
{
dWorldID phWorld=0;
if (psDeviceFlags.test(mtPhysics)) Device.seqFrameMT.Add (this,REG_PRIORITY_HIGH);
else Device.seqFrame.Add (this,REG_PRIORITY_LOW);
m_commander =xr_new<CPHCommander>();
//dVector3 extensions={2048,256,2048};
/*
Fbox level_box = Level().ObjectSpace.GetBoundingVolume();
Fvector level_size,level_center;
level_box . getsize (level_size);
level_box . getcenter (level_center);
dVector3 extensions = { level_size.x ,256.f,level_size.z};
dVector3 center = {level_center.x,0.f,level_center.z};
*/
#ifdef ODE_SLOW_SOLVER
#else
dWorldSetAutoEnableDepthSF1(phWorld, 100000000);
///dWorldSetContactSurfaceLayer(phWorld,0.f);
//phWorld->contactp.min_depth =0.f;
#endif
ContactGroup = dJointGroupCreate(0);
dWorldSetGravity (phWorld, 0,-Gravity(), 0);//-2.f*9.81f
Mesh.Create (0,phWorld);
#ifdef PH_PLAIN
plane=dCreatePlane(Space,0,1,0,0.3f);
#endif
//const dReal k_p=2400000.f;//550000.f;///1000000.f;
//const dReal k_d=200000.f;
dWorldSetERP(phWorld, ERP(world_spring,world_damping) );
dWorldSetCFM(phWorld, CFM(world_spring,world_damping));
//dWorldSetERP(phWorld, 0.2f);
//dWorldSetCFM(phWorld, 0.000001f);
disable_count=0;
m_motion_ray=dCreateRayMotions(0);
phBoundaries.set(Level().ObjectSpace.GetBoundingVolume());
phBoundaries.y1-=30.f;
CPHCollideValidator::Init();
b_exist=true;
}
//===========================================================================
void cODEGenericBody::createStaticPlane(const cVector3d& a_position,
const cVector3d& a_normal)
{
// object is static by default
m_static = true;
// temp variables
cVector3d normal = a_normal;
cVector3d offset(0,0,0);
// check normal
if (normal.length() == 0) { return; }
// compute parameters
normal.normalize();
double a = normal.x;
double b = normal.y;
double c = normal.z;
offset = cProject(a_position, normal);
double d = offset.length();
if (d > 0)
{
if (cAngle(offset, normal) > cDegToRad(90))
{
d = -d;
}
}
// build fixed plane
m_ode_geom = dCreatePlane(m_ODEWorld->m_ode_space, a, b, c, d);
// store dynamic model type
m_typeDynamicCollisionModel = ODE_MODEL_PLANE;
// store dynamic model parameters
m_paramDynColModel0 = normal.x;
m_paramDynColModel1 = normal.y;
m_paramDynColModel2 = normal.z;
m_posOffsetDynColModel = a_position;
m_rotOffsetDynColModel.identity();
}
void create_world(Controller* controller,bool log,bool bMoviePlay)
{
// create world
dRandSetSeed(10);
dInitODE();
creatures.clear();
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
floorplane = dCreatePlane (space,0,0,1, 0.0);
dWorldSetERP(world,0.1);
dWorldSetCFM(world,1E-4);
Biped* biped = new Biped(log,bMoviePlay);
dVector3 pos={0.0,0.0,0.0};
biped->Create(world,space,pos,controller);
creatures.push_back(biped);
}
