C++ Vector3f::normalize方法代码示例

bool targetViewpoint(const Eigen::Vector3f& rayo,const Eigen::Vector3f& target,const Eigen::Vector3f& down,
                     Eigen::Affine3f& transf)
{
  // uz: versor pointing toward the destination
  Eigen::Vector3f uz = target - rayo;
  if (std::abs(uz.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": target point on ray origin!" << std::endl;
    return false;
  }
  uz.normalize();
  //std::cout << "uz " << uz.transpose() << ", norm " << uz .norm() << std::endl;
  // ux: versor pointing toward the ground
  Eigen::Vector3f ux = down - down.dot(uz) * uz;  
  if (std::abs(ux.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": ray to target toward ground direction!" << std::endl;
    return false;
  }
  ux.normalize();
  //std::cout << "ux " << ux.transpose() << ", norm " << ux.norm() << std::endl;
  Eigen::Vector3f uy = uz.cross(ux);
  //std::cout << "uy " << uy.transpose() << ", norm " << uy.norm() << std::endl;
  Eigen::Matrix3f rot;
  rot << ux.x(), uy.x(), uz.x(),
         ux.y(), uy.y(), uz.y(),
         ux.z(), uy.z(), uz.z();
  transf.setIdentity();
  transf.translate(rayo);
  transf.rotate(rot);
  //std::cout << __FILE__ << "\nrotation\n" << rot << "\ntranslation\n" << rayo << "\naffine\n" << transf.matrix() << std::endl;
  return true;
}

void fuzzyAffines()
{
    std::vector<Eigen::Matrix4f> trans;
    trans.reserve(count/10);
    for( size_t i=0; i<count/10; i++ )
    {
        Eigen::Vector3f x = Eigen::Vector3f::Random();
        Eigen::Vector3f y = Eigen::Vector3f::Random();

        x.normalize();
        y.normalize();

        Eigen::Vector3f z = x.cross(y);
        z.normalize();

        y = z.cross(x);
        y.normalize();

        Eigen::Affine3f t = Eigen::Affine3f::Identity();
        Eigen::Matrix3f r = Eigen::Matrix3f::Identity();

        r.col(0) = x;
        r.col(1) = y;
        r.col(2) = z;

        t.rotate(r);
        t.translate( 0.5f * Eigen::Vector3f::Random() + Eigen::Vector3f(0.5,0.5,0.5) );

        trans.push_back( t.matrix() );
    }

    s_plot.setColor( Eigen::Vector4f(1,0,0,1) );
    s_plot.setLineWidth( 3.0 );
    s_plot( trans, nox::plot<float>::Pos | nox::plot<float>::CS );
}

    /**
     * @brief Computes the trackball's rotation, using stored initial and final position vectors.
     */
    void computeRotationAngle (void)
    {
        //Given two position vectors, corresponding to the initial and final mouse coordinates, calculate the rotation of the sphere that will keep the mouse always in the initial position.

        if(initialPosition.norm() > 0) {
            initialPosition.normalize();
        }
        if(finalPosition.norm() > 0) {
            finalPosition.normalize();
        }

        //cout << "Initial Position: " << initialPosition.transpose() << " Final Position: " << finalPosition.transpose() << endl << endl;

        Eigen::Vector3f rotationAxis = initialPosition.cross(finalPosition);

        if(rotationAxis.norm() != 0) {
            rotationAxis.normalize();
        }

        float dot = initialPosition.dot(finalPosition);

        float rotationAngle = (dot <= 1) ? acos(dot) : 0;//If, by losing floating point precision, the dot product between the initial and final positions is bigger than one, ignore the rotation.

        Eigen::Quaternion<float> q (Eigen::AngleAxis<float>(rotationAngle,rotationAxis));

        quaternion = q * quaternion;
        quaternion.normalize();
    }

/* http://geomalgorithms.com/a07-_distance.html */
void intersectLines(const Eigen::Vector3f &p0, const Eigen::Vector3f &p1,
                    const Eigen::Vector3f &q0, const Eigen::Vector3f &q1,
                    Eigen::Vector3f &pointOnP, Eigen::Vector3f &pointOnQ)
{
    Eigen::Vector3f w0 = p0 - q0;

    /* the two vectors on the lines */
    Eigen::Vector3f u = p1 - p0;
    u.normalize();
    Eigen::Vector3f v = q0 - q1;
    v.normalize();

    float a = u.dot(u);
    float b = u.dot(v);
    float c = v.dot(v);
    float d = u.dot(w0);
    float e = v.dot(w0);

    float normFactor = a*c - b*b;
    float sc = (b*e - c*d) / normFactor;
    float tc = (a*e - b*d) / normFactor;

    /* the two nearest points on the lines */
    Eigen::ParametrizedLine<float, 3> lineP(p0, u);
    pointOnP = lineP.pointAt(sc);
    Eigen::ParametrizedLine<float, 3> lineQ(q0, v);
    pointOnQ = lineQ.pointAt(tc);
}

/**
 * Verifica si un plano corta a una nube en dos
 * @param  pc    nube de puntos a evaluar
 * @param  coefs Coeficientes del plano
 * @return       True si lo corta, false en caso contrario.
 */
bool isPointCloudCutByPlane(PointCloud<PointXYZ>::Ptr pc, ModelCoefficients::Ptr coefs, PointXYZ p_plane){
	/*
	Algoritmo:
		- Obtener vector normal a partir de coeficientes
		- Iterar sobre puntos de la nube
			- Calcular vector delta entre punto entregado (dentro del plano) y punto iterado
			- Calcular ángulo entre vector delta y normal
			- Angulos menores a PI/2 son de un lado, mayores son de otro
			- Si aparecen puntos de ambos lados, retornar True, else, false.
	*/
	const double PI = 3.1416;
	Eigen::Vector3f normal = Eigen::Vector3f(coefs->values[0], coefs->values[1], coefs->values[2]);
	normal.normalize();
	// cout << "Normal: " << normal << endl;
	bool side;
	// Iterar sobre los puntos
	for (int i=0; i<pc->points.size(); i++){
		// Calcular ángulo entre punto y normal
		Eigen::Vector3f delta = Eigen::Vector3f (p_plane.x, p_plane.y, p_plane.z) - Eigen::Vector3f(pc->points[i].x, pc->points[i].y, pc->points[i].z);
		delta.normalize();
		double alpha = acos(normal.dot(delta));
		// printf ("Alpha: %f°\n", (alpha*180/PI));
		if (i==0){
			side = (alpha < PI/2.0);
			// printf("Lado escogido: %s", side ? "true": "false");
			continue;
		}
		if (side != (alpha < PI/2.0)){
			// printf("Nube es cortada por plano\n");
			return true;
		}
	}
	// printf("Nube NO es cortada por plano\n");
	return false;
}

		int process(const tendrils& inputs, const tendrils& outputs,
					boost::shared_ptr<const ::pcl::PointCloud<Point> >& input)
		{
      eigenVectors_->clear();
      centroids_->clear();
      ::pcl::ExtractIndices<Point> filter;
      filter.setInputCloud(input);

      rectangles_->resize(static_cast<std::size_t>(clusters_->size()));
      for(std::size_t i = 0; i < clusters_->size(); ++i)
      {
        rectangles_->at(i).resize(4);
      }

      for(std::size_t i = 0; i < clusters_->size(); ++i)
      {
        if(clusters_->at(i).indices.size() < 3)
          continue;

        boost::shared_ptr< ::pcl::PointCloud<Point> > cloud;
        cloud = boost::make_shared< ::pcl::PointCloud<Point> > ();
        // extract indices into a cloud
        filter.setIndices( ::pcl::PointIndicesPtr(
        new ::pcl::PointIndices ((*clusters_)[i])) );
        filter.filter(*cloud);

        // extract the eigen vectors
        ::pcl::PointCloud< ::pcl::PointXYZ> proj;
        pcl::PCA <Point > pca;
        pca.setInputCloud(cloud);
        eigenVectors_->push_back(pca.getEigenVectors());
        centroids_->push_back(pca.getMean());

        //generate the rectangles
        Eigen::Vector3f center = Eigen::Vector3f(pca.getMean().x(),
                                                 pca.getMean().y(),
                                                 pca.getMean().z());
        Eigen::Vector3f longAxis = Eigen::Vector3f(pca.getEigenVectors()(0,0),
                                                 pca.getEigenVectors()(1,0),
                                                 pca.getEigenVectors()(2,0));
        Eigen::Vector3f shortAxis = Eigen::Vector3f(pca.getEigenVectors()(0,1),
                                                 pca.getEigenVectors()(1,1),
                                                 pca.getEigenVectors()(2,1));
        longAxis.normalize();
        longAxis = (*length_rectangles_)*longAxis;        
        shortAxis.normalize();
        shortAxis = (*width_rectangles_)*shortAxis;        
        
        rectangles_->at(i)[0] = center - longAxis/2 - shortAxis/2;
        rectangles_->at(i)[1] = center + longAxis/2 - shortAxis/2;
        rectangles_->at(i)[2] = center + longAxis/2 + shortAxis/2;
        rectangles_->at(i)[3] = center - longAxis/2 + shortAxis/2;

      }
      
			return ecto::OK;
		}

	/**
	 * @brief Compose rotation and translation
	 */
	void updateViewMatrix() override
	{
		Eigen::Vector3f xAxis = rotation_matrix.row(0);
		Eigen::Vector3f yAxis = rotation_matrix.row(1);
		Eigen::Vector3f zAxis = rotation_matrix.row(2);
		
		if( rotation_Z_axis )
		{
			Eigen::AngleAxisf transfRotZ = Eigen::AngleAxisf(rotation_Z_axis, zAxis);
			
			// compute X axis restricted to a rotation around Z axis
			xAxis = transfRotZ * xAxis;
			xAxis.normalize();
			
			// compute Y axis restricted to a rotation around Z axis
			yAxis = transfRotZ * yAxis;
			yAxis.normalize();
			
			rotation_Z_axis = 0.0;
		}
		
		Eigen::AngleAxisf transfRotY = Eigen::AngleAxisf(rotation_Y_axis, yAxis);
		
		// compute X axis restricted to a rotation around Y axis
		Eigen::Vector3f rotX = transfRotY * xAxis;
		rotX.normalize();

		Eigen::AngleAxisf transfRotX = Eigen::AngleAxisf(rotation_X_axis, rotX);
		
		// rotate Z axis around Y axis, then rotate new Z axis around X new axis
		Eigen::Vector3f rotZ = transfRotY * zAxis;
		rotZ = transfRotX * rotZ;
		rotZ.normalize();

		// rotate Y axis around X new axis
		Eigen::Vector3f rotY = transfRotX * yAxis;
		rotY.normalize();

		rotation_matrix.row(0) = rotX;
		rotation_matrix.row(1) = rotY;
		rotation_matrix.row(2) = rotZ;
		
		resetViewMatrix();
		view_matrix.rotate (rotation_matrix);
		view_matrix.translate (translation_vector);
		
		rotation_X_axis = 0.0;
		rotation_Y_axis = 0.0;
	}

png::image<png::rgb_pixel_16> bumpier::model::calculate_normals(const png::image<png::gray_pixel_16>& input, double phi, space_type space) const
{
    int width = input.get_width(), height = input.get_height();
    png::image<png::rgb_pixel_16> output(width, height);

    for (int y = 0; y < height; y++)
        for (int x = 0; x < width; x++)
        {
            int xmin = (x + width  - 1) % width,
                ymin = (y + height - 1) % height,
                xmax = (x + 1) % width,
                ymax = (y + 1) % height;

            Eigen::Vector3f normal = (position(input, x, ymax) - position(input, x, ymin)).cross(
                                      position(input, xmax, y) - position(input, xmin, y));

            if(space == tangent_space)
				normal = tangentspace(Eigen::Vector2f((float)x / width, (float)y / height)) * normal;

			normal.normalize();

            normal = (normal.array() * 0.5 + 0.5) * 0xFFFF;
            output.set_pixel(x, y, png::rgb_pixel_16(normal[0], normal[1], normal[2]));
        }

    return output;
}

template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::randomOrthogonalAxis (
                                                                                             Eigen::Vector3f const &axis,
                                                                                             Eigen::Vector3f &rand_ortho_axis)
{
  if (!areEquals (axis.z (), 0.0f))
  {
    rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z ();
  }
  else if (!areEquals (axis.y (), 0.0f))
  {
    rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y ();
  }
  else if (!areEquals (axis.x (), 0.0f))
  {
    rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x ();
  }

  rand_ortho_axis.normalize ();

  // check if the computed x axis is orthogonal to the normal
  //assert(areEquals(rand_ortho_axis.dot(axis), 0.0f, 1E-6f));
}

pcl::PointCloud<pcl::PointNormal>::Ptr
meshToFaceCloud (const pcl::PolygonMesh &mesh)
{
  pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
  pcl::PointCloud<pcl::PointXYZ> vertices;
  pcl::fromPCLPointCloud2 (mesh.cloud, vertices);

  for (size_t i = 0; i < mesh.polygons.size (); ++i)
  {
    if (mesh.polygons[i].vertices.size () != 3)
    {
      PCL_ERROR ("Found a polygon of size %d\n", mesh.polygons[i].vertices.size ());
      continue;
    }
    Eigen::Vector3f v0 = vertices.at (mesh.polygons[i].vertices[0]).getVector3fMap ();
    Eigen::Vector3f v1 = vertices.at (mesh.polygons[i].vertices[1]).getVector3fMap ();
    Eigen::Vector3f v2 = vertices.at (mesh.polygons[i].vertices[2]).getVector3fMap ();
    float area = ((v1 - v0).cross (v2 - v0)).norm () / 2. * 1E4;
    Eigen::Vector3f normal = ((v1 - v0).cross (v2 - v0));
    normal.normalize ();
    pcl::PointNormal p_new;
    p_new.getVector3fMap () = (v0 + v1 + v2)/3.;
    p_new.normal_x = normal (0);
    p_new.normal_y = normal (1);
    p_new.normal_z = normal (2);
    cloud->points.push_back (p_new);
  }
  cloud->height = 1;
  cloud->width = cloud->size ();
  return (cloud);
}

void Kamikaze::updateEnemy(Level * currLev) {
	//todo move at ship;

	if (!dead) {
		Eigen::Vector3f shipLoc = currLev->ship->position;
		Eigen::Vector3f direction = shipLoc - position;
		
		direction.normalize();
		direction /= 1000;
		direction.z() -= currLev->ship->velocity.z();



		Eigen::Vector3f mousePos = Eigen::Vector3f(shipLoc.x(), shipLoc.y(), shipLoc.z());
		float rotateY = RADIANS_TO_DEGREES(atan((shipLoc.x() - position.x()) / (shipLoc.z() - position.z()))) - 90.0;
		float rotateX = -RADIANS_TO_DEGREES(atan((shipLoc.y() - position.y()) / (shipLoc.z() - position.z())));

		rotate.y() = rotateY;
		rotate.x() = rotateX;

		// if we're behind ship, keep going in that direction
		if (direction.z() <= 0) {
			direction[2] *= -1;
			direction[2] += 0.2;
		}
		velocity = direction;
	}
}

void StarCamera::calculateSpotVectors()
{
    if(mSpots.empty())
        throw std::runtime_error("No extracted spots in List");

    bool zeroNorm = !(mDistortionCoeffi.norm() != 0.0f);

    // Clear SpotVectors
    mSpotVectors.clear();

    std::vector<Spot>::iterator pSpot;
    for(pSpot=mSpots.begin(); pSpot != mSpots.end(); ++pSpot)
    {
        // Substract principal point and divide by the focal length

        Eigen::Vector2f Xd(pSpot->center.x, pSpot->center.y);
        Xd = (Xd - mPrincipalPoint).array() / mFocalLength.array();

        // Undo skew
        Xd(0) = Xd(0) - mPixelSkew * Xd(1);

        Eigen::Vector3f spotVec;
        if(!zeroNorm)  // Use epsilon environment?
        {
            Xd = undistortRadialTangential(Xd);
        }
        spotVec << Xd(0), Xd(1), 1.0f;
        spotVec.normalize();
        mSpotVectors.push_back(spotVec);
    }
}

template <typename PointNT> bool
pcl::GridProjection<PointNT>::isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
                                             std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
                                             std::vector <int> &pt_union_indices)
{
  assert (end_pts.size () == 2);
  assert (vect_at_end_pts.size () == 2);

  double length[2];
  for (size_t i = 0; i < 2; ++i)
  {
    length[i] = vect_at_end_pts[i].norm ();
    vect_at_end_pts[i].normalize ();
  }
  double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
  if (dot_prod < 0)
  {
    double ratio = length[0] / (length[0] + length[1]);
    Eigen::Vector4f start_pt = 
      end_pts[0] + (end_pts[1] - end_pts[0]) * static_cast<float> (ratio);
    Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
    findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);

    Eigen::Vector3f vec;
    getVectorAtPoint (intersection_pt, pt_union_indices, vec);
    vec.normalize ();

    double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
    if (d2 < 0)
      return (true);
  }
  return (false);
}

