三维空间中点到平面的投影点坐标

代码：

 struct PlaneEquation {
  double A;
  double B;
  double C;
  double D;
 };

 1 PointType RanSac::projectPointToPlane(PointType &pt, PlaneEquation &pe)
 2 {
 3     PointType ptemp;
 4     double abc = pe.A*pe.A + pe.B*pe.B + pe.C*pe.C;
 5 
 6     ptemp.x = ((pe.B*pe.B + pe.C*pe.C)*pt.x - pe.A*(pe.B*pt.y + pe.C*pt.z + pe.D)) / abc;
 7     ptemp.y = ((pe.A*pe.A + pe.C*pe.C)*pt.y - pe.B*(pe.A*pt.x + pe.C*pt.z + pe.D)) / abc;
 8     ptemp.z = ((pe.A*pe.A + pe.B*pe.B)*pt.z - pe.C*(pe.A*pt.x + pe.B*pt.y + pe.D)) / abc;
 9 
10     return ptemp;
11 }