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