visualization_msgs::MarkerArray OccupancyGrid::getVisualization(std::string type) { visualization_msgs::MarkerArray ma; if(type.compare("bounds") == 0) { double dimx, dimy, dimz, originx, originy, originz; std::vector<geometry_msgs::Point> pts(10); getOrigin(originx, originy, originz); getWorldSize(dimx,dimy,dimz); pts[0].x = originx; pts[0].y = originy; pts[0].z = originz; pts[1].x = originx+dimx; pts[1].y = originy; pts[1].z = originz; pts[2].x = originx+dimx; pts[2].y = originy+dimy; pts[2].z = originz; pts[3].x = originx; pts[3].y = originy+dimy; pts[3].z = originz; pts[4].x = originx; pts[4].y = originy; pts[4].z = originz; pts[5].x = originx; pts[5].y = originy; pts[5].z = originz+dimz; pts[6].x = originx+dimx; pts[6].y = originy; pts[6].z = originz+dimz; pts[7].x = originx+dimx; pts[7].y = originy+dimy; pts[7].z = originz+dimz; pts[8].x = originx; pts[8].y = originy+dimy; pts[8].z = originz+dimz; pts[9].x = originx; pts[9].y = originy; pts[9].z = originz+dimz; ma.markers.resize(1); ma.markers[0] = viz::getLineMarker(pts, 0.05, 10, getReferenceFrame(), "collision_space_bounds", 0); } else if(type.compare("distance_field") == 0) { visualization_msgs::Marker m; // grid_->getIsoSurfaceMarkers(0.01, 0.03, getReferenceFrame(), ros::Time::now(), Eigen::Affine3d::Identity(), m); //grid_->getIsoSurfaceMarkers(0.01, 0.08, getReferenceFrame(), ros::Time::now(), tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,0)), m); grid_->getIsoSurfaceMarkers(0.01, 0.02, getReferenceFrame(), ros::Time::now(), m); m.color.a +=0.2; ma.markers.push_back(m); } else if(type.compare("occupied_voxels") == 0) { visualization_msgs::Marker marker; std::vector<geometry_msgs::Point> voxels; getOccupiedVoxels(voxels); marker.header.seq = 0; marker.header.stamp = ros::Time::now(); marker.header.frame_id = getReferenceFrame(); marker.ns = "occupied_voxels"; marker.id = 1; marker.type = visualization_msgs::Marker::POINTS; marker.action = visualization_msgs::Marker::ADD; marker.lifetime = ros::Duration(0.0); marker.scale.x = grid_->getResolution() / 2.0; marker.scale.y = grid_->getResolution() / 2.0; marker.scale.z = grid_->getResolution() / 2.0; marker.color.r = 0.8; marker.color.g = 0.3; marker.color.b = 0.5; marker.color.a = 1; marker.points = voxels; ma.markers.push_back(marker); } else ROS_ERROR("No visualization found of type '%s'.", type.c_str()); return ma; }
bool AbsoluteModelTransform::computeWorldToLocalMatrix( osg::Matrix& matrix, osg::NodeVisitor* nv ) const { osg::Matrix inv( osg::Matrix::inverse( _matrix ) ); if( getReferenceFrame() == osg::Transform::ABSOLUTE_RF ) { osg::Matrix invView; if( !nv ) osg::notify( osg::NOTICE ) << "AbsoluteModelTransform: NULL NodeVisitor; can't get invView." << std::endl; else if( nv->getVisitorType() != osg::NodeVisitor::CULL_VISITOR ) osg::notify( osg::NOTICE ) << "AbsoluteModelTransform: NodeVisitor is not CullVisitor; can't get invView." << std::endl; else { osgUtil::CullVisitor* cv = dynamic_cast< osgUtil::CullVisitor* >( nv ); osg::Camera* cam = cv->getCurrentCamera(); cam->computeWorldToLocalMatrix( invView, cv ); } matrix = ( invView * inv ); } else // RELATIVE_RF matrix.postMult( inv ); return( true ); }
void Cylinder::getRandomSurfacePoint(Vector3& p, Vector3& N) const { float h = height(); float r = radius(); // Create a random point on a standard cylinder and then rotate to the global frame. // Relative areas (factor of 2PI already taken out) float capRelArea = square(r) / 2.0f; float sideRelArea = r * h; float r1 = uniformRandom(0, capRelArea * 2 + sideRelArea); if (r1 < capRelArea * 2) { // Select a point uniformly at random on a disk // @cite http://mathworld.wolfram.com/DiskPointPicking.html float a = uniformRandom(0, (float)twoPi()); float r2 = sqrt(uniformRandom(0, 1)) * r; p.x = cos(a) * r2; p.z = sin(a) * r2; N.x = 0; N.z = 0; if (r1 < capRelArea) { // Top p.y = h / 2.0f; N.y = 1; } else { // Bottom p.y = -h / 2.0f; N.y = -1; } } else { // Side float a = uniformRandom(0, (float)twoPi()); N.x = cos(a); N.y = 0; N.z = sin(a); p.x = N.x * r; p.z = N.y * r; p.y = uniformRandom(-h / 2.0f, h / 2.0f); } // Transform to world space CoordinateFrame cframe; getReferenceFrame(cframe); p = cframe.pointToWorldSpace(p); N = cframe.normalToWorldSpace(N); }
void osgParticleHPS::ModularEmitter::emit(double dt) { int n = counter_->numParticlesToCreate(dt); for (int i=0; i<n; ++i) { Particle *P = getParticleSystem()->createParticle(); if (P) { placer_->place(P); shooter_->shoot(P); if (getReferenceFrame() == RELATIVE_TO_PARENTS) { P->transformPositionVelocity(getLocalToWorldMatrix()); } } } }
Vector3 Cylinder::randomInteriorPoint() const { float h = height(); float r = radius(); // Create a random point in a standard cylinder and then rotate to the global frame. // Select a point uniformly at random on a disk // @cite http://mathworld.wolfram.com/DiskPointPicking.html float a = uniformRandom(0, (float)twoPi()); float r2 = sqrt(uniformRandom(0, 1)) * r; Vector3 p( cos(a) * r2, uniformRandom(-h / 2.0f, h / 2.0f), sin(a) * r2); // Transform to world space CoordinateFrame cframe; getReferenceFrame(cframe); return cframe.pointToWorldSpace(p); }
bool AbsoluteModelTransform::computeLocalToWorldMatrix( osg::Matrix& matrix, osg::NodeVisitor* nv ) const { if( getReferenceFrame() == osg::Transform::ABSOLUTE_RF ) { osg::Matrix view; if( !nv ) osg::notify( osg::INFO ) << "AbsoluteModelTransform: NULL NodeVisitor; can't get view." << std::endl; else if( nv->getVisitorType() != osg::NodeVisitor::CULL_VISITOR ) osg::notify( osg::INFO ) << "AbsoluteModelTransform: NodeVisitor is not CullVisitor; can't get view." << std::endl; else { osgUtil::CullVisitor* cv = dynamic_cast< osgUtil::CullVisitor* >( nv ); #ifdef SCENEVIEW_ANAGLYPHIC_STEREO_SUPPORT // If OSG_STEREO=ON is in the environment, SceneView hides the view matrix // in a stack rather than placing it in a Camera node. Enable this code // (using CMake) to use a less-efficient way to compute the view matrix that // is compatible with SceneView's usage. osg::NodePath np = nv->getNodePath(); np.pop_back(); osg::Matrix l2w = osg::computeLocalToWorld( np ); osg::Matrix invL2w = osg::Matrix::inverse( l2w ); view = invL2w * *( cv->getModelViewMatrix() ); #else // Faster way to determine the view matrix, but not compatible with // SceneView anaglyphic stereo. osg::Camera* cam = cv->getCurrentCamera(); cam->computeLocalToWorldMatrix( view, cv ); #endif } matrix = ( _matrix * view ); } else // RELATIVE_RF matrix.preMult(_matrix); return( true ); }
void Capsule::getRandomSurfacePoint(Vector3& p, Vector3& N) const { float h = height(); float r = radius(); // Create a random point on a standard capsule and then rotate to the global frame. // Relative areas float capRelArea = sqrt(r) / 2.0f; float sideRelArea = r * h; float r1 = uniformRandom(0, capRelArea * 2 + sideRelArea); if (r1 < capRelArea * 2) { // Select a point uniformly at random on a sphere N = Sphere(Vector3::zero(), 1).randomSurfacePoint(); p = N * r; p.y += sign(p.y) * h / 2.0f; } else { // Side float a = uniformRandom(0, (float)twoPi()); N.x = cos(a); N.y = 0; N.z = sin(a); p.x = N.x * r; p.z = N.y * r; p.y = uniformRandom(-h / 2.0f, h / 2.0f); } // Transform to world space CoordinateFrame cframe; getReferenceFrame(cframe); p = cframe.pointToWorldSpace(p); N = cframe.normalToWorldSpace(N); }
Vector3 Capsule::randomInteriorPoint() const { float h = height(); float r = radius(); // Create a random point in a standard capsule and then rotate to the global frame. Vector3 p; float hemiVolume = pi() * (r*r*r) * 4 / 6.0; float cylVolume = pi() * square(r) * h; float r1 = uniformRandom(0, 2.0 * hemiVolume + cylVolume); if (r1 < 2.0 * hemiVolume) { p = Sphere(Vector3::zero(), r).randomInteriorPoint(); p.y += sign(p.y) * h / 2.0f; } else { // Select a point uniformly at random on a disk float a = uniformRandom(0, (float)twoPi()); float r2 = sqrt(uniformRandom(0, 1)) * r; p = Vector3(cos(a) * r2, uniformRandom(-h / 2.0f, h / 2.0f), sin(a) * r2); } // Transform to world space CoordinateFrame cframe; getReferenceFrame(cframe); return cframe.pointToWorldSpace(p); }
/** * A map detector ID and Q ranges * This method looks unnecessary as it could be calculated on the fly but * the parallelization means that lazy instantation slows it down due to the * necessary CRITICAL sections required to update the cache. The Q range * values are required very frequently so the total time is more than * offset by this precaching step */ DetectorAngularCache initAngularCaches(const MatrixWorkspace *const workspace) { const size_t nhist = workspace->getNumberHistograms(); std::vector<double> thetas(nhist); std::vector<double> thetaWidths(nhist); std::vector<double> detectorHeights(nhist); auto inst = workspace->getInstrument(); const auto samplePos = inst->getSample()->getPos(); const V3D upDirVec = inst->getReferenceFrame()->vecPointingUp(); for (size_t i = 0; i < nhist; ++i) // signed for OpenMP { IDetector_const_sptr det; try { det = workspace->getDetector(i); } catch (Exception::NotFoundError &) { // Catch if no detector. Next line tests whether this happened - test // placed // outside here because Mac Intel compiler doesn't like 'continue' in a // catch // in an openmp block. } // If no detector found, skip onto the next spectrum if (!det || det->isMonitor()) { thetas[i] = -1.0; // Indicates a detector to skip thetaWidths[i] = -1.0; continue; } // We have to convert theta from radians to degrees const double theta = workspace->detectorSignedTwoTheta(*det) * rad2deg; thetas[i] = theta; /** * Determine width from shape geometry. A group is assumed to contain * detectors with the same shape & r, theta value, i.e. a ring mapped-group * The shape is retrieved and rotated to match the rotation of the detector. * The angular width is computed using the l2 distance from the sample */ if (auto group = boost::dynamic_pointer_cast<const DetectorGroup>(det)) { // assume they all have same shape and same r,theta auto dets = group->getDetectors(); det = dets[0]; } const auto pos = det->getPos() - samplePos; double l2(0.0), t(0.0), p(0.0); pos.getSpherical(l2, t, p); // Get the shape auto shape = det->shape(); // Defined in its own reference frame with centre at 0,0,0 BoundingBox bbox = shape->getBoundingBox(); auto maxPoint(bbox.maxPoint()); auto minPoint(bbox.minPoint()); auto span = maxPoint - minPoint; detectorHeights[i] = span.scalar_prod(upDirVec); thetaWidths[i] = 2.0 * std::fabs(std::atan((detectorHeights[i] / 2) / l2)) * 180.0 / M_PI; } DetectorAngularCache cache; cache.thetas = thetas; cache.thetaWidths = thetaWidths; cache.detectorHeights = detectorHeights; return cache; }