// If the dragger is to detect for collision between the moving object
// and the rest of the world, set up the appropriate collision models.
void IvObjectDragger::CheckCollision(bool flag)
{
_checkCollision = flag;
ItemPtr selectedItem = GetSelectedItem();
if (_checkCollision && !!selectedItem) {
// synchronize the collision model transform
KinBodyItemPtr pbody = boost::dynamic_pointer_cast<KinBodyItem>(selectedItem);
if( !!pbody ) {
EnvironmentMutex::scoped_try_lock lock(_penv->GetMutex());
if( !!lock ) {
int prevoptions = _penv->GetCollisionChecker()->GetCollisionOptions();
_penv->GetCollisionChecker()->SetCollisionOptions(CO_Contacts);
CollisionReportPtr preport(new CollisionReport());
if( pbody->GetBody()->CheckSelfCollision(preport) ) {
RAVELOG_VERBOSE(str(boost::format("self-collision %s\n")%preport->__str__()));
_SetColor(COLLISION_COLOR);
}
else if( _penv->CheckCollision(KinBodyConstPtr(pbody->GetBody()), preport)) {
// if there is a collision, revert to the original transform
RAVELOG_VERBOSE(str(boost::format("collision %s\n")%preport->__str__()));
_SetColor(COLLISION_COLOR);
}
else {
_SetColor(CHECK_COLOR);
}
_penv->GetCollisionChecker()->SetCollisionOptions(prevoptions);
}
}
}
}
virtual ~ViewerRecorder()
{
RAVELOG_VERBOSE("~ViewerRecorder\n");
_bContinueThread = false;
_Reset();
{
boost::mutex::scoped_lock lock(_mutex);
_condnewframe.notify_all();
}
_threadrecord->join();
}
int PlannerBase::PlannerParameters::SetStateValues(const std::vector<dReal>& values, int options) const
{
if( !!_setstatevaluesfn ) {
return _setstatevaluesfn(values, options);
}
if( !!_setstatefn ) {
RAVELOG_VERBOSE("Using deprecated PlannerParameters::_setstatefn, please set _setstatevaluesfn instead");
_setstatefn(values);
return 0;
}
throw openrave_exception("need to set PlannerParameters::_setstatevaluesfn");
}
extern "C" void openravepy_viewer_sigint_handler(int sig) //, siginfo_t *siginfo, void *context)
{
RAVELOG_VERBOSE("openravepy_viewer_sigint_handler\n");
openravepy::s_nInterruptCount += 1;
//PyErr_SetInterrupt();
//PyGILState_STATE gstate = PyGILState_Ensure();
// try
// {
// exec("raise KeyboardInterrupt()\n"); // boost::python::globals(),locals);
// //exec("import thread; thread.interrupt_main()\n"); // boost::python::globals(),locals);
// }
// catch( const error_already_set& e )
// {
// // should be the interrupt we just raised...
// //RAVELOG_WARN(boost::str(boost::format("failed to throw KeyboardInterrupt from signal handler: %s\n")%e));
// }
////
// PyGILState_Release(gstate);
// FOREACHC(itviewer, openravepy::s_listViewersToQuit) {
// ++(*itviewer)->_nSetInterrupt;
// }
// (*itviewer)->quitmainloop();
// }
// openravepy::s_listViewersToQuit.clear();
// is this call necessary? perhaps could get the C++ planners out of their loops?
// RaveDestroy();
#ifndef _WIN32
//struct sigaction act;
if( sigaction(SIGINT, &openravepy::s_signalActionPrev, NULL) < 0 ) {
RAVELOG_WARN("failed to restore old signal\n");
}
kill(0 /*getpid()*/, SIGINT);
//sigaction(SIGINT,&act,NULL);
#else
signal(SIGINT, openravepy::s_signalActionPrev);
#endif
}
bool InterfaceBase::SendCommand(ostream& sout, istream& sinput)
{
string cmd;
sinput >> cmd;
if( !sinput ) {
throw openrave_exception("invalid command",ORE_InvalidArguments);
}
boost::shared_ptr<InterfaceCommand> interfacecmd;
{
boost::shared_lock< boost::shared_mutex > lock(_mutexInterface);
CMDMAP::iterator it = __mapCommands.find(cmd);
if( it == __mapCommands.end() ) {
throw openrave_exception(str(boost::format("failed to find command '%s' in interface %s\n")%cmd.c_str()%GetXMLId()),ORE_CommandNotSupported);
}
interfacecmd = it->second;
}
if( !interfacecmd->fn(sout,sinput) ) {
RAVELOG_VERBOSE(str(boost::format("command failed in interface %s: %s\n")%GetXMLId()%cmd));
return false;
}
return true;
}
virtual bool InitEnvironment()
{
RAVELOG_VERBOSE("init bullet physics environment\n");
_space->SetSynchronizationCallback(boost::bind(&BulletPhysicsEngine::_SyncCallback, shared_physics(),_1));
_broadphase.reset(new btDbvtBroadphase());
//btVector3 worldMin(-1000,-1000,-1000);
//btVector3 worldMax(1000,1000,1000);
//_broadphase.reset(new btAxisSweep3(worldMin,worldMax));
// allowes configuration of collision detection
_collisionConfiguration.reset(new btDefaultCollisionConfiguration());
// handels conves and concave collisions
//_dispatcher = new btOpenraveDispatcher::btOpenraveDispatcher(_collisionConfiguration);
_dispatcher.reset(new btCollisionDispatcher(_collisionConfiguration.get()));
_solver.reset(new btSequentialImpulseConstraintSolver());
// btContinuousDynamicsWorld gives a segfault for some reason
_dynamicsWorld.reset(new btDiscreteDynamicsWorld(_dispatcher.get(),_broadphase.get(),_solver.get(),_collisionConfiguration.get()));
_filterCallback.reset(new PhysicsFilterCallback());
_dynamicsWorld->getPairCache()->setOverlapFilterCallback(_filterCallback.get());
btContactSolverInfo& solverInfo = _dynamicsWorld->getSolverInfo();
RAVELOG_DEBUG(str(boost::format("bullet dynamics: m_numIterations=%d, m_globalCfm=%e")%_solver_iterations%_global_contact_force_mixing));
solverInfo.m_numIterations = _solver_iterations;
solverInfo.m_globalCfm = _global_contact_force_mixing;
if(!_space->InitEnvironment(_dynamicsWorld)) {
return false;
}
vector<KinBodyPtr> vbodies;
GetEnv()->GetBodies(vbodies);
FOREACHC(itbody, vbodies) {
InitKinBody(*itbody);
}
void SetGlobalDAE(boost::shared_ptr<DAE> newdae)
{
RAVELOG_VERBOSE("resetting global collada DAE\n");
s_dae = newdae;
}
