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);*/
		}