ViSP  2.9.0
vpPlane.cpp
1 /****************************************************************************
2  *
3  * $Id: vpPlane.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Plane geometrical structure.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
49 #include <visp/vpPlane.h>
50 
51 #include <cmath> // std::fabs
52 #include <limits> // numeric_limits
53 
57 vpPlane&
59 {
60  A = p.A ;
61  B = p.B ;
62  C = p.C ;
63  D = p.D ;
64 
65  return *this ;
66 }
67 
71 vpPlane::vpPlane() : A(0), B(0), C(0), D(0) {}
72 
83 vpPlane::vpPlane(const double a,const double b,const double c, const double d)
84  : A(a), B(b), C(c), D(d) {}
85 
90  : A(0), B(0), C(0), D(0)
91 {
92  setA(P.getA()) ;
93  setB(P.getB()) ;
94  setC(P.getC()) ;
95  setD(P.getD()) ;
96 }
97 
113  : A(0), B(0), C(0), D(0)
114 {
115  //Equation of the plane is given by:
116  A = n[0];
117  B = n[1];
118  C = n[2];
119 
120  D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
121 }
122 
128 void vpPlane::init(const vpPlane& P)
129 {
130  setA(P.getA()) ;
131  setB(P.getB()) ;
132  setC(P.getC()) ;
133  setD(P.getD()) ;
134 }
135 
146 void vpPlane::init(const vpColVector & P, const vpColVector &n)
147 {
148  //Equation of the plane is given by:
149  A = n[0];
150  B = n[1];
151  C = n[2];
152 
153  D=-(A*P[0]+B*P[1]+C*P[2]);
154 }
155 
163 void
164 vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R)
165 {
166  vpColVector a(3);
167  vpColVector b(3);
168  vpColVector n(3);
169  //Calculate vector corresponding to PQ
170  a[0]=P.get_X()-Q.get_X();
171  a[1]=P.get_Y()-Q.get_Y();
172  a[2]=P.get_Z()-Q.get_Z();
173 
174  //Calculate vector corresponding to PR
175  b[0]=P.get_X()-R.get_X();
176  b[1]=P.get_Y()-R.get_Y();
177  b[2]=P.get_Z()-R.get_Z();
178 
179  //Calculate normal vector to plane PQ x PR
180  n=vpColVector::cross(a,b);
181 
182  //Equation of the plane is given by:
183  A = n[0];
184  B = n[1];
185  C = n[2];
186  D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
187 
188  double norm = sqrt(A*A+B*B+C*C) ;
189  A /= norm ;
190  B /= norm ;
191  C /= norm ;
192  D /= norm ;
193 }
194 
195 
204 vpPlane::vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R)
205  : A(0), B(0), C(0), D(0)
206 {
207  init(P,Q,R) ;
208 }
209 
219 {
220  vpColVector n(3);
221  n[0] = A ;
222  n[1] = B ;
223  n[2] = C ;
224 
225  return n ;
226 }
227 
239 {
240  n.resize(3) ;
241  n[0] = A ;
242  n[1] = B ;
243  n[2] = C ;
244 }
245 
252 void
254 {
255  double x0,y0,z0 ;
256  double rho ;
257 
258  x0 = P.get_X()/P.get_W() ;
259  y0 = P.get_Y()/P.get_W() ;
260  z0 = P.get_Z()/P.get_W() ;
261 
262  rho = - (A*x0+B*y0+C*z0+D)/(A*A+B*B+C*C) ;
263 
264  Pproj.set_X(x0+A*rho) ;
265  Pproj.set_Y(y0+B*rho) ;
266  Pproj.set_Z(z0+C*rho) ;
267  Pproj.set_W(1) ;
268 }
269 
270 
271 double
273  const vpPoint &M1,
274  vpColVector &H ) const
275 {
276 
277  double k,scal;
278  double R[3];
279 
280  // if(M0.get_X()!=0 || M0.get_Y()!=0 || M0.get_Z()!=0)
281  if(std::fabs(M0.get_X()) > std::numeric_limits<double>::epsilon()
282  || std::fabs(M0.get_Y()) > std::numeric_limits<double>::epsilon()
283  || std::fabs(M0.get_Z()) > std::numeric_limits<double>::epsilon())
284  {
285  R[0]= M1.get_X() - M0.get_X();
286  R[1]= M1.get_Y() - M0.get_Y();
287  R[2]= M1.get_Z() - M0.get_Z();
288 
289  scal = getA()*R[0] + getB()*R[1] + getC()*R[2];
290  //if (scal != 0)
291  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
292  k = -( getA()*M0.get_X() + getB()*M0.get_Y() + getC()*M0.get_Z() + getD())/scal;
293  else
294  k = 0;
295 
296  H[0] = M0.get_X()+ k*R[0];
297  H[1] = M0.get_Y()+ k*R[1];
298  H[2] = M0.get_Z()+ k*R[2];
299  }
300  else
301  {
302  scal = getA()*M1.get_X() + getB()*M1.get_Y() + getC()*M1.get_Z();
303  //if (scal != 0)
304  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
305  k = -getD()/scal;
306  else
307  k=0;
308  H[0] = k*M1.get_X();
309  H[1] = k*M1.get_Y();
310  H[2] = k*M1.get_Z();
311  }
312 
313  return k;
314 
315 }
316 
318 {
319 
320  double k,scal;
321 
322  scal = A*M1[0] + B*M1[1] + C*M1[2];
323  //if (scal != 0)
324  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
325  k = -getD()/scal;
326  else
327  k=0;
328  H[0] = k*M1[0];
329  H[1] = k*M1[1];
330  H[2] = k*M1[2];
331 
332  return k;
333 
334 }
335 
345 {
346  // Save current plane parameters
347  double Ao = A, Bo = B, Co = C, Do =D ;
348  A = cMo[0][0]*Ao + cMo[0][1]*Bo + cMo[0][2]*Co;
349  B = cMo[1][0]*Ao + cMo[1][1]*Bo + cMo[1][2]*Co;
350  C = cMo[2][0]*Ao + cMo[2][1]*Bo + cMo[2][2]*Co;
351  D = Do - (cMo[0][3]*A + cMo[1][3]*B + cMo[2][3]*C);
352 }
353 
360 VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpPlane& p)
361 {
362  return (os << "("<<p.getA() << ","<<p.getB()
363  << ","<<p.getC()<< ","<<p.getD() <<") ") ;
364 } ;
double B
Definition: vpPlane.h:76
void setD(const double d)
Definition: vpPlane.h:96
vpPlane & operator=(const vpPlane &f)
Definition: vpPlane.cpp:58
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:153
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
double C
Definition: vpPlane.h:76
void set_X(const double X)
Set the point X coordinate in the camera frame.
Definition: vpPoint.h:176
double get_W() const
Get the point W coordinate in the camera frame.
Definition: vpPoint.h:124
Class that defines what is a point.
Definition: vpPoint.h:65
double rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const
Definition: vpPlane.cpp:272
double D
Definition: vpPlane.h:76
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.h:180
void set_W(const double W)
Set the point W coordinate in the camera frame.
Definition: vpPoint.h:182
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:344
void set_Y(const double Y)
Set the point Y coordinate in the camera frame.
Definition: vpPoint.h:178
void setA(const double a)
Definition: vpPlane.h:90
void init(const vpPoint &P, const vpPoint &Q, const vpPoint &R)
Definition: vpPlane.cpp:164
void projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const
Definition: vpPlane.cpp:253
vpPlane()
Definition: vpPlane.cpp:71
vpColVector getNormal() const
Definition: vpPlane.cpp:218
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.h:120
double getIntersection(const vpColVector &M1, vpColVector &H) const
Definition: vpPlane.cpp:317
void setC(const double c)
Definition: vpPlane.h:94
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.h:122
double A
Definition: vpPlane.h:76
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
double getB() const
Definition: vpPlane.h:113
void setB(const double b)
Definition: vpPlane.h:92
double getA() const
Definition: vpPlane.h:111
double getC() const
Definition: vpPlane.h:115
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.h:118
double getD() const
Definition: vpPlane.h:117
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94