/**
* \brief The constructor for the physical world.
*
* pre:
* - none
*
* post:
* - all private variables should be initialized correct
* should correct be spezified?
* - world, space, contactgroup and world_init to false (0)
*/
WorldPhysics::WorldPhysics(ControlCenter *control) {
this->control = control;
draw_contact_points = 0;
fast_step = 0;
world_cfm = 1e-10;
world_erp = 0.1;
world_gravity = Vector(0.0, 0.0, -9.81);
ground_friction = 20;
ground_cfm = 0.00000001;
ground_erp = 0.1;
world = 0;
space = 0;
contactgroup = 0;
world_init = 0;
num_contacts = 0;
create_contacts = 1;
log_contacts = 0;
// the step size in seconds
step_size = 0.01;
// dInitODE is relevant for using trimesh objects as correct as
// possible in the ode implementation
MutexLocker locker(&iMutex);
#ifdef ODE11
// for ode-0.11
dInitODE2(0);
dAllocateODEDataForThread(dAllocateMaskAll);
#else
dInitODE();
#endif
dSetErrorHandler (myErrorFunction);
dSetDebugHandler (myDebugFunction);
dSetMessageHandler (myMessageFunction);
}
static int SimulationThread(void *unused)
{
dAllocateODEDataForThread(dAllocateFlagCollisionData);
if (SDL_SetThreadPriority(SDL_THREAD_PRIORITY_HIGH))
{
//SDL_perror("SDL_SetThreadPriority");
}
while (!Quit)
{
if (SDL_LockMutex(Mutex))
{
SDL_perror("SDL_LockMutex");
break;
}
if (SDL_CondWait(Cond, Mutex))
{
SDL_perror("SDL_CondWait");
break;
}
PushEvent(UPDATE);
dSpaceCollide(Space, 0, &Near);
dWorldStep(World, Step);
dJointGroupEmpty(Group);
SDL_UnlockMutex(Mutex);
}
dCleanupODEAllDataForThread();
return 0;
}
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = { -6, 8, 6};
static float hpr[3] = { -65.0f, -27.0f, 0.0f};
dsSetViewpoint (xyz,hpr);
}
static void start()
{
static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dAllocateODEDataForThread(dAllocateMaskAll);
dsSetViewpoint (xyz,hpr);
}
void dInitODE()
{
int bInitResult = dInitODE2(0);
dIASSERT(bInitResult); dVARIABLEUSED(bInitResult);
int ibAllocResult = dAllocateODEDataForThread(dAllocateMaskAll);
dIASSERT(ibAllocResult); dVARIABLEUSED(ibAllocResult);
}
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {-8,-9,3};
static float hpr[3] = {45.0000f,-27.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
}
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {14.6117f,-6.4433f,9.3700f};
static float hpr[3] = {140.5000f,-20.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
}
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {2.4807,-1.8023,2.7600};
static float hpr[3] = {141.5000,-18.5000,0.0000};
dsSetViewpoint (xyz,hpr);
}
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("Press 'e' to start/stop occasional error.\n");
}
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("Press 'q,a,z' to control one axis of lmotor connectiong two bodies. (q is +,a is 0, z is -)\n");
printf ("Press 'w,e,r' to control one axis of lmotor connectiong first body with world. (w is +,e is 0, r is -)\n");
}
void dInitODE()
{
if (g_bODEInitialized)
return;
int bInitResult = dInitODE2(dAllocateMaskAll);
dIASSERT(bInitResult); dVARIABLEUSED(bInitResult);
int ibAllocResult = dAllocateODEDataForThread(dAllocateMaskAll);
dIASSERT(ibAllocResult); dVARIABLEUSED(ibAllocResult);
}
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
// printf ("Press:\t'1' to save the current state to 'state.dif'.\n");
// envs->Mode_View_Set_Back();
envs->Mode_View_Set_Side();
Setup_Auto_Evolve(); // for Governor's Institute of VT students
}
void dxThreadPoolThreadInfo::ThreadProcedure()
{
bool init_result = dAllocateODEDataForThread(m_ode_data_allocate_flags) != 0;
ReportInitStatus(init_result);
if (init_result)
{
RunCommandHandlingLoop();
// dCleanupODEAllDataForThread(); -- this function can only be called if ODE was initialized for manual cleanup. And that is unknown here...
}
}
/***********************************************************
init function
***********************************************************/
void ODEPhysicHandler::Init()
{
dInitODE2(0);
dAllocateODEDataForThread(dAllocateFlagCollisionData);
_world = dWorldCreate();
_space = dHashSpaceCreate(0);
// Set up gravity force
dWorldSetGravity(_world, 0, -9.81f, 0);
// Create contact group
_contactgroup = dJointGroupCreate(0);
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
dsSetViewpoint (xyz,hpr);
printf ("This program demonstrates how the Piston joint works.\n");
printf ("A Piston joint enables the sliding of a body with respect to another body\n");
printf ("and the 2 bodies are free to rotate about the sliding axis.\n\n");
printf ("The yellow body is fixed to the world\n");
printf ("The yellow body and the blue body are attached by a Piston joint with\n");
printf ("the axis along the x direction.\n");
printf ("The purple object is a geometry obstacle.\n");
printKeyBoardShortCut();
}
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
/*
printf ("Press:\t'a' to increase speed.\n"
"\t'z' to decrease speed.\n"
"\t',' to steer left.\n"
"\t'.' to steer right.\n"
"\t' ' to reset speed and steering.\n"
"\t'1' to save the current state to 'state.dif'.\n");
*/
envs->Mode_View_Set_Back();
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
dsSetViewpoint (xyz,hpr);
printf ("This program demonstrates how the PU joint works.\n");
printf ("A PU joint is a combination of a Universal joint and a Slider joint.\n");
printf ("It is a universal joint with a slider between the anchor point and \n");
printf ("body 1.\n\n");
printf ("The upper yellow body is fixed to the world\n");
printf ("The lower yellow body is attached to the upper body by a PU joint\n");
printf ("The green object is one aprt of the slider.\n");
printf ("The purple object is the second part of the slider.\n");
printf ("The red object represent the axis1 of the universal part. \n");
printf ("The blue object represent the axis2 of the universal part. \n");
printf ("The gray object represent the anchor2 of the PU joint. \n");
printKeyBoardShortCut();
}
static void start()
{
dAllocateODEDataForThread( dAllocateMaskAll );
static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dsSetViewpoint( xyz,hpr );
printf( "To drop another object, press:\n" );
printf( " b for box.\n" );
printf( " s for sphere.\n" );
printf( " c for capsule.\n" );
printf( " v for a convex.\n" );
printf( "To select an object, press space.\n" );
printf( "To disable the selected object, press d.\n" );
printf( "To enable the selected object, press e.\n" );
printf( "To toggle showing the geom AABBs, press a.\n" );
printf( "To toggle showing the contact points, press t.\n" );
printf( "To toggle dropping from random position/orientation, press r.\n" );
}
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {3.8548f,9.0843f,7.5900f};
static float hpr[3] = {-145.5f,-22.5f,0.25f};
dsSetViewpoint (xyz,hpr);
printf ("Press:\t'a' to increase speed.\n"
"\t'z' to decrease speed.\n"
"\t',' to steer left.\n"
"\t'.' to steer right.\n"
"\t' ' to reset speed and steering.\n"
"\t'[' to turn the cannon left.\n"
"\t']' to turn the cannon right.\n"
"\t'1' to raise the cannon.\n"
"\t'2' to lower the cannon.\n"
"\t'x' to shoot from the cannon.\n"
"\t'f' to toggle fast step mode.\n"
"\t'+' to increase AutoEnableDepth.\n"
"\t'-' to decrease AutoEnableDepth.\n"
"\t'r' to reset simulation.\n");
}
bool physics_init(void)
{
printlog(0, "Initiating physics");
dInitODE2(0);
dAllocateODEDataForThread(dAllocateFlagBasicData | dAllocateFlagCollisionData);
world = dWorldCreate();
//TODO: move to "dQuadTreeSpaceCreate()" - much better performance!!!
printlog(1, "TODO: create world space using dQuadTreeSpaceCreate() - much better performance!");
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
dWorldSetQuickStepNumIterations (world, internal.iterations);
//autodisable
dWorldSetAutoDisableFlag (world, 1);
dWorldSetAutoDisableLinearThreshold (world, internal.dis_linear);
dWorldSetAutoDisableAngularThreshold (world, internal.dis_angular);
dWorldSetAutoDisableSteps (world, internal.dis_steps);
dWorldSetAutoDisableTime (world, internal.dis_time);
return true;
}
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("To drop another object, press:\n");
printf (" b for box.\n");
printf (" s for sphere.\n");
printf (" c for capsule.\n");
printf (" y for cylinder.\n");
printf (" v for a convex object.\n");
printf (" x for a composite object.\n");
printf ("To select an object, press space.\n");
printf ("To disable the selected object, press d.\n");
printf ("To enable the selected object, press e.\n");
printf ("To dump transformation data for the selected object, press p.\n");
printf ("To toggle showing the geom AABBs, press a.\n");
printf ("To toggle showing the contact points, press t.\n");
printf ("To toggle dropping from random position/orientation, press r.\n");
printf ("To save the current state to 'state.dif', press 1.\n");
printf ("To show joint feedbacks of selected object, press f.\n");
}
