void processDirectionElement(VertexData* vertexData, const VertexElement* vertexElem )
{
Ogre::Quaternion rotation = mTransform.extractQuaternion();
rotation.normalise();
int nMaxVert= vertexData->vertexCount ;
//const Ogre::VertexElement* VertexEle_POS = vertexData->vertexDeclaration->findElementBySemantic( Ogre::VES_POSITION );
// get vertex buffer info via the input element
Ogre::HardwareVertexBufferSharedPtr VertexBufPOS = vertexData->vertexBufferBinding->getBuffer( vertexElem->getSource() );
//LOCK BUFFER
unsigned char* VertexPtrPOS = static_cast<unsigned char*>( VertexBufPOS->lock( Ogre::HardwareBuffer::HBL_NORMAL ) );
int VertSizePOS=VertexBufPOS->getVertexSize();
float * pElementPOS=NULL;
//A vector of every vertices position
std::vector<Ogre::Vector3> positions(nMaxVert);
//Copy each position into position vector
for(int nVert=0 ; nVert<nMaxVert ; nVert++)
{
vertexElem->baseVertexPointerToElement( VertexPtrPOS, &pElementPOS );
Ogre::Vector3 vertex(pElementPOS);
vertex = rotation * vertex;
if (mNormaliseNormals)
{
vertex.normalise();
}
pElementPOS[0] = vertex.x;
pElementPOS[1] = vertex.y;
pElementPOS[2] = vertex.z;
//mBoundingBox.merge(vertex);
VertexPtrPOS+=VertSizePOS ;
}
//UNLOCK BUFFER
if(VertexBufPOS->isLocked()){VertexBufPOS->unlock();}
}
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());
}
void BoneAim::update(float time){
if (mBone == NULL){
mBone = mGo->getEntity()->getSkeleton()->getBone(mBoneName);
mTarget = Level::getSingleton()->getCurrentSegment()->getObjectByName(mTargetName);
if (mTarget == NULL){
throw("No such object : "+mTargetName);
}
}
//Ogre::Entity* entity;
//mTarget = Level::getSingleton()->getPlayerShip();
Ogre::Matrix4 matrixA = mGo->getNode()->_getFullTransform()*mBone->_getFullTransform();
Ogre::Matrix4 matrix = matrixA;
Ogre::Matrix4 transform = matrix.inverse()*mTarget->getNode()->_getFullTransform();
Ogre::Vector3 v = transform.getTrans();
/*Util::Log(
"Bone:"+ts(matrixA.getTrans())+
" Ship:"+ts(mTarget->getNode()->_getFullTransform().getTrans())+
" Diff:"+ts(v)
,0
);*/
//targetMatrix = Ogre::Matrix4::IDENTITY;
//matrix.setTrans(mTarget->getNode()->_getFullTransform());
//Ogre::Quaternion q = transform.extractQuaternion();
Ogre::Quaternion q;
switch (mAxis){
case AXIS_X:
q.FromAngleAxis(Ogre::Math::ATan2(v.y, v.z),Ogre::Vector3(-1,0,0));
break;
case AXIS_Y:
q.FromAngleAxis(Ogre::Math::ATan2(v.x, v.z),Ogre::Vector3(0,1,0));
break;
case AXIS_Z:
q.FromAngleAxis(Ogre::Math::ATan2(v.x, v.y),Ogre::Vector3(0,0,1));
break;
case AXIS_ALL:
//q.FromAngleAxis(Ogre::Radian(0),v);
q = transform.extractQuaternion();// - matrixA.extractQuaternion();
//q = transform.extractQuaternion() - matrixA.extractQuaternion();
break;
}
mBone->setManuallyControlled(true);
//mBone->setOrientation(q);
float slerp = time*mStrength;
if (slerp > 1){
slerp = 1;
}
Ogre::Quaternion r = Quaternion::Slerp(slerp, Ogre::Quaternion::IDENTITY, q); //Level::getSingleton()->getTimeDelta()
mBone->rotate(r);
/*mBone->yaw(Ogre::Radian(Ogre::Math::RangeRandom(-Math::PI, Math::PI)));
mBone->pitch(Ogre::Radian(Ogre::Math::RangeRandom(-Math::PI, Math::PI)));
mBone->roll(Ogre::Radian(Ogre::Math::RangeRandom(-Math::PI, Math::PI)));*/
//mBone->_update(true, true);
Parent::update(time);
//mSpeed += .6*time;
//mGo->getNode()->translate(0, 0-mSpeed, 0);*/
}
