void EditableSceneBodyImpl::attachPositionDragger(Link* link) { SceneLink* sceneLink = self->sceneLink(link->index()); double r = sceneLink->untransformedBoundingBox().boundingSphereRadius(); positionDragger->setRadius(r); sceneLink->addChild(positionDragger); }
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; }
void EditableSceneBodyImpl::attachPositionDragger(Link* link) { SceneLink* sceneLink = self->sceneLink(link->index()); positionDragger->adjustSize(sceneLink->untransformedBoundingBox()); sceneLink->addChild(positionDragger); }