#include<pcl/point_cloud.h>
#include<pcl/point_types.h>
#include<pcl/visualization/pcl_visualizer.h> //cout endl

#include <pcl/common/geometry.h>
#include <Eigen/Core>
#include <math.h>

typedef pcl::PointXYZ PointT;

int main()
{
    //(1)兩點間的距離
    //float pcl::geometry::distance (const PointT &p1, const PointT &p2)
    float distance = pcl::geometry::distance(PointT(0, 0, 0), PointT(1, 2, 3));

    //(2)兩點間的距離的平方
    //float pcl::geometry::squaredDistance (const PointT &p1, const PointT &p2)
    float squaredDistance = pcl::geometry::squaredDistance(PointT(0, 0, 0), PointT(1, 2, 3));

    //(3)返回投影的點（原點-法向量-投影）
    //void  pcl::geometry::project (const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
    pcl::Normal plane_normal(0,0,1);
    PointT a;
    //pcl::geometry::project(PointT(3, 3, 3), PointT(0, 0, 0), pcl::Normal(0,0,1), a);//失敗羡鸥，

    //void pcl::geometry::project(const Eigen::Vector3f & point, const Eigen::Vector3f & plane_origin, const Eigen::Vector3f & plane_normal, Eigen::Vector3f & projected)
    //平面的法向量長度為1時岖食，投影點可以落在平面上{返回投影的點（原點-法向量-投影）}
    Eigen::Vector3f projected;
    //pcl::geometry::project(Eigen::Vector3f(3,3,3), Eigen::Vector3f(0,0,0), Eigen::Vector3f(0,0,1), projected);
    pcl::geometry::project(Eigen::Vector3f(3, 3, 3), Eigen::Vector3f(0, 0, 0), Eigen::Vector3f(sqrt(3)/3, sqrt(3)/3, sqrt(3)/3), projected);

    //(4)給定一個由 plane_origin 和 plane_normal 定義的平面秤标，找到從 plane_origin 指向平面上點的投影的單位向量
    //Eigen::Vector3f pcl::geometry::projectedAsUnitVector(Eigen::Vector3f const& point, Eigen::Vector3f const& plane_origin, Eigen::Vector3f const& plane_normal)
    //投影是(3,3,0)婉称，從原點(0监徘，0，0)出發(fā)到(3,3,0)的單位向量即為((根號2)/2,(根號2)/2,0)紫皇。
    Eigen::Vector3f unit_vector = pcl::geometry::projectedAsUnitVector(Eigen::Vector3f(3, 3, 3), Eigen::Vector3f(0, 0, 0), Eigen::Vector3f(0,0,1));

    //(5)定義一個與軸正交的隨機單位向量草丧。
    //Eigen::Vector3f pcl::geometry::randomOrthogonalAxis (Eigen::Vector3f const &axis)
    Eigen::Vector3f random_unit_vector = pcl::geometry::randomOrthogonalAxis(Eigen::Vector3f(0,0,1));
    Eigen::Vector3f random_unit_vector1 = pcl::geometry::randomOrthogonalAxis(Eigen::Vector3f(0, 0, 1));

    system("pause");
    return 0;
}

參考文章：
http://pointclouds.org/documentation/common_2include_2pcl_2common_2geometry_8h.html
http://pointclouds.org/documentation/common_2include_2pcl_2common_2geometry_8h_source.html