ViSP  2.8.0
vpPlane.cpp
1 /****************************************************************************
2  *
3  * $Id: vpPlane.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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 
72 {
73  setA(0) ;
74  setB(0) ;
75  setC(0) ;
76  setD(0) ;
77 }
78 
89 vpPlane::vpPlane(const double A,const double B,const double C, const double D)
90 {
91  setA(A) ;
92  setB(B) ;
93  setC(C) ;
94  setD(D) ;
95 }
96 
101 {
102  setA(P.getA()) ;
103  setB(P.getB()) ;
104  setC(P.getC()) ;
105  setD(P.getD()) ;
106 }
107 
123 {
124  //Equation of the plane is given by:
125  A = n[0];
126  B = n[1];
127  C = n[2];
128 
129  D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
130 }
131 
137 void vpPlane::init(const vpPlane& P)
138 {
139  setA(P.getA()) ;
140  setB(P.getB()) ;
141  setC(P.getC()) ;
142  setD(P.getD()) ;
143 }
144 
155 void vpPlane::init(const vpColVector & P, const vpColVector &n)
156 {
157  //Equation of the plane is given by:
158  A = n[0];
159  B = n[1];
160  C = n[2];
161 
162  D=-(A*P[0]+B*P[1]+C*P[2]);
163 }
164 
172 void
173 vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R)
174 {
175  vpColVector a(3);
176  vpColVector b(3);
177  vpColVector n(3);
178  //Calculate vector corresponding to PQ
179  a[0]=P.get_X()-Q.get_X();
180  a[1]=P.get_Y()-Q.get_Y();
181  a[2]=P.get_Z()-Q.get_Z();
182 
183  //Calculate vector corresponding to PR
184  b[0]=P.get_X()-R.get_X();
185  b[1]=P.get_Y()-R.get_Y();
186  b[2]=P.get_Z()-R.get_Z();
187 
188  //Calculate normal vector to plane PQ x PR
189  n=vpColVector::cross(a,b);
190 
191  //Equation of the plane is given by:
192  A = n[0];
193  B = n[1];
194  C = n[2];
195  D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
196 
197  double norm = sqrt(A*A+B*B+C*C) ;
198  A /= norm ;
199  B /= norm ;
200  C /= norm ;
201  D /= norm ;
202 }
203 
204 
213 vpPlane::vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R)
214 {
215  init(P,Q,R) ;
216 }
217 
227 {
228  vpColVector n(3);
229  n[0] = A ;
230  n[1] = B ;
231  n[2] = C ;
232 
233  return n ;
234 }
235 
247 {
248  n.resize(3) ;
249  n[0] = A ;
250  n[1] = B ;
251  n[2] = C ;
252 }
253 
260 void
262 {
263  double x0,y0,z0 ;
264  double rho ;
265 
266  x0 = P.get_X()/P.get_W() ;
267  y0 = P.get_Y()/P.get_W() ;
268  z0 = P.get_Z()/P.get_W() ;
269 
270  rho = - (A*x0+B*y0+C*z0+D)/(A*A+B*B+C*C) ;
271 
272  Pproj.set_X(x0+A*rho) ;
273  Pproj.set_Y(y0+B*rho) ;
274  Pproj.set_Z(z0+C*rho) ;
275  Pproj.set_W(1) ;
276 }
277 
278 
279 double
281  const vpPoint &M1,
282  vpColVector &H ) const
283 {
284 
285  double k,scal;
286  double R[3];
287 
288  // if(M0.get_X()!=0 || M0.get_Y()!=0 || M0.get_Z()!=0)
289  if(std::fabs(M0.get_X()) > std::numeric_limits<double>::epsilon()
290  || std::fabs(M0.get_Y()) > std::numeric_limits<double>::epsilon()
291  || std::fabs(M0.get_Z()) > std::numeric_limits<double>::epsilon())
292  {
293  R[0]= M1.get_X() - M0.get_X();
294  R[1]= M1.get_Y() - M0.get_Y();
295  R[2]= M1.get_Z() - M0.get_Z();
296 
297  scal = getA()*R[0] + getB()*R[1] + getC()*R[2];
298  //if (scal != 0)
299  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
300  k = -( getA()*M0.get_X() + getB()*M0.get_Y() + getC()*M0.get_Z() + getD())/scal;
301  else
302  k = 0;
303 
304  H[0] = M0.get_X()+ k*R[0];
305  H[1] = M0.get_Y()+ k*R[1];
306  H[2] = M0.get_Z()+ k*R[2];
307  }
308  else
309  {
310  scal = getA()*M1.get_X() + getB()*M1.get_Y() + getC()*M1.get_Z();
311  //if (scal != 0)
312  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
313  k = -getD()/scal;
314  else
315  k=0;
316  H[0] = k*M1.get_X();
317  H[1] = k*M1.get_Y();
318  H[2] = k*M1.get_Z();
319  }
320 
321  return k;
322 
323 }
324 
326 {
327 
328  double k,scal;
329 
330  scal = A*M1[0] + B*M1[1] + C*M1[2];
331  //if (scal != 0)
332  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
333  k = -getD()/scal;
334  else
335  k=0;
336  H[0] = k*M1[0];
337  H[1] = k*M1[1];
338  H[2] = k*M1[2];
339 
340  return k;
341 
342 }
343 
353 {
354  // Save current plane parameters
355  double Ao = A, Bo = B, Co = C, Do =D ;
356  A = cMo[0][0]*Ao + cMo[0][1]*Bo + cMo[0][2]*Co;
357  B = cMo[1][0]*Ao + cMo[1][1]*Bo + cMo[1][2]*Co;
358  C = cMo[2][0]*Ao + cMo[2][1]*Bo + cMo[2][2]*Co;
359  D = Do - (cMo[0][3]*A + cMo[1][3]*B + cMo[2][3]*C);
360 }
361 
double B
Definition: vpPlane.h:76
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:280
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:352
void set_Y(const double Y)
Set the point Y coordinate in the camera frame.
Definition: vpPoint.h:178
void init(const vpPoint &P, const vpPoint &Q, const vpPoint &R)
Definition: vpPlane.cpp:173
void projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const
Definition: vpPlane.cpp:261
vpPlane()
Definition: vpPlane.cpp:71
vpColVector getNormal() const
Definition: vpPlane.cpp:226
void setA(const double A)
Definition: vpPlane.h:91
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:325
void setC(const double C)
Definition: vpPlane.h:95
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.h:122
double A
Definition: vpPlane.h:76
void setD(const double D)
Definition: vpPlane.h:97
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:114
double getA() const
Definition: vpPlane.h:112
double getC() const
Definition: vpPlane.h:116
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
void setB(const double B)
Definition: vpPlane.h:93
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.h:118
double getD() const
Definition: vpPlane.h:118
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94