三維空間中點到平面的投影點坐標

代碼：

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

 

PointType RanSac::projectPointToPlane(PointType &pt, PlaneEquation &pe)
{
    PointType ptemp;
    double abc = pe.A*pe.A + pe.B*pe.B + pe.C*pe.C;

    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;
    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;
    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;

    return ptemp;
}