template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getApproxIntersectedVoxelCentersBySegment (
    const Eigen::Vector3f& origin,
    const Eigen::Vector3f& end,
    AlignedPointTVector &voxel_center_list,
    float precision)
{
  Eigen::Vector3f direction = end - origin;
  float norm = direction.norm ();
  direction.normalize ();

  const float step_size = static_cast<const float> (resolution_) * precision;
  // Ensure we get at least one step for the first voxel.
  const int nsteps = std::max (1, static_cast<int> (norm / step_size));

  OctreeKey prev_key;

  bool bkeyDefined = false;

  // Walk along the line segment with small steps.
  for (int i = 0; i < nsteps; ++i)
  {
    Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));

    PointT octree_p;
    octree_p.x = p.x ();
    octree_p.y = p.y ();
    octree_p.z = p.z ();

    OctreeKey key;
    this->genOctreeKeyforPoint (octree_p, key);

    // Not a new key, still the same voxel.
    if ((key == prev_key) && (bkeyDefined) )
      continue;

    prev_key = key;
    bkeyDefined = true;

    PointT center;
    genLeafNodeCenterFromOctreeKey (key, center);
    voxel_center_list.push_back (center);
  }

  OctreeKey end_key;
  PointT end_p;
  end_p.x = end.x ();
  end_p.y = end.y ();
  end_p.z = end.z ();
  this->genOctreeKeyforPoint (end_p, end_key);
  if (!(end_key == prev_key))
  {
    PointT center;
    genLeafNodeCenterFromOctreeKey (end_key, center);
    voxel_center_list.push_back (center);
  }

  return (static_cast<int> (voxel_center_list.size ()));
}

Eigen::Vector3f RayTracer::findReflect(Eigen::Vector3f ray, Eigen::Vector3f normal, SceneObject* obj) {
   Eigen::Vector3f reflectRay;
   
   reflectRay = ray + (2.0*normal*(normal.dot(-ray)));
   reflectRay.normalize();
   
   return reflectRay;
}