void VisionRenderer::updateScene(bool updateSensorForRenderingThread)
{
for(size_t i=0; i < sceneBodies.size(); ++i){
SceneBody* sceneBody = sceneBodies[i];
sceneBody->updateLinkPositions();
sceneBody->updateSceneDevices();
}
if(updateSensorForRenderingThread){
deviceForRendering->copyStateFrom(*device);
}
}
void VisionRenderer::renderInSimulationThread()
{
for(size_t i=0; i < sceneBodies.size(); ++i){
SceneBody* sceneBody = sceneBodies[i];
sceneBody->updateLinkPositions();
sceneBody->updateSceneDevices();
}
pixelBuffer->makeCurrent();
renderer.render();
renderer.flush();
pixelBuffer->doneCurrent();
}
void VisionRenderer::startConcurrentRendering()
{
{
boost::unique_lock<boost::mutex> lock(renderingMutex);
for(size_t i=0; i < sceneBodies.size(); ++i){
SceneBody* sceneBody = sceneBodies[i];
sceneBody->updateLinkPositions();
sceneBody->updateSceneDevices();
}
sensorForRendering->copyStateFrom(*sensor);
isRenderingRequested = true;
}
renderingCondition.notify_all();
}
/**
\todo use cache of the cloned scene graph nodes
*/
void VisionRenderer::initializeScene(const vector<SimulationBody*>& simBodies)
{
sceneGroup = new SgGroup;
#ifndef CNOID_REFERENCED_USE_ATOMIC_COUNTER
simImpl->cloneMap.clear();
#endif
// create scene bodies
for(size_t i=0; i < simBodies.size(); ++i){
SceneBody* sceneBody = new SceneBody(simBodies[i]->body());
#ifndef CNOID_REFERENCED_USE_ATOMIC_COUNTER
sceneBody->cloneShapes(simImpl->cloneMap);
#endif
sceneBodies.push_back(sceneBody);
sceneGroup->addChild(sceneBody);
}
if(simImpl->shootAllSceneObjects){
WorldItem* worldItem = simImpl->self->findOwnerItem<WorldItem>();
if(worldItem){
ItemList<> items;
items.extractChildItems(worldItem);
for(size_t i=0; i < items.size(); ++i){
Item* item = items.get(i);
SceneProvider* sceneProvider = dynamic_cast<SceneProvider*>(item);
if(sceneProvider && !dynamic_cast<BodyItem*>(item)){
SgNode* scene = sceneProvider->getScene(simImpl->cloneMap);
if(scene){
sceneGroup->addChild(scene);
}
}
}
}
}
}
SgCamera* VisionRenderer::initializeCamera()
{
SgCamera* sceneCamera = 0;
SceneBody* sceneBody = sceneBodies[bodyIndex];
if(camera){
SceneDevice* sceneDevice = sceneBody->getSceneDevice(device);
if(sceneDevice){
sceneCamera = sceneDevice->findNodeOfType<SgCamera>();
pixelWidth = camera->resolutionX();
pixelHeight = camera->resolutionY();
double frameRate = std::max(0.1, std::min(camera->frameRate(), simImpl->maxFrameRate));
cycleTime = 1.0 / frameRate;
if(simImpl->isVisionDataRecordingEnabled){
camera->setImageStateClonable(true);
}
}
} else if(rangeSensor){
const double thresh = (170.0 * PI / 180.0); // 170[deg]
bool isWithinPossibleRanges =
(rangeSensor->yawRange() < thresh) && (rangeSensor->pitchRange() < thresh);
if(isWithinPossibleRanges){
SceneLink* sceneLink = sceneBody->sceneLink(rangeSensor->link()->index());
if(sceneLink){
SgPerspectiveCamera* persCamera = new SgPerspectiveCamera;
sceneCamera = persCamera;
persCamera->setNearDistance(rangeSensor->minDistance());
persCamera->setFarDistance(rangeSensor->maxDistance());
SgPosTransform* cameraPos = new SgPosTransform();
cameraPos->setTransform(rangeSensor->T_local());
cameraPos->addChild(persCamera);
sceneLink->addChild(cameraPos);
if(rangeSensor->yawRange() > rangeSensor->pitchRange()){
pixelWidth = rangeSensor->yawResolution() * simImpl->rangeSensorPrecisionRatio;
if(rangeSensor->pitchRange() == 0.0){
pixelHeight = 1;
double r = tan(rangeSensor->yawRange() / 2.0) * 2.0 / pixelWidth;
persCamera->setFieldOfView(atan2(r / 2.0, 1.0) * 2.0);
} else {
pixelHeight = rangeSensor->pitchResolution() * simImpl->rangeSensorPrecisionRatio;
persCamera->setFieldOfView(rangeSensor->pitchRange());
}
} else {
pixelHeight = rangeSensor->pitchResolution() * simImpl->rangeSensorPrecisionRatio;
if(rangeSensor->yawRange() == 0.0){
pixelWidth = 1;
double r = tan(rangeSensor->pitchRange() / 2.0) * 2.0 / pixelHeight;
persCamera->setFieldOfView(atan2(r / 2.0, 1.0) * 2.0);
} else {
pixelWidth = rangeSensor->yawResolution() * simImpl->rangeSensorPrecisionRatio;
persCamera->setFieldOfView(rangeSensor->yawRange());
}
}
double frameRate = std::max(0.1, std::min(rangeSensor->frameRate(), simImpl->maxFrameRate));
cycleTime = 1.0 / frameRate;
if(simImpl->isVisionDataRecordingEnabled){
rangeSensor->setRangeDataStateClonable(true);
}
depthError = simImpl->depthError;
}
}
}
return sceneCamera;
}
