C++ Quaternion::normalise方法代码示例

void ConvexShapeObstacle::updateOrientation(Ogre::Quaternion orientation)
{
    // Modify orientation if difference larger than epsilon
    if(! mOrientation.equals(orientation, Ogre::Degree(ORIENTATION_TOLERANCE_DEGREES))) {
        // Remove old obstacle from tilecache
        mDetourTileCache->removeConvexShapeObstacle(mConvexHull);

        // In case we didn't generate inputgeom yet (we previously generated hull directly from the bounding box), do it now
        if(!mInputGeom)
            mInputGeom = new InputGeom(mEnt);

        // Apply rotation to the inputGeometry and calculate a new 2D convex hull
        Ogre::Quaternion relativeOrientation = orientation * mOrientation.Inverse();    // Calculate relative rotation from current rotation to the specified one
        orientation.normalise();    // Make sure quaternion is normalized
        mInputGeom->applyOrientation(relativeOrientation, mPosition);   // Rotate around obstacle position (center or origin point)
        if (mConvexHull)
            delete mConvexHull;
        mConvexHull = mInputGeom->getConvexHull(mOffset);
        mConvexHull->area = RC_NULL_AREA;   // Be sure to set the proper area for the convex shape!

        // Add new hull as obstacle to tilecache
        mDetourTileCache->addConvexShapeObstacle(mConvexHull);

        mOrientation = orientation;


        // Now also set the rotation to the visual obstacle entity
        mNode->setOrientation(orientation);

        // Create new debug drawing of the convex hull
        mConvexHullDebug->detachFromParent();
        mSceneMgr->destroyManualObject(mConvexHullDebug);
        mConvexHullDebug = InputGeom::drawConvexVolume(mConvexHull, mSceneMgr);
    }
}

//-----------------------------------------------------------------------------------------
void CCameraEditor::roll(const Ogre::Radian &value)
{
    Ogre::Quaternion q;
    Ogre::Vector3 axis = mOrientation->get() * Ogre::Vector3::UNIT_Z;
    q.FromAngleAxis(value, axis);
    q.normalise();
    
    mOrientation->set(q * mOrientation->get());
}

//-----------------------------------------------------------------------------------------
void CCameraEditor::yaw(const Ogre::Radian &value)
{
    Ogre::Quaternion q;

    q.FromAngleAxis(value, Ogre::Vector3::UNIT_Y);
    q.normalise();
    
    mOrientation->set(q * mOrientation->get());
}

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

 
}

//.........这里部分代码省略.........
				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 Billboard::addTrajectoryPredictionMarkers()
{
  visualization_msgs::InteractiveMarker predictionMarker;
  predictionMarker = object_;
  predictionMarker.controls.clear();

  Ogre::Matrix3 *rotation = new Ogre::Matrix3();
  Ogre::Quaternion orientation;
  orientation.x = direction_.x;
  orientation.y = direction_.y;
  orientation.z = direction_.z;
  orientation.w = direction_.w;
  orientation.normalise();
  orientation.ToRotationMatrix(*rotation);
  Ogre::Vector3 position;
  position.x = velocity_;
  position.y = 0;
  position.z = 0;
  position = rotation->operator *(position);

  predictionMarker.pose.position.x = pose_.position.x;
  predictionMarker.pose.position.y = pose_.position.y;
  predictionMarker.pose.position.z = pose_.position.z;

  visualization_msgs::InteractiveMarkerControl predictionControl;
  predictionControl.name = "prediction_control";
  predictionControl.always_visible = true;
  predictionControl.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
  predictionControl.interaction_mode = InteractiveMarkerControl::NONE;

  for (int i = 1; i < PREDICTIONS_COUNT + 1; i++)
  {
    std::stringstream name;
    name << name_ << "_prediction_" << i;
    predictionMarker.name = name.str();
    std::stringstream desc;
    desc << i << "s";
    predictionMarker.description = desc.str();
    predictionMarker.pose.position.x += position.x;
    predictionMarker.pose.position.y += position.y;
    predictionMarker.pose.position.z += position.z;

    predictionControl.markers.clear();
    predictionMarker.controls.clear();

    visualization_msgs::Marker sphere;
    sphere.type = visualization_msgs::Marker::SPHERE;
    sphere.color.g = 1;
    sphere.color.b = 1;
    sphere.color.a = 1;
    sphere.scale.x = PREDICTION_SPHERE_SIZE;
    sphere.scale.y = PREDICTION_SPHERE_SIZE;
    sphere.scale.z = PREDICTION_SPHERE_SIZE;

    predictionControl.markers.push_back(sphere);
    predictionMarker.controls.push_back(predictionControl);

    server_->insert(predictionMarker);
  }
  server_->applyChanges();
}

