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;
}
