bool Panel::injectMouseMoved(const Ogre::Ray& ray)
{
Ogre::Matrix4 transform;
transform.makeTransform(mNode->getPosition(), mNode->getScale(), mNode->getOrientation());
Ogre::AxisAlignedBox aabb = mScreenRenderable->getBoundingBox();
aabb.transform(transform);
pair<bool, Ogre::Real> result = Ogre::Math::intersects(ray, aabb);
if (result.first == false)
{
unOverAllElements();
return false;
}
Ogre::Vector3 a,b,c,d;
Ogre::Vector2 halfSize = (mSize/100) * 0.5f;
a = transform * Ogre::Vector3(-halfSize.x,-halfSize.y,0);
b = transform * Ogre::Vector3( halfSize.x,-halfSize.y,0);
c = transform * Ogre::Vector3(-halfSize.x, halfSize.y,0);
d = transform * Ogre::Vector3( halfSize.x, halfSize.y,0);
result = Ogre::Math::intersects(ray, c, b, a);
if (result.first == false)
result = Ogre::Math::intersects(ray, c, d, b);
if (result.first == false)
{
unOverAllElements();
return false;
}
if (result.second > mDistanceFromPanelToInteractWith)
{
unOverAllElements();
return false;
}
Ogre::Vector3 hitPos = (ray.getOrigin() + (ray.getDirection() * result.second));
Ogre::Vector3 localPos = transform.inverse() * hitPos;
localPos.x += halfSize.x;
localPos.y -= halfSize.y;
localPos.x *= 100;
localPos.y *= 100;
// Cursor clip
localPos.x = Ogre::Math::Clamp<Ogre::Real>(localPos.x, 0, mSize.x - 10);
localPos.y = Ogre::Math::Clamp<Ogre::Real>(-localPos.y, 0, mSize.y - 18);
mInternalMousePos = Ogre::Vector2(localPos.x, localPos.y);
mMousePointer->position(mInternalMousePos);
// Let's actualize the "over" for each elements
for (size_t i=0; i < mPanelElements.size(); i++)
mPanelElements[i]->isOver(mInternalMousePos);
return true;
}
Button* check(const Ogre::Ray& ray, bool& isOver)
{
isOver = false;
Ogre::Matrix4 transform;
transform.makeTransform(mNode->getPosition(), mNode->getScale(), mNode->getOrientation());
Ogre::AxisAlignedBox aabb = mScreen->getBoundingBox();
aabb.transform(transform);
std::pair<bool, Ogre::Real> result = Ogre::Math::intersects(ray, aabb);
if (result.first == false)
return 0;
Ogre::Vector3 a,b,c,d;
Ogre::Vector2 halfSize = mSize * 0.5f;
a = transform * Ogre::Vector3(-halfSize.x,-halfSize.y,0);
b = transform * Ogre::Vector3( halfSize.x,-halfSize.y,0);
c = transform * Ogre::Vector3(-halfSize.x, halfSize.y,0);
d = transform * Ogre::Vector3( halfSize.x, halfSize.y,0);
result = Ogre::Math::intersects(ray, c, b, a);
if (result.first == false)
result = Ogre::Math::intersects(ray, c, d, b);
if (result.first == false)
return 0;
if (result.second > 6.0f)
return 0;
isOver = true;
Ogre::Vector3 hitPos = ( ray.getOrigin() + (ray.getDirection() * result.second) );
Ogre::Vector3 localPos = transform.inverse() * hitPos;
localPos.x += halfSize.x;
localPos.y -= halfSize.y;
localPos.x *= 100;
localPos.y *= 100;
// Cursor clip
localPos.x = Ogre::Math::Clamp<Ogre::Real>(localPos.x, 0, (mSize.x * 100) - 10);
localPos.y = Ogre::Math::Clamp<Ogre::Real>(-localPos.y, 0, (mSize.y * 100) - 18);
mMousePointer->position(localPos.x, localPos.y);
for (size_t i=0;i < mButtons.size();i++)
{
if (mButtons[i]->isOver(mMousePointer->position()))
return mButtons[i];
}
return 0;
}
Ogre::AxisAlignedBox AACamera::GetBoundingBox(bool transformed) const
{
Ogre::AxisAlignedBox box = Box;
if (!transformed)
return box;
Ogre::Matrix4 transforms;
//Node->getWorldTransforms(&transforms);
transforms.makeTransform(Node->getPosition(),Node->getScale(),Node->getOrientation());
box.transform(transforms);
return box;
}
void OgreCollada::MeshWriter::pass1Finish() {
// build scene graph and record transformations for each geometry instantiation
// determine initial transformation
Ogre::Matrix4 xform;
xform.makeTransform(Ogre::Vector3::ZERO, // no translation
Ogre::Vector3(m_ColladaScale.x, m_ColladaScale.y, m_ColladaScale.z),
Ogre::Quaternion(m_ColladaRotation.w, m_ColladaRotation.x, m_ColladaRotation.y, m_ColladaRotation.z));
// recursively find geometry instances and their transforms
for (size_t i = 0; i < m_vsRootNodes.size(); ++i) {
createSceneDFS(m_vsRootNodes[i], xform);
}
// create manualobject for use by pass2 writeGeometry calls
m_manobj = new Ogre::ManualObject(m_vsRootNodes[0]->getName() + "_mobj");
}
float3 EC_WaterPlane::GetPointOnPlane(const float3 &point) const
{
if (node_ == 0)
return float3::nan;
Ogre::Quaternion rot = node_->_getDerivedOrientation();
Ogre::Vector3 trans = node_->_getDerivedPosition();
Ogre::Vector3 scale = node_->_getDerivedScale();
Ogre::Matrix4 worldTM;
worldTM.makeTransform(trans, scale, rot);
// In Ogre 1.7.1 we could simply use the following line, but since we're also supporting Ogre 1.6.4 for now, the above
// lines are used instead, which work in both.
// Ogre::Matrix4 worldTM = node_->_getFullTransform(); // local->world.
Ogre::Matrix4 inv = worldTM.inverse(); // world->local
Ogre::Vector4 local = inv * Ogre::Vector4(point.x, point.y, point.z, 1.f);
local.y = 0;
Ogre::Vector4 world = worldTM * local;
return float3(world.x, world.y, world.z);
}
void NIFMeshLoader::createSubMesh(Ogre::Mesh *mesh, const Nif::NiTriShape *shape)
{
const Nif::NiTriShapeData *data = shape->data.getPtr();
const Nif::NiSkinInstance *skin = (shape->skin.empty() ? NULL : shape->skin.getPtr());
std::vector<Ogre::Vector3> srcVerts = data->vertices;
std::vector<Ogre::Vector3> srcNorms = data->normals;
Ogre::HardwareBuffer::Usage vertUsage = Ogre::HardwareBuffer::HBU_STATIC;
bool vertShadowBuffer = false;
bool geomMorpherController = false;
if(!shape->controller.empty())
{
Nif::ControllerPtr ctrl = shape->controller;
do {
if(ctrl->recType == Nif::RC_NiGeomMorpherController)
{
vertUsage = Ogre::HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY;
vertShadowBuffer = true;
geomMorpherController = true;
break;
}
} while(!(ctrl=ctrl->next).empty());
}
if(skin != NULL)
{
vertUsage = Ogre::HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY;
vertShadowBuffer = true;
// Only set a skeleton when skinning. Unskinned meshes with a skeleton will be
// explicitly attached later.
mesh->setSkeletonName(mName);
// Convert vertices and normals to bone space from bind position. It would be
// better to transform the bones into bind position, but there doesn't seem to
// be a reliable way to do that.
std::vector<Ogre::Vector3> newVerts(srcVerts.size(), Ogre::Vector3(0.0f));
std::vector<Ogre::Vector3> newNorms(srcNorms.size(), Ogre::Vector3(0.0f));
const Nif::NiSkinData *data = skin->data.getPtr();
const Nif::NodeList &bones = skin->bones;
for(size_t b = 0;b < bones.length();b++)
{
Ogre::Matrix4 mat;
mat.makeTransform(data->bones[b].trafo.trans, Ogre::Vector3(data->bones[b].trafo.scale),
Ogre::Quaternion(data->bones[b].trafo.rotation));
mat = bones[b]->getWorldTransform() * mat;
const std::vector<Nif::NiSkinData::VertWeight> &weights = data->bones[b].weights;
for(size_t i = 0;i < weights.size();i++)
{
size_t index = weights[i].vertex;
float weight = weights[i].weight;
newVerts.at(index) += (mat*srcVerts[index]) * weight;
if(newNorms.size() > index)
{
Ogre::Vector4 vec4(srcNorms[index][0], srcNorms[index][1], srcNorms[index][2], 0.0f);
vec4 = mat*vec4 * weight;
newNorms[index] += Ogre::Vector3(&vec4[0]);
}
}
}
srcVerts = newVerts;
srcNorms = newNorms;
}
else
{
Ogre::SkeletonManager *skelMgr = Ogre::SkeletonManager::getSingletonPtr();
if(skelMgr->getByName(mName).isNull())
{
// No skinning and no skeleton, so just transform the vertices and
// normals into position.
Ogre::Matrix4 mat4 = shape->getWorldTransform();
for(size_t i = 0;i < srcVerts.size();i++)
{
Ogre::Vector4 vec4(srcVerts[i].x, srcVerts[i].y, srcVerts[i].z, 1.0f);
vec4 = mat4*vec4;
srcVerts[i] = Ogre::Vector3(&vec4[0]);
}
for(size_t i = 0;i < srcNorms.size();i++)
{
Ogre::Vector4 vec4(srcNorms[i].x, srcNorms[i].y, srcNorms[i].z, 0.0f);
vec4 = mat4*vec4;
srcNorms[i] = Ogre::Vector3(&vec4[0]);
}
}
}
// Set the bounding box first
BoundsFinder bounds;
bounds.add(&srcVerts[0][0], srcVerts.size());
if(!bounds.isValid())
{
float v[3] = { 0.0f, 0.0f, 0.0f };
bounds.add(&v[0], 1);
}
mesh->_setBounds(Ogre::AxisAlignedBox(bounds.minX()-0.5f, bounds.minY()-0.5f, bounds.minZ()-0.5f,
bounds.maxX()+0.5f, bounds.maxY()+0.5f, bounds.maxZ()+0.5f));
mesh->_setBoundingSphereRadius(bounds.getRadius());
// This function is just one long stream of Ogre-barf, but it works
// great.
Ogre::HardwareBufferManager *hwBufMgr = Ogre::HardwareBufferManager::getSingletonPtr();
Ogre::HardwareVertexBufferSharedPtr vbuf;
Ogre::HardwareIndexBufferSharedPtr ibuf;
Ogre::VertexBufferBinding *bind;
Ogre::VertexDeclaration *decl;
int nextBuf = 0;
Ogre::SubMesh *sub = mesh->createSubMesh();
// Add vertices
sub->useSharedVertices = false;
sub->vertexData = new Ogre::VertexData();
sub->vertexData->vertexStart = 0;
sub->vertexData->vertexCount = srcVerts.size();
decl = sub->vertexData->vertexDeclaration;
bind = sub->vertexData->vertexBufferBinding;
if(srcVerts.size())
{
vbuf = hwBufMgr->createVertexBuffer(Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3),
srcVerts.size(), vertUsage, vertShadowBuffer);
vbuf->writeData(0, vbuf->getSizeInBytes(), &srcVerts[0][0], true);
decl->addElement(nextBuf, 0, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
bind->setBinding(nextBuf++, vbuf);
}
// Vertex normals
if(srcNorms.size())
{
vbuf = hwBufMgr->createVertexBuffer(Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3),
srcNorms.size(), vertUsage, vertShadowBuffer);
vbuf->writeData(0, vbuf->getSizeInBytes(), &srcNorms[0][0], true);
decl->addElement(nextBuf, 0, Ogre::VET_FLOAT3, Ogre::VES_NORMAL);
bind->setBinding(nextBuf++, vbuf);
}
// Vertex colors
const std::vector<Ogre::Vector4> &colors = data->colors;
if(colors.size())
{
Ogre::RenderSystem *rs = Ogre::Root::getSingleton().getRenderSystem();
std::vector<Ogre::RGBA> colorsRGB(colors.size());
for(size_t i = 0;i < colorsRGB.size();i++)
{
Ogre::ColourValue clr(colors[i][0], colors[i][1], colors[i][2], colors[i][3]);
rs->convertColourValue(clr, &colorsRGB[i]);
}
vbuf = hwBufMgr->createVertexBuffer(Ogre::VertexElement::getTypeSize(Ogre::VET_COLOUR),
colorsRGB.size(), Ogre::HardwareBuffer::HBU_STATIC);
vbuf->writeData(0, vbuf->getSizeInBytes(), &colorsRGB[0], true);
decl->addElement(nextBuf, 0, Ogre::VET_COLOUR, Ogre::VES_DIFFUSE);
bind->setBinding(nextBuf++, vbuf);
}
// Texture UV coordinates
size_t numUVs = data->uvlist.size();
if (numUVs)
{
size_t elemSize = Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT2);
for(size_t i = 0; i < numUVs; i++)
decl->addElement(nextBuf, elemSize*i, Ogre::VET_FLOAT2, Ogre::VES_TEXTURE_COORDINATES, i);
vbuf = hwBufMgr->createVertexBuffer(decl->getVertexSize(nextBuf), srcVerts.size(),
Ogre::HardwareBuffer::HBU_STATIC);
std::vector<Ogre::Vector2> allUVs;
allUVs.reserve(srcVerts.size()*numUVs);
for (size_t vert = 0; vert<srcVerts.size(); ++vert)
for(size_t i = 0; i < numUVs; i++)
allUVs.push_back(data->uvlist[i][vert]);
vbuf->writeData(0, elemSize*srcVerts.size()*numUVs, &allUVs[0], true);
bind->setBinding(nextBuf++, vbuf);
}
// Triangle faces
const std::vector<short> &srcIdx = data->triangles;
if(srcIdx.size())
{
ibuf = hwBufMgr->createIndexBuffer(Ogre::HardwareIndexBuffer::IT_16BIT, srcIdx.size(),
Ogre::HardwareBuffer::HBU_STATIC);
ibuf->writeData(0, ibuf->getSizeInBytes(), &srcIdx[0], true);
sub->indexData->indexBuffer = ibuf;
sub->indexData->indexCount = srcIdx.size();
sub->indexData->indexStart = 0;
}
// Assign bone weights for this TriShape
if(skin != NULL)
{
Ogre::SkeletonPtr skel = Ogre::SkeletonManager::getSingleton().getByName(mName);
const Nif::NiSkinData *data = skin->data.getPtr();
const Nif::NodeList &bones = skin->bones;
for(size_t i = 0;i < bones.length();i++)
{
Ogre::VertexBoneAssignment boneInf;
boneInf.boneIndex = skel->getBone(bones[i]->name)->getHandle();
const std::vector<Nif::NiSkinData::VertWeight> &weights = data->bones[i].weights;
for(size_t j = 0;j < weights.size();j++)
{
boneInf.vertexIndex = weights[j].vertex;
boneInf.weight = weights[j].weight;
sub->addBoneAssignment(boneInf);
}
}
}
const Nif::NiTexturingProperty *texprop = NULL;
const Nif::NiMaterialProperty *matprop = NULL;
const Nif::NiAlphaProperty *alphaprop = NULL;
const Nif::NiVertexColorProperty *vertprop = NULL;
const Nif::NiZBufferProperty *zprop = NULL;
const Nif::NiSpecularProperty *specprop = NULL;
const Nif::NiWireframeProperty *wireprop = NULL;
bool needTangents = false;
shape->getProperties(texprop, matprop, alphaprop, vertprop, zprop, specprop, wireprop);
std::string matname = NIFMaterialLoader::getMaterial(data, mesh->getName(), mGroup,
texprop, matprop, alphaprop,
vertprop, zprop, specprop,
wireprop, needTangents);
if(matname.length() > 0)
sub->setMaterialName(matname);
// build tangents if the material needs them
if (needTangents)
{
unsigned short src,dest;
if (!mesh->suggestTangentVectorBuildParams(Ogre::VES_TANGENT, src,dest))
mesh->buildTangentVectors(Ogre::VES_TANGENT, src,dest);
}
// Create a dummy vertex animation track if there's a geom morpher controller
// This is required to make Ogre create the buffers we will use for software vertex animation
if (srcVerts.size() && geomMorpherController)
mesh->createAnimation("dummy", 0)->createVertexTrack(1, sub->vertexData, Ogre::VAT_MORPH);
}
void MapCloudDisplay::update( float wall_dt, float ros_dt )
{
rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
if (needs_retransform_)
{
retransform();
needs_retransform_ = false;
}
{
boost::mutex::scoped_lock lock(new_clouds_mutex_);
if( !new_cloud_infos_.empty() )
{
float size;
if( mode == rviz::PointCloud::RM_POINTS ) {
size = point_pixel_size_property_->getFloat();
} else {
size = point_world_size_property_->getFloat();
}
std::map<int, CloudInfoPtr>::iterator it = new_cloud_infos_.begin();
std::map<int, CloudInfoPtr>::iterator end = new_cloud_infos_.end();
for (; it != end; ++it)
{
CloudInfoPtr cloud_info = it->second;
cloud_info->cloud_.reset( new rviz::PointCloud() );
cloud_info->cloud_->addPoints( &(cloud_info->transformed_points_.front()), cloud_info->transformed_points_.size() );
cloud_info->cloud_->setRenderMode( mode );
cloud_info->cloud_->setAlpha( alpha_property_->getFloat() );
cloud_info->cloud_->setDimensions( size, size, size );
cloud_info->cloud_->setAutoSize(false);
cloud_info->manager_ = context_->getSceneManager();
cloud_info->scene_node_ = scene_node_->createChildSceneNode();
cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
cloud_info->scene_node_->setVisible(false);
cloud_infos_.insert(*it);
}
new_cloud_infos_.clear();
}
}
{
boost::recursive_mutex::scoped_try_lock lock( transformers_mutex_ );
if( lock.owns_lock() )
{
if( new_xyz_transformer_ || new_color_transformer_ )
{
M_TransformerInfo::iterator it = transformers_.begin();
M_TransformerInfo::iterator end = transformers_.end();
for (; it != end; ++it)
{
const std::string& name = it->first;
TransformerInfo& info = it->second;
setPropertiesHidden( info.xyz_props, name != xyz_transformer_property_->getStdString() );
setPropertiesHidden( info.color_props, name != color_transformer_property_->getStdString() );
}
}
}
new_xyz_transformer_ = false;
new_color_transformer_ = false;
}
int totalPoints = 0;
int totalNodesShown = 0;
{
// update poses
boost::mutex::scoped_lock lock(current_map_mutex_);
if(!current_map_.empty())
{
for (std::map<int, rtabmap::Transform>::iterator it=current_map_.begin(); it != current_map_.end(); ++it)
{
std::map<int, CloudInfoPtr>::iterator cloudInfoIt = cloud_infos_.find(it->first);
if(cloudInfoIt != cloud_infos_.end())
{
totalPoints += cloudInfoIt->second->transformed_points_.size();
cloudInfoIt->second->pose_ = it->second;
Ogre::Vector3 framePosition;
Ogre::Quaternion frameOrientation;
if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header, framePosition, frameOrientation))
{
// Multiply frame with pose
Ogre::Matrix4 frameTransform;
frameTransform.makeTransform( framePosition, Ogre::Vector3(1,1,1), frameOrientation);
const rtabmap::Transform & p = cloudInfoIt->second->pose_;
Ogre::Matrix4 pose(p[0], p[1], p[2], p[3],
p[4], p[5], p[6], p[7],
p[8], p[9], p[10], p[11],
0, 0, 0, 1);
frameTransform = frameTransform * pose;
Ogre::Vector3 posePosition = frameTransform.getTrans();
Ogre::Quaternion poseOrientation = frameTransform.extractQuaternion();
poseOrientation.normalise();
cloudInfoIt->second->scene_node_->setPosition(posePosition);
cloudInfoIt->second->scene_node_->setOrientation(poseOrientation);
cloudInfoIt->second->scene_node_->setVisible(true);
++totalNodesShown;
}
else
{
ROS_ERROR("MapCloudDisplay: Could not update pose of node %d", it->first);
}
}
}
//hide not used clouds
for(std::map<int, CloudInfoPtr>::iterator iter = cloud_infos_.begin(); iter!=cloud_infos_.end(); ++iter)
{
if(current_map_.find(iter->first) == current_map_.end())
{
iter->second->scene_node_->setVisible(false);
}
}
}
}
this->setStatusStd(rviz::StatusProperty::Ok, "Points", tr("%1").arg(totalPoints).toStdString());
this->setStatusStd(rviz::StatusProperty::Ok, "Nodes", tr("%1 shown of %2").arg(totalNodesShown).arg(cloud_infos_.size()).toStdString());
}
