1 // This is core/vgl/vgl_point_3d.h
2 #ifndef vgl_point_3d_h
3 #define vgl_point_3d_h
4 //:
5 // \file
6 // \brief a point in 3D nonhomogeneous space
7 // \author Don Hamilton, Peter Tu
8 //
9 // \verbatim
10 // Modifications
11 // Peter Vanroose - 2 July 2001 - Added constructor from 3 planes
12 // Peter Vanroose - 24 Oct. 2002 - Added coplanar()
13 // Peter Vanroose - 21 May 2009 - istream operator>> re-implemented
14 // \endverbatim
15
16 #include <iosfwd>
17 #include <vector>
18 #ifdef _MSC_VER
19 # include <vcl_msvc_warnings.h>
20 #endif
21 #include "vgl_fwd.h" // forward declare vgl_plane_3d
22 #include "vgl_vector_3d.h"
23 #include <cassert>
24
25 //: Represents a cartesian 3D point
26 template <class Type>
27 class vgl_point_3d
28 {
29 // the data associated with this point
30 Type x_;
31 Type y_;
32 Type z_;
33
34 public:
35
36 // Constructors/Initializers/Destructor------------------------------------
37
38 //: Default constructor
39 inline vgl_point_3d () = default;
40
41 //: Construct from three Types.
vgl_point_3d(Type px,Type py,Type pz)42 inline vgl_point_3d(Type px, Type py, Type pz) : x_(px), y_(py), z_(pz) {}
43
44 //: Construct from 3-array.
vgl_point_3d(const Type v[3])45 inline vgl_point_3d (const Type v[3]) : x_(v[0]), y_(v[1]), z_(v[2]) {}
46
47 //: Construct from homogeneous point
48 vgl_point_3d (vgl_homg_point_3d<Type> const& p);
49
50 //: Construct from 3 planes (intersection).
51 vgl_point_3d (vgl_plane_3d<Type> const& pl1,
52 vgl_plane_3d<Type> const& pl2,
53 vgl_plane_3d<Type> const& pl3);
54
55 //: Casting constructors
56 vgl_point_3d(vgl_point_3d<Type> const&) = default;
57
58 template<typename Other>
vgl_point_3d(vgl_point_3d<Other> const & other)59 explicit vgl_point_3d(vgl_point_3d<Other> const& other)
60 : x_(other.x()), y_(other.y()), z_(other.z()) {}
61
62 #if 0 // The compiler defaults for these are doing what they should do:
63 //: Copy constructor
64 inline vgl_point_3d(vgl_point_3d<Type> const& p) : x_(p.x()), y_(p.y()), z_(p.z()) {}
65 //: Destructor
66 inline ~vgl_point_3d () {}
67 //: Assignment
68 inline vgl_point_3d<Type>& operator=(vgl_point_3d<Type>const& p)
69 { set(p.x(),p.y(),p.z()); return *this; }
70 #endif
71
72 //: Test for equality
73 bool operator==(const vgl_point_3d<Type> &p) const;
74 inline bool operator!=(vgl_point_3d<Type>const& p) const { return !operator==(p); }
75
76 // Data Access-------------------------------------------------------------
77
x()78 inline Type &x() {return x_;}
y()79 inline Type &y() {return y_;}
z()80 inline Type &z() {return z_;}
81
x()82 inline Type x() const {return x_;}
y()83 inline Type y() const {return y_;}
z()84 inline Type z() const {return z_;}
85
86 //: Set \a x, \a y and \a z
87 // Note that \a x, \a y, and \a z can also be set individually
set(Type px,Type py,Type pz)88 inline void set(Type px, Type py, Type pz) { x_ = px; y_ = py; z_ = pz; }
89
90 //: Set \a x, \a y and \a z
91 // Note that \a x, \a y, and \a z can also be set individually
set(Type const p[3])92 inline void set(Type const p[3]) { x_ = p[0]; y_ = p[1]; z_ = p[2]; }
93
94 //: Return true iff the point is at infinity (an ideal point).
95 // Always returns false.
96 inline bool ideal(Type = (Type)0) const { return false; }
97
98 //: Read from stream, possibly with formatting
99 // Either just reads three blank-separated numbers,
100 // or reads three comma-separated numbers,
101 // or reads three numbers in parenthesized form "(123, 321, 567)"
102 // \relatesalso vgl_point_3d
103 std::istream& read(std::istream& is);
104 };
105
106 // +-+-+ point_3d simple I/O +-+-+
107
108 //: Write "<vgl_point_3d x,y,z> " to stream
109 // \relatesalso vgl_point_3d
110 template <class Type>
111 std::ostream& operator<<(std::ostream& s, vgl_point_3d<Type> const& p);
112
113 //: Read from stream, possibly with formatting
114 // Either just reads three blank-separated numbers,
115 // or reads three comma-separated numbers,
116 // or reads three numbers in parenthesized form "(123, 321, 567)"
117 // \relatesalso vgl_point_3d
118 template <class Type>
119 std::istream& operator>>(std::istream& s, vgl_point_3d<Type>& p);
120
121 // +-+-+ point_3d arithmetic +-+-+
122
123 //: Return true iff the point is at infinity (an ideal point).
124 // Always returns false.
125 template <class Type> inline
126 bool is_ideal(vgl_point_3d<Type> const&, Type = 0) { return false; }
127
128 //: The difference of two points is the vector from second to first point
129 // \relatesalso vgl_point_3d
130 template <class Type> inline
131 vgl_vector_3d<Type> operator-(vgl_point_3d<Type> const& p1,
132 vgl_point_3d<Type> const& p2)
133 { return vgl_vector_3d<Type>(p1.x()-p2.x(), p1.y()-p2.y(), p1.z()-p2.z()); }
134
135 //: Adding a vector to a point gives a new point at the end of that vector
136 // Note that vector + point is not defined! It's always point + vector.
137 // \relatesalso vgl_point_3d
138 template <class Type> inline
139 vgl_point_3d<Type> operator+(vgl_point_3d<Type> const& p,
140 vgl_vector_3d<Type> const& v)
141 { return vgl_point_3d<Type>(p.x()+v.x(), p.y()+v.y(), p.z()+v.z()); }
142
143 //: Adding a vector to a point gives the point at the end of that vector
144 // \relatesalso vgl_point_3d
145 template <class Type> inline
146 vgl_point_3d<Type>& operator+=(vgl_point_3d<Type>& p,
147 vgl_vector_3d<Type> const& v)
148 { p.set(p.x()+v.x(), p.y()+v.y(), p.z()+v.z()); return p; }
149
150 //: Subtracting a vector from a point is the same as adding the inverse vector
151 // \relatesalso vgl_point_3d
152 template <class Type> inline
153 vgl_point_3d<Type> operator-(vgl_point_3d<Type> const& p,
154 vgl_vector_3d<Type> const& v)
155 { return p + (-v); }
156
157 //: Subtracting a vector from a point is the same as adding the inverse vector
158 // \relatesalso vgl_point_3d
159 template <class Type> inline
160 vgl_point_3d<Type>& operator-=(vgl_point_3d<Type>& p,
161 vgl_vector_3d<Type> const& v)
162 { return p += (-v); }
163
164 // +-+-+ point_3d geometry +-+-+
165
166 //: cross ratio of four collinear points
167 // This number is projectively invariant, and it is the coordinate of p4
168 // in the reference frame where p2 is the origin (coordinate 0), p3 is
169 // the unity (coordinate 1) and p1 is the point at infinity.
170 // This cross ratio is often denoted as ((p1, p2; p3, p4)) (which also
171 // equals ((p3, p4; p1, p2)) or ((p2, p1; p4, p3)) or ((p4, p3; p2, p1)) )
172 // and is calculated as
173 // \verbatim
174 // p1 - p3 p2 - p3 (p1-p3)(p2-p4)
175 // ------- : -------- = --------------
176 // p1 - p4 p2 - p4 (p1-p4)(p2-p3)
177 // \endverbatim
178 // If three of the given points coincide, the cross ratio is not defined.
179 //
180 // In this implementation, a least-squares result is calculated when the
181 // points are not exactly collinear.
182 //
183 // \relatesalso vgl_point_3d
184 template <class T>
185 double cross_ratio(vgl_point_3d<T>const& p1, vgl_point_3d<T>const& p2,
186 vgl_point_3d<T>const& p3, vgl_point_3d<T>const& p4);
187
188 //: Are three points collinear, i.e., do they lie on a common line?
189 // \relatesalso vgl_point_3d
190 template <class Type> inline
collinear(vgl_point_3d<Type> const & p1,vgl_point_3d<Type> const & p2,vgl_point_3d<Type> const & p3)191 bool collinear(vgl_point_3d<Type> const& p1,
192 vgl_point_3d<Type> const& p2,
193 vgl_point_3d<Type> const& p3)
194 { return parallel(p1-p2, p1-p3); }
195
196 //: Return the relative distance to p1 wrt p1-p2 of p3.
197 // The three points should be collinear and p2 should not equal p1.
198 // This is the coordinate of p3 in the affine 1D reference frame (p1,p2).
199 // If p3=p1, the ratio is 0; if p1=p3, the ratio is 1.
200 // The mid point of p1 and p2 has ratio 0.5.
201 // Note that the return type is double, not Type, since the ratio of e.g.
202 // two vgl_vector_3d<int> need not be an int.
203 // \relatesalso vgl_point_3d
204 template <class Type> inline
ratio(vgl_point_3d<Type> const & p1,vgl_point_3d<Type> const & p2,vgl_point_3d<Type> const & p3)205 double ratio(vgl_point_3d<Type> const& p1,
206 vgl_point_3d<Type> const& p2,
207 vgl_point_3d<Type> const& p3)
208 { return (p3-p1)/(p2-p1); }
209
210 //: Return the point at a given ratio wrt two other points.
211 // By default, the mid point (ratio=0.5) is returned.
212 // Note that the third argument is Type, not double, so the midpoint of e.g.
213 // two vgl_point_3d<int> is not a valid concept. But the reflection point
214 // of p2 wrt p1 is: in that case f=-1.
215 // \relatesalso vgl_point_3d
216 template <class Type> inline
217 vgl_point_3d<Type> midpoint(vgl_point_3d<Type> const& p1,
218 vgl_point_3d<Type> const& p2,
219 Type f = (Type)0.5)
220 {
221 return vgl_point_3d<Type>((Type)((1-f)*p1.x() + f*p2.x()),
222 (Type)((1-f)*p1.y() + f*p2.y()),
223 (Type)((1-f)*p1.z() + f*p2.z()));
224 }
225
226
227 //: Return the point at the centre of gravity of two given points.
228 // Identical to midpoint(p1,p2).
229 // \relatesalso vgl_point_3d
230 template <class Type> inline
centre(vgl_point_3d<Type> const & p1,vgl_point_3d<Type> const & p2)231 vgl_point_3d<Type> centre(vgl_point_3d<Type> const& p1,
232 vgl_point_3d<Type> const& p2)
233 {
234 return vgl_point_3d<Type>((p1.x() + p2.x())/2 ,
235 (p1.y() + p2.y())/2 ,
236 (p1.z() + p2.z())/2 );
237 }
238
239 //: Return the point at the centre of gravity of three given points.
240 // \relatesalso vgl_point_3d
241 template <class Type> inline
centre(vgl_point_3d<Type> const & p1,vgl_point_3d<Type> const & p2,vgl_point_3d<Type> const & p3)242 vgl_point_3d<Type> centre(vgl_point_3d<Type> const& p1,
243 vgl_point_3d<Type> const& p2,
244 vgl_point_3d<Type> const& p3)
245 {
246 return vgl_point_3d<Type>((p1.x() + p2.x() + p3.x())/3 ,
247 (p1.y() + p2.y() + p3.y())/3 ,
248 (p1.z() + p2.z() + p3.z())/3 );
249 }
250
251 //: Return the point at the centre of gravity of four given points.
252 // \relatesalso vgl_point_3d
253 template <class Type> inline
centre(vgl_point_3d<Type> const & p1,vgl_point_3d<Type> const & p2,vgl_point_3d<Type> const & p3,vgl_point_3d<Type> const & p4)254 vgl_point_3d<Type> centre(vgl_point_3d<Type> const& p1,
255 vgl_point_3d<Type> const& p2,
256 vgl_point_3d<Type> const& p3,
257 vgl_point_3d<Type> const& p4)
258 {
259 return vgl_point_3d<Type>((p1.x() + p2.x() + p3.x() + p4.x())/4 ,
260 (p1.y() + p2.y() + p3.y() + p4.y())/4 ,
261 (p1.z() + p2.z() + p3.z() + p4.z())/4 );
262 }
263
264 //: Return the point at the centre of gravity of a set of given points.
265 // Beware of possible rounding errors when Type is e.g. int.
266 // \relatesalso vgl_point_3d
267 template <class Type> inline
centre(std::vector<vgl_point_3d<Type>> const & v)268 vgl_point_3d<Type> centre(std::vector<vgl_point_3d<Type> > const& v)
269 {
270 int n = (int)(v.size());
271 assert(n>0); // it is *not* correct to return the point (0,0) when n==0.
272 Type x = 0, y = 0, z = 0;
273 for (int i=0; i<n; ++i) x+=v[i].x(), y+=v[i].y(), z+=v[i].z();
274 return vgl_point_3d<Type>(x/n,y/n,z/n);
275 }
276
277 //: Return the "average deviation" of a set of given points from its centre of gravity.
278 // "Average" in the sense of the standard deviation (2-norm, i.e., square root
279 // of sum of squares) of the distances from that centre of gravity.
280 // \relatesalso vgl_point_3d
281 template <class Type>
282 double stddev(std::vector<vgl_point_3d<Type> > const& v);
283
284 //: Return true iff the 4 points are coplanar, i.e., they belong to a common plane
285 // \relatesalso vgl_point_3d
286 template <class Type>
287 bool coplanar(vgl_point_3d<Type> const& p1,
288 vgl_point_3d<Type> const& p2,
289 vgl_point_3d<Type> const& p3,
290 vgl_point_3d<Type> const& p4);
291
292 #define VGL_POINT_3D_INSTANTIATE(T) extern "please include vgl/vgl_point_3d.hxx first"
293
294 #endif // vgl_point_3d_h
295