/*
 
 Generates the decal and returns a Decal object. The caller is responsible for NULL-checking the manual object.
 This gigantic function should probably be chopped into smaller bite-sized pieces, but I'm just doo darn lazy.
 
 @param mesh    The mesh to project the decal onto.
 @param pos     The position of the decal
 @param width   The width of the decal
 @param height  The height of the decal
                Note: The aspect ratio defined by width/height should match the texture, otherwise it will appear stretched.
 
 @param materialName    The name of the material to use for the decal
 @param flipTexture     Will randomly flip the texture to introduce variety (useful for blood splatter, explosion decals, etc.)
 @param decalObject     If NULL, this function will automatically create a new manual object (default). Otherwise, it will re-use the one passed in.
                        For dynamic decals (generating one every frame), it is much more efficient to reuse the same manual object,
                        as long as the material doesn't change.
 

 */
Decal DecalGenerator::createDecal( TriangleMesh* mesh, const Ogre::Vector3& pos, float width, float height, 
                                  const Ogre::String& materialName, bool flipTexture, Ogre::ManualObject* decalObject )
{
	//Variable de error para dar de salida en el object un NULL y que no pete
	bool bError = false;

    /// Clear out any old left-over stuff from the fridge.
    triangles.clear();
    uniquePoints.clear();
    finalPolys.clear();
    polygon_points.clear();
    
    float depth = max(width, height);

    /// Define our AABB 
    Ogre::Vector3 aabbMin = pos + Ogre::Vector3( -depth, -depth, -depth );
    Ogre::Vector3 aabbMax = pos + Ogre::Vector3( depth, depth, depth);
    
    /// We're gonna need triangles. Lot's of triangles.
    mesh->findTrianglesInAABB( aabbMin, aabbMax, triangles );
    
    if (triangles.empty())
    {
        /// No triangles were found, return an empty Decal
        /// Note that the caller is responsible for verifying the returned object
        return Decal();
    }
    
    std::vector< Triangle >::iterator iter;
    Ogre::Vector3 averageN(0, 0, 0);
    
    /// Calculate the average normal of all the triangles gathered from our AABB
    for (iter = triangles.begin(); iter != triangles.end(); ++iter)
    {
        averageN += iter->normal;
    }
    
    /// This average normal length is too close to zero, which is a bad omen. Get out while we still can!
    if (averageN.length() < VEC_EPSILON)
    {
        return Decal();
    }
    
    averageN.normalise();
    
    Ogre::Vector3 right, up;
    
    /// Calculate a coordinate space from the the average of all the triangle normals
    /// We're creating the projection box that will be used to clip the triangles
    
    if (averageN == Ogre::Vector3(0, 1, 0))
    {
        right = Ogre::Vector3(1, 0, 0);
    }
    else if (averageN == Ogre::Vector3(0, -1, 0))
    {
        right = Ogre::Vector3(-1, 0, 0);
    }
    else                
    {
        right = (-averageN).crossProduct( Ogre::Vector3(0, 1, 0) );
    }
    
    right.normalise();
    
    up = right.crossProduct( -averageN );
    up.normalise();
        
    
    /// Now that we have our coordinate space, let's define some planes. No silly, not the ones that fly in the sky!
    // These are the clipping planes! Be careful, because you might get cut.
    const int NUM_EDGE_PLANES = 6;
    
    Ogre::Vector4 edgePlanes[NUM_EDGE_PLANES];
    
    Ogre::Vector3 planeR[6];
    Ogre::Vector3 planeN[6];
    
    planeN[0] = averageN;  
    planeN[1] = -averageN; 
    planeN[2] = right;      
//.........这里部分代码省略.........