/*!
Build a 3D plane thanks to 3 points and stores it in \f$ plane \f$.
\param P : The first point to define the plane
\param Q : The second point to define the plane
\param R : The third point to define the plane
\param plane : The vpPlane instance used to store the computed plane equation.
*/
void
buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
{
vpColVector a(3);
vpColVector b(3);
vpColVector n(3);
//Calculate vector corresponding to PQ
a[0]=P.get_oX()-Q.get_oX();
a[1]=P.get_oY()-Q.get_oY();
a[2]=P.get_oZ()-Q.get_oZ();
//Calculate vector corresponding to PR
b[0]=P.get_oX()-R.get_oX();
b[1]=P.get_oY()-R.get_oY();
b[2]=P.get_oZ()-R.get_oZ();
//Calculate normal vector to plane PQ x PR
n=vpColVector::cross(a,b);
//Equation of the plane is given by:
double A = n[0];
double B = n[1];
double C = n[2];
double D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
double norm = sqrt(A*A+B*B+C*C) ;
plane.setA(A/norm) ;
plane.setB(B/norm) ;
plane.setC(C/norm) ;
plane.setD(D/norm) ;
}
static void
getPlaneInfo(vpPlane &oN, vpHomogeneousMatrix &cMo, vpColVector &cN, double &cd)
{
double A1 = cMo[0][0]*oN.getA()+ cMo[0][1]*oN.getB() + cMo[0][2]*oN.getC();
double B1 = cMo[1][0]*oN.getA()+ cMo[1][1]*oN.getB() + cMo[1][2]*oN.getC();
double C1 = cMo[2][0]*oN.getA()+ cMo[2][1]*oN.getB() + cMo[2][2]*oN.getC();
double D1 = oN.getD() - (cMo[0][3]*A1 + cMo[1][3]*B1 + cMo[2][3]*C1);
cN.resize(3) ;
cN[0] = A1 ;
cN[1] = B1 ;
cN[2] = C1 ;
cd = -D1 ;
}
void planeToABC(const vpPlane& pl, double& A,double& B, double& C)
{
A=-pl.getA()/pl.getD();
B=-pl.getB()/pl.getD();
C=-pl.getC()/pl.getD();
}
