bool NamingContextHelper::checkOrUpdateNamingContext() { if(failedInLastAccessToNamingContext){ return false; } if (!CORBA::is_nil(namingContext)) { return true; } try { omniORB::setClientCallTimeout(500); CORBA::Object_var obj = getORB()->string_to_object(namingContextLocation.c_str()); namingContext = CosNaming::NamingContext::_narrow(obj); omniORB::setClientCallTimeout(namingContext, 150); if (CORBA::is_nil(namingContext)) { errorMessage_ = format("The object at {} is not a NamingContext object.", namingContextLocation); failedInLastAccessToNamingContext = true; } } catch (CORBA::SystemException& ex) { errorMessage_ = format("A NameService doesn't exist at \"{}\".", namingContextLocation); namingContext = CosNaming::NamingContext::_nil(); failedInLastAccessToNamingContext = true; } omniORB::setClientCallTimeout(0); // reset the global timeout setting? return (!CORBA::is_nil(namingContext)); }
void KinematicFaultCheckerImpl::putJointPositionFault(int frame, Link* joint, std::ostream& os) { if(frame > lastPosFaultFrames[joint->jointId()] + 1){ double q, l, u, m; if(joint->isRotationalJoint()){ q = degree(joint->q()); l = degree(joint->q_lower()); u = degree(joint->q_upper()); m = degree(angleMargin); } else { q = joint->q(); l = joint->q_lower(); u = joint->q_upper(); m = translationMargin; } if(m != 0.0){ os << format(_("{0:7.3f} [s]: Position limit over of {1} ({2} is beyond the range ({3} , {4}) with margin {5}.)"), (frame / frameRate), joint->name(), q, l, u, m) << endl; } else { os << format(_("{0:7.3f} [s]: Position limit over of {1} ({2} is beyond the range ({3} , {4}).)"), (frame / frameRate), joint->name(), q, l, u) << endl; } numFaults++; } lastPosFaultFrames[joint->jointId()] = frame; }
void SimulationBar::forEachSimulator(std::function<void(SimulatorItem* simulator)> callback, bool doSelect) { MessageView* mv = MessageView::instance(); /* ItemList<SimulatorItem> simulators = ItemTreeView::mainInstance()->selectedItems<SimulatorItem>(); */ ItemList<SimulatorItem> simulators = ItemTreeView::mainInstance()->selectedItems<SimulatorItem>(); if(simulators.empty()){ simulators.extractChildItems(RootItem::instance()); if(simulators.empty()){ mv->notify(_("There is no simulator item.")); } else if(simulators.size() > 1){ simulators.clear(); mv->notify(_("Please select a simulator item to simulate.")); } else { if(doSelect){ ItemTreeView::instance()->selectItem(simulators.front()); } } } typedef map<WorldItem*, SimulatorItem*> WorldToSimulatorMap; WorldToSimulatorMap worldToSimulator; for(size_t i=0; i < simulators.size(); ++i){ SimulatorItem* simulator = simulators.get(i); WorldItem* world = simulator->findOwnerItem<WorldItem>(); if(world){ WorldToSimulatorMap::iterator p = worldToSimulator.find(world); if(p == worldToSimulator.end()){ worldToSimulator[world] = simulator; } else { p->second = 0; // skip if multiple simulators are selected } } } for(size_t i=0; i < simulators.size(); ++i){ SimulatorItem* simulator = simulators.get(i); WorldItem* world = simulator->findOwnerItem<WorldItem>(); if(!world){ mv->notify(format(_("{} cannot be processed because it is not related with a world."), simulator->name())); } else { WorldToSimulatorMap::iterator p = worldToSimulator.find(world); if(p != worldToSimulator.end()){ if(!p->second){ mv->notify(format(_("{} cannot be processed because another simulator" "in the same world is also selected."), simulator->name())); } else { callback(simulator); } } } } }
SgNode* YAMLSceneLoaderImpl::load(const std::string& filename) { SgNodePtr scene; MappingPtr topNode; try { YAMLReader reader; topNode = reader.loadDocument(filename)->toMapping(); if(topNode){ boost::filesystem::path filepath(filename); sceneReader.setBaseDirectory(filepath.parent_path().string()); sceneReader.readHeader(*topNode); ValueNodePtr sceneElements = topNode->findMapping("scene"); if(!sceneElements->isValid()){ os() << format(_("Scene file \"{}\" does not have the \"scene\" node."), filename) << endl; } else { scene = sceneReader.readNodeList(*sceneElements); if(!scene){ os() << format(_("Scene file \"{}\" is an empty scene."), filename) << endl; scene = new SgNode; } } } } catch(const ValueNode::Exception& ex){ os() << ex.message(); } os().flush(); sceneReader.clear(); return scene.retn(); }
void KinematicFaultCheckerImpl::apply() { ItemList<BodyMotionItem> items = ItemTreeView::mainInstance()->selectedItems<BodyMotionItem>(); if(items.empty()){ mes.notify(_("No BodyMotionItems are selected.")); } else { for(size_t i=0; i < items.size(); ++i){ BodyMotionItem* motionItem = items.get(i); BodyItem* bodyItem = motionItem->findOwnerItem<BodyItem>(); if(!bodyItem){ mes.notify(format(_("{} is not owned by any BodyItem. Check skiped."), motionItem->name())); } else { mes.putln(); mes.notify(format(_("Applying the Kinematic Fault Checker to {} ..."), motionItem->headItem()->name())); dynamic_bitset<> linkSelection; if(selectedJointsRadio.isChecked()){ linkSelection = LinkSelectionView::mainInstance()->linkSelection(bodyItem); } else if(nonSelectedJointsRadio.isChecked()){ linkSelection = LinkSelectionView::mainInstance()->linkSelection(bodyItem); linkSelection.flip(); } else { linkSelection.resize(bodyItem->body()->numLinks(), true); } double beginningTime = 0.0; double endingTime = motionItem->motion()->getTimeLength(); std::numeric_limits<double>::max(); if(onlyTimeBarRangeCheck.isChecked()){ TimeBar* timeBar = TimeBar::instance(); beginningTime = timeBar->minTime(); endingTime = timeBar->maxTime(); } int n = checkFaults(bodyItem, motionItem, mes.cout(), positionCheck.isChecked(), velocityCheck.isChecked(), collisionCheck.isChecked(), linkSelection, beginningTime, endingTime); if(n > 0){ if(n == 1){ mes.notify(_("A fault has been detected.")); } else { mes.notify(format(_("{} faults have been detected."), n)); } } else { mes.notify(_("No faults have been detected.")); } } } } }
CORBA::Object_ptr NamingContextHelper::findObjectSub(std::vector<ObjectPath>& pathList) { CORBA::Object_ptr obj = CORBA::Object::_nil(); if(checkOrUpdateNamingContext()){ string fullName; CosNaming::Name ncName; ncName.length(pathList.size()); for(int index = 0; index < pathList.size(); index++){ ncName[index].id = CORBA::string_dup(pathList[index].id.c_str()); ncName[index].kind = CORBA::string_dup(pathList[index].kind.c_str()); if(0 < fullName.length()){ fullName = fullName + "/"; } fullName = fullName + pathList[index].id; } try { obj = namingContext->resolve(ncName); } catch (const CosNaming::NamingContext::NotFound &ex) { errorMessage_ = format("\"{}\" is not found: ", fullName); switch (ex.why) { case CosNaming::NamingContext::missing_node: errorMessage_ += "Missing Node"; break; case CosNaming::NamingContext::not_context: errorMessage_ += "Not Context"; break; case CosNaming::NamingContext::not_object: errorMessage_ += "Not Object"; break; default: errorMessage_ += "Unknown Error"; break; } } catch (CosNaming::NamingContext::CannotProceed &exc) { errorMessage_ = format("Resolving \"{}\" cannot be proceeded.", fullName); } catch (CosNaming::NamingContext::AlreadyBound &exc) { errorMessage_ = format("\"{}\" has already been bound.", fullName); } catch (const CORBA::TRANSIENT &) { errorMessage_ = format("Resolving \"{}\" failed with the TRANSIENT exception.", fullName); } } return obj; }
bool YAMLReaderImpl::load(const std::string& filename) { yaml_parser_initialize(&parser); clearDocuments(); if(isRegularMultiListingExpected){ expectedListingSizes.clear(); } currentDocumentIndex = 0; bool result = false; FILE* file = fopen(filename.c_str(), "rb"); if(file==NULL){ errorMessage = strerror(errno); } else { yaml_parser_set_input_file(&parser, file); try { result = parse(); } catch(const ValueNode::Exception& ex){ errorMessage = format(_("{0} at line {1}, column {2}"), ex.message(), ex.line(), ex.column()); } fclose(file); } return result; }
/** \todo Use integated nested map whose node is a single path element to be more efficient. */ std::string ParametricPathProcessor::parameterize(const std::string& orgPathString) { filesystem::path orgPath(orgPathString); // In the case where the path is originally relative one if(!orgPath.is_complete()){ return getGenericPathString(orgPath); } else { filesystem::path relativePath; if(!impl->baseDirPath.empty() && findSubDirectory(impl->baseDirPath, orgPath, relativePath)){ return getGenericPathString(relativePath); } else { string varName; if(impl->findSubDirectoryOfDirectoryVariable(orgPath, varName, relativePath)){ return format("${{{0}}}/{1}", varName, getGenericPathString(relativePath)); } else if(findSubDirectory(impl->shareDirPath, orgPath, relativePath)){ return string("${SHARE}/") + getGenericPathString(relativePath); } else if(findSubDirectory(impl->topDirPath, orgPath, relativePath)){ return string("${PROGRAM_TOP}/") + getGenericPathString(relativePath); } else if(!impl->isBaseDirInHome && findSubDirectory(impl->homeDirPath, orgPath, relativePath)){ return string("${HOME}/") + getGenericPathString(relativePath); } else if(!impl->baseDirPath.empty() && findRelativePath(impl->baseDirPath, orgPath, relativePath)){ return getGenericPathString(relativePath); } } } return getGenericPathString(orgPath); }
void cnoid::initializeCorbaUtil(bool activatePOAManager, int listeningPort) { if (orb) { return; // already initialized } int numArgs = (listeningPort >= 0) ? 5 : 1; char** argv = new char*[numArgs]; int argc = 0; argv[argc++] = (char*)"choreonoid"; string endpoint; if (listeningPort >= 0) { endpoint = format("giop:tcp::{}", listeningPort); argv[argc++] = (char*)"-ORBendPoint"; argv[argc++] = (char*)endpoint.c_str(); argv[argc++] = (char*)"-ORBpoaUniquePersistentSystemIds"; argv[argc++] = (char*)"1"; } // Initialize ORB orb = CORBA::ORB_init(argc, argv); delete[] argv; initializeCorbaUtil(orb, activatePOAManager); }
void Source::initializePlayback(double time) { pa_threaded_mainloop_lock(manager->mainloop); waitForOperation(); if(!isActive_ && connectStream()){ seek(time); initialWritingDone = false; size_t writableSize = pa_stream_writable_size(stream); if(writableSize <= 0){ manager->os << format(_("PulseAudio stream for {} cannot be written."), audioItem->name()) << endl; disconnectStream(); } else { timeToFinish = std::numeric_limits<double>::max(); write(writableSize, true); isActive_ = true; pa_stream_set_write_callback(stream, pa_stream_write_callback, (void*)this); pa_stream_set_overflow_callback(stream, pa_stream_overflow_notify_callback, (void*)this); pa_stream_set_underflow_callback(stream, pa_stream_underflow_notify_callback, (void*)this); } } pa_threaded_mainloop_unlock(manager->mainloop); }
bool AizuWheelController::initialize(SimpleControllerIO* io) { body = io->body(); dt = io->timeStep(); actuationMode = Link::JOINT_TORQUE; string option = io->optionString(); if(!option.empty()){ if(option == "velocity" || option == "position"){ actuationMode = Link::JOINT_VELOCITY; } else if(option == "torque"){ actuationMode = Link::JOINT_TORQUE; } else { io->os() << format("Warning: Unknown option \"{}\".", option) << endl; } } vector<string> wheelNames = { "L_WHEEL", "R_WHEEL" }; if(!initializeWheels(io, wheelNames)){ return false; } joystick = io->getOrCreateSharedObject<SharedJoystick>("joystick"); targetMode = joystick->addMode(); return true; }
void LightingProgram::initialize() { numLightsLocation = getUniformLocation("numLights"); lightInfos.resize(maxNumLights()); string lightFormat("lights[{}]."); for(int i=0; i < maxNumLights(); ++i){ LightInfo& light = lightInfos[i]; string prefix = format(lightFormat, i); light.positionLocation = getUniformLocation(prefix + "position"); light.intensityLocation = getUniformLocation(prefix + "intensity"); light.ambientIntensityLocation = getUniformLocation(prefix + "ambientIntensity"); light.constantAttenuationLocation = getUniformLocation(prefix + "constantAttenuation"); light.linearAttenuationLocation = getUniformLocation(prefix + "linearAttenuation"); light.quadraticAttenuationLocation = getUniformLocation(prefix + "quadraticAttenuation"); light.cutoffAngleLocation = getUniformLocation(prefix + "cutoffAngle"); light.beamWidthLocation = getUniformLocation(prefix + "beamWidth"); light.cutoffExponentLocation = getUniformLocation(prefix + "cutoffExponent"); light.directionLocation = getUniformLocation(prefix + "direction"); } maxFogDistLocation = getUniformLocation("maxFogDist"); minFogDistLocation = getUniformLocation("minFogDist"); fogColorLocation = getUniformLocation("fogColor"); isFogEnabledLocation = getUniformLocation("isFogEnabled"); }
SgNode* VRMLSceneLoaderImpl::load(const std::string& filename) { converter.clearConvertedNodeMap(); SgGroupPtr top = new SgGroup; try { parser.load(filename); while(VRMLNodePtr vrml = parser.readNode()){ SgNodePtr node = converter.convert(vrml); if(node){ top->addChild(node); } } parser.checkEOF(); } catch(EasyScanner::Exception& ex){ os() << ex.getFullMessage() << endl; return 0; } if(top->empty()){ os() << format(_("VRML file \"{}\" does not have any valid entity."), filename) << endl; return 0; } if(top->numChildren() == 1){ return top->child(0); } return top.retn(); }
bool ZMPSeqItem::makeRootRelative(bool on) { BodyMotionItem* bodyMotionItem = dynamic_cast<BodyMotionItem*>(parentItem()); if(bodyMotionItem){ if(cnoid::makeRootRelative(*zmpseq_, *bodyMotionItem->motion(), on)){ mvout() << format(_("{0} of {1} has been converted to {2}."), name(), bodyMotionItem->name(), (on ? _("the root relative coordinate") : _("the global coordinate"))) << endl; return true; } } mvout() << format(_("{0}'s coordinate system cannot be changed " "because there is no root link motion associated with {0}."), name()) << endl; return false; }
bool MediaItem::setMediaFilePath(const std::string& filepath) { mediaFilePath_.clear(); mediaURI_.clear(); filesystem::path fpath(filepath); if(filesystem::exists(fpath) && !filesystem::is_directory(fpath)){ mediaFilePath_ = filepath; filesystem::path fullpath = getAbsolutePath(fpath); mediaURI_ = format("file://{}", getPathString(fullpath)); return true; } else { lastErrorMessage_ = format(_("Media file \"{}\" does not exist."), filepath); return false; } }
void YAMLReaderImpl::setAnchor(ValueNode* node, yaml_char_t* anchor, const yaml_mark_t& mark) { pair<AnchorMap::iterator, bool> inserted = anchorMap.insert(AnchorMap::value_type((char*)anchor, node)); if(!inserted.second){ ValueNode::Exception ex; ex.setMessage(format(_("Anchor \"{}\" is duplicated"), (char*)anchor)); ex.setPosition(mark.line, mark.column); throw ex; } }
void PythonPlugin::executeScriptFileOnStartup(const string& scriptFile) { MessageView::instance()->putln(format(_("Executing python script \"{}\" ..."), scriptFile)); executor().execFile(scriptFile); if(!executor().hasException()){ MessageView::instance()->putln(_("The script finished.")); } else { MessageView::instance()->putln(MessageView::WARNING, _("Failed to run the python script.")); python::gil_scoped_acquire lock; MessageView::instance()->put(executor().exceptionText()); } }
void NamingContextHelper::setLocation(const std::string& host, int port) { if(failedInLastAccessToNamingContext){ if(host != host_ || port != port_){ failedInLastAccessToNamingContext = false; } } host_ = host; port_ = port; namingContextLocation = format("corbaloc:iiop:{0}:{1}/NameService", host, port); namingContext = CosNaming::NamingContext::_nil(); }
void Source::adjustTime(const char* reason) { pa_threaded_mainloop_lock(manager->mainloop); double time = manager->timeBar->realPlaybackTime(); seek(time); doAdjustTime = true; pa_threaded_mainloop_unlock(manager->mainloop); manager->os << format(_("PulseAudioManager: Buffer of {0} {1}. Its playback time is adjusted to {2}."), audioItem->name(), reason, time) << endl; }
bool AbstractSeq::doWriteSeq(YAMLWriter& writer, std::function<void()> additionalPartCallback) { if(seqType_.empty()){ if(contentName_.empty()){ writer.putMessage(_("The type of the sequence to write is unknown.\n")); } else { writer.putMessage(format(_("The type of the {} sequence to write is unknown.\n"), contentName_)); } return false; } const double frameRate = getFrameRate(); if(frameRate <= 0.0){ writer.putMessage( format(_("Frame rate {0} of {1} is invalid.\n"), frameRate, (contentName_.empty() ? seqType_ : contentName_))); return false; } writer.startMapping(); writer.putKeyValue("type", seqType_); if(!contentName_.empty()){ writer.putKeyValue("content", contentName_); } double version = writer.info("formatVersion", 0.0); if(version == 0.0 || !writer.info("isComponent", false)){ writer.putKeyValue("formatVersion", version == 0.0 ? 2.0 : version); } writer.putKeyValue("frameRate", frameRate); writer.putKeyValue("numFrames", getNumFrames()); if(additionalPartCallback) additionalPartCallback(); writer.endMapping(); return true; }
MaterialTable* WorldItemImpl::getOrLoadMaterialTable(bool checkFileUpdate) { bool failedToLoad = false; if(!materialTable){ materialTable = new MaterialTable; if(materialTable->load(materialTableFile, os)){ materialTableTimestamp = filesystem::last_write_time(materialTableFile); } else { failedToLoad = true; } } else if(checkFileUpdate){ if(!materialTableFile.empty()){ filesystem::path fpath(materialTableFile); if(filesystem::exists(fpath)){ std::time_t newTimestamp = filesystem::last_write_time(materialTableFile); if(newTimestamp > materialTableTimestamp){ MaterialTablePtr newMaterialTable = new MaterialTable; if(newMaterialTable->load(materialTableFile, os)){ materialTable = newMaterialTable; materialTableTimestamp = newTimestamp; MessageView::instance()->putln( format(_("Material table \"{}\" has been reloaded."), materialTableFile)); } else { failedToLoad = true; } } } } } if(failedToLoad){ MessageView::instance()->putln( format(_("Reloading material table \"{}\" failed."), materialTableFile), MessageView::WARNING); } return materialTable; }
void BodyStateSubscriberRTCItemImpl::createRTC() { deleteRTC(); if(!bodyItem){ return; } const string param( "VisionSensorSubscriber?" "instance_name={0}&" "exec_cxt.periodic.type=PeriodicExecutionContext&" "exec_cxt.periodic.rate={1}"); RTC::RtcBase* rtc = createManagedRTC(format(param, self->name(), periodicRate)); if(!rtc){ mv->putln(format(_("RTC for \"{}\" cannot be created."), self->name()), MessageView::ERROR); return; } subscriberRTC = dynamic_cast<SubscriberRTC*>(rtc); // For the backward compatibility bool isVisionSensorSubscriber = dynamic_cast<VisionSensorSubscriberRTCItem*>(self); subscriberRTC->createInPorts(bodyItem, pointCloudPortType.which(), isVisionSensorSubscriber); execContext = RTC::ExecutionContext::_nil(); RTC::ExecutionContextList_var eclist = rtc->get_owned_contexts(); for(CORBA::ULong i=0; i < eclist->length(); ++i){ execContext = eclist[i]; if(!CORBA::is_nil(execContext)){ execContext->activate_component(subscriberRTC->getObjRef()); break; } } }
JoiningBoundedTimeline<void> detectVoiceActivity(const AudioClip& inputAudioClip, int maxThreadCount, ProgressSink& progressSink) { // Prepare audio for VAD const unique_ptr<AudioClip> audioClip = inputAudioClip.clone() | resample(16000) | removeDcOffset(); JoiningBoundedTimeline<void> activity(audioClip->getTruncatedRange()); std::mutex activityMutex; // Split audio into segments and perform parallel VAD const int segmentCount = maxThreadCount; centiseconds audioDuration = audioClip->getTruncatedRange().getDuration(); vector<TimeRange> audioSegments; for (int i = 0; i < segmentCount; ++i) { TimeRange segmentRange = TimeRange(i * audioDuration / segmentCount, (i + 1) * audioDuration / segmentCount); audioSegments.push_back(segmentRange); } runParallel([&](const TimeRange& segmentRange, ProgressSink& segmentProgressSink) { unique_ptr<AudioClip> audioSegment = audioClip->clone() | segment(segmentRange); JoiningBoundedTimeline<void> activitySegment = webRtcDetectVoiceActivity(*audioSegment, segmentProgressSink); std::lock_guard<std::mutex> lock(activityMutex); for (auto activityRange : activitySegment) { activityRange.getTimeRange().shift(segmentRange.getStart()); activity.set(activityRange); } }, audioSegments, segmentCount, progressSink); // Fill small gaps in activity const centiseconds maxGap(5); for (const auto& pair : getPairs(activity)) { if (pair.second.getStart() - pair.first.getEnd() <= maxGap) { activity.set(pair.first.getEnd(), pair.second.getStart()); } } // Shorten activities. WebRTC adds a bit of buffer at the end. const centiseconds tail(5); for (const auto& utterance : JoiningBoundedTimeline<void>(activity)) { if (utterance.getDuration() > tail && utterance.getEnd() < audioDuration) { activity.clear(utterance.getEnd() - tail, utterance.getEnd()); } } logging::debugFormat("Found {} sections of voice activity: {}", activity.size(), join(activity | transformed([](const Timed<void>& t) { return format("{0}-{1}", t.getStart(), t.getEnd()); }), ", ")); return activity; }
boost::optional<std::string> ParametricPathProcessorImpl::expand(const std::string& pathString) { if(!isVariableRegexAssinged){ variableRegex = "^\\$\\{(\\w+)\\}"; isVariableRegexAssinged = true; } filesystem::path path; smatch match; if(!regex_search(pathString, match, variableRegex)){ path = pathString; } else { int pos = match.position(); int len = match.length(); string varname = match.str(1); string expanded(pathString); if(varname == "SHARE"){ expanded.replace(pos, len, shareDirString); } else if(varname == "PROGRAM_TOP"){ expanded.replace(pos, len, topDirString); } else if(varname == "HOME"){ expanded.replace(pos, len, homeDirString); } else if(varname == "PROJECT_DIR"){ if(projectDirString.empty()){ errorMessage = format(_("PROJECT_DIR of \"{}\" cannot be expanded."), pathString); return boost::none; } expanded.replace(pos, len, projectDirString); } else { if(!replaceDirectoryVariable(expanded, varname, pos, len)){ return boost::none; } } path = expanded; } if(checkAbsolute(path) || baseDirPath.empty()){ return getNativePathString(path); } else { filesystem::path fullPath = baseDirPath / path; if(!path.empty() && (*path.begin() == "..")){ filesystem::path compact(getCompactPath(fullPath)); return getNativePathString(compact); } else { return getNativePathString(fullPath); } } }
void PhongShadowProgram::initializeShadowInfo(int index) { ShadowInfo& shadow = shadowInfos[index]; shadow.shadowMatrixLocation = getUniformLocation(format("shadowMatrices[{}]", index)); string prefix = format("shadows[{}].", index); shadow.lightIndexLocation = getUniformLocation(prefix + "lightIndex"); shadow.shadowMapLocation = getUniformLocation(prefix + "shadowMap"); static const GLfloat border[] = { 1.0f, 0.0f, 0.0f, 0.0f }; glGenTextures(1, &shadow.depthTexture); glActiveTexture(GL_TEXTURE1 + index); glBindTexture(GL_TEXTURE_2D, shadow.depthTexture); glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT24, shadowMapWidth_, shadowMapHeight_, 0, GL_DEPTH_COMPONENT, GL_FLOAT, NULL); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER); glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, border); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_COMPARE_REF_TO_TEXTURE); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LESS); glGenFramebuffers(1, &shadow.frameBuffer); glBindFramebuffer(GL_FRAMEBUFFER, shadow.frameBuffer); glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, shadow.depthTexture, 0); glDrawBuffer(GL_NONE); glReadBuffer(GL_NONE); GLenum result = glCheckFramebufferStatus(GL_FRAMEBUFFER); if(result != GL_FRAMEBUFFER_COMPLETE) { throw Exception("Framebuffer is not complete.\n"); } }
ValueNode* YAMLReader::document(int index) { if(index >= static_cast<int>(impl->documents.size())){ ValueNode::DocumentNotFoundException ex; if(index == 0){ ex.setMessage(_("The yaml file does not contains any documents.")); } else { ex.setMessage( format(_("The yaml file does not contains {}-th document."), index)); } ex.setPosition(-1, -1); throw ex; } return impl->documents[index]; }
void YAMLReaderImpl::onAlias(yaml_event_t& event) { if(debugTrace){ cout << "YAMLReaderImpl::onAlias()" << endl; } auto node = self->findAnchoredNode((char*)event.data.alias.anchor); if(!node){ ValueNode::Exception ex; ex.setMessage(format(_("Anchor \"{}\" is not defined"), (char*)event.data.alias.anchor)); const yaml_mark_t& mark = event.start_mark; ex.setPosition(mark.line, mark.column); throw ex; } addNode(node, event); }
bool AizuWheelController::initializeWheels(SimpleControllerIO* io, vector<string>& names) { wheels.clear(); qprev.clear(); for(auto& name : names){ auto link = body->link(name); if(!link){ io->os() << format("{0} of {1} is not found", name, body->name()) << endl; return false; } link->setActuationMode(actuationMode); io->enableOutput(link); wheels.push_back(link); qprev.push_back(link->q()); } return true; }
bool ParametricPathProcessorImpl::replaceDirectoryVariable (string& io_pathString, const string& varname, int pos, int len) { Listing* paths = variables->findListing(varname); if(paths){ for(int i=0; i < paths->size(); ++i){ string vpath; string replaced(io_pathString); replaced.replace(pos, len, paths->at(i)->toString()); filesystem::file_status fstatus = filesystem::status(filesystem::path(replaced)); if(filesystem::is_directory(fstatus) || filesystem::exists(fstatus)) { io_pathString = replaced; return true; } } } errorMessage = format(_("{0} of \"{1}\" cannot be expanded."), varname, io_pathString); return false; }
void KinematicFaultCheckerImpl::putSelfCollision(Body* body, int frame, const CollisionPair& collisionPair, std::ostream& os) { bool putMessage = false; GeometryPair gPair(collisionPair.geometries()); auto p = lastCollisionFrames.find(gPair); if(p == lastCollisionFrames.end()){ putMessage = true; lastCollisionFrames[gPair] = frame; } else { if(frame > p->second + 1){ putMessage = true; } p->second = frame; } if(putMessage){ Link* link0 = static_cast<Link*>(collisionPair.object(0)); Link* link1 = static_cast<Link*>(collisionPair.object(1)); os << format(_("{0:7.3f} [s]: Collision between {1} and {2}"), (frame / frameRate), link0->name(), link1->name()) << endl; numFaults++; } }