iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
PointImpl.H
Go to the documentation of this file.
1 /*! @file
2  @author Randolph Voorhies (voorhies at usc dot edu)
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 
36 #include <nrt/Core/Debugging/Log.H>
37 #include <boost/algorithm/string/erase.hpp>
38 #include <cmath> // for std::round()
39 
40 // ######################################################################
41 template <class T, size_t Dimensions> inline
43 { }
44 
45 // ######################################################################
46 template <class T, size_t Dimensions>
47 template <typename ... S>
49 {
50  VariadicConstructorHelper(0, args...);
51 }
52 
53 // ######################################################################
54 template <class T, size_t Dimensions>
55 template <typename Head, typename ... Tail>
56 void nrt::Point<T,Dimensions>::VariadicConstructorHelper(size_t idx, Head h, Tail... tail)
57 {
58  itsData[idx] = nrt::clamped_convert<T>(h);
59  VariadicConstructorHelper(++idx, tail...);
60 }
61 
62 // ######################################################################
63 template <class T, size_t Dimensions>
64 template <typename Head>
66 {
67  itsData[idx] = nrt::clamped_convert<T>(h);
68 }
69 
70 
71 // ######################################################################
72 template <class T, size_t Dimensions>
73 nrt::Point<T, Dimensions>::Point(std::array<T,Dimensions> const & init) :
74  itsData(init)
75 { }
76 
77 // ######################################################################
78 template <class T, size_t Dimensions> template <class U> inline
80 {
81  std::transform(other.begin(), other.end(), this->begin(), nrt::clamped_convert<T,U>);
82 }
83 
84 // ######################################################################
85 template <class T, size_t Dimensions> inline
87 { return itsData[index]; }
88 
89 // ######################################################################
90 template <class T, size_t Dimensions> inline
91 T const & nrt::Point<T,Dimensions>::operator[](size_t index) const
92 { return itsData[index]; }
93 
94 // ######################################################################
95 template <class T, size_t Dimensions> template <class U> inline
97 {
98  return sqrt( squaredDistanceTo( other ) );
99 }
100 
101 // ######################################################################
102 template <class T, size_t Dimensions> template <class U> inline
104 {
105  double distance = 0.0;
106  T const * dptr = begin(); T const * dend = end(); U const * optr = other.begin();
107  while (dptr != dend)
108  {
109  double const diff = double(*dptr++) - double(*optr++);
110  distance += diff * diff;
111  }
112 
113  return distance;
114 }
115 
116 // ######################################################################
117 template <class T, size_t Dimensions> inline
119 {
120  double mag = 0;
121  for (T const & d : itsData) mag += d*d;
122  return sqrt(mag);
123 }
124 
125 // ######################################################################
126 template <class T, size_t Dimensions> template <class U> inline
128 {
129  double distance = 0.0;
130  T const * dptr = begin(); T const * dend = end(); U const * optr = other.begin();
131  while (dptr != dend)
132  {
133  double const diff = double(*dptr++) - double(*optr++);
134  distance += std::abs(diff);
135  }
136  return distance;
137 }
138 
139 // ######################################################################
140 template <class T, size_t Dimensions> inline
142 { return itsData.begin(); }
143 
144 // ######################################################################
145 template <class T, size_t Dimensions> inline
147 { return itsData.end(); }
148 
149 // ######################################################################
150 template <class T, size_t Dimensions> inline
152 { return itsData.begin(); }
153 
154 // ######################################################################
155 template <class T, size_t Dimensions> inline
157 { return itsData.end(); }
158 
159 // ######################################################################
160 // ######################################################################
161 template <class T> inline
163  nrt::Point<T,2>()
164 { }
165 
166 // ######################################################################
167 template <class T> inline
169  nrt::Point<T,2>(x,y)
170 { }
171 
172 // ######################################################################
173 template <class T> inline
175  nrt::Point<T,2>(other)
176 { }
177 
178 // ######################################################################
179 template <class T> inline
181  nrt::Point<T,2>(std::forward<nrt::Point<T,2> >(other))
182 { }
183 
184 // ######################################################################
185 template <class T> inline
186 nrt::Point2D<T>::Point2D(std::array<T,2> const & init) :
187  nrt::Point<T,2>(init)
188 { }
189 
190 // ######################################################################
191 template <class T> template <class U> inline
193  nrt::Point<T,2>(other)
194 { }
195 
196 // ######################################################################
197 template <class T> inline
199 { return nrt::Point<T,2>::operator[](0); }
200 
201 // ######################################################################
202 template <class T> inline
203 T const & nrt::Point2D<T>::x() const
204 { return nrt::Point<T,2>::operator[](0); }
205 
206 // ######################################################################
207 template <class T> inline
209 { return nrt::Point<T,2>::operator[](1); }
210 
211 // ######################################################################
212 template <class T> inline
213 T const & nrt::Point2D<T>::y() const
214 { return nrt::Point<T,2>::operator[](1); }
215 
216 // ######################################################################
217 template <class T> inline
219 { }
220 
221 // ######################################################################
222 template <class T> inline
223 nrt::Point3D<T>::Point3D(T x, T y, T z) :
224  nrt::Point<T, 3>(x,y,z)
225 { }
226 
227 // ######################################################################
228 template <class T> inline
229 nrt::Point3D<T>::Point3D(std::array<T,3> const & init) :
230  nrt::Point<T, 3>(init)
231 { }
232 
233 // ######################################################################
234 template <class T> inline
236  nrt::Point<T,3>(other)
237 { }
238 
239 // ######################################################################
240 template <class T> inline
242  nrt::Point<T,3>(std::forward<nrt::Point<T,3> >(other))
243 { }
244 
245 // ######################################################################
246 template <class T> template <class U> inline
248  nrt::Point<T,3>(other)
249 { }
250 
251 // ######################################################################
252 template <class T> inline
254 { return nrt::Point<T,3>::operator[](0); }
255 
256 // ######################################################################
257 template <class T> inline
258 T const & nrt::Point3D<T>::x() const
259 { return nrt::Point<T,3>::operator[](0); }
260 
261 // ######################################################################
262 template <class T> inline
264 { return nrt::Point<T,3>::operator[](1); }
265 
266 // ######################################################################
267 template <class T> inline
268 T const & nrt::Point3D<T>::y() const
269 { return nrt::Point<T,3>::operator[](1); }
270 
271 // ######################################################################
272 template <class T> inline
274 { return nrt::Point<T,3>::operator[](2); }
275 
276 // ######################################################################
277 template <class T> inline
278 T const & nrt::Point3D<T>::z() const
279 { return nrt::Point<T,3>::operator[](2); }
280 
281 // ######################################################################
282 template <class T, size_t Dimensions> inline
283 std::ostream& nrt::operator<<(std::ostream & out, nrt::Point<T, Dimensions> const & p)
284 {
285  for (size_t i = 0; i < Dimensions; ++i) { out << p[i]; if (i < Dimensions-1) out << ", "; }
286 
287  return out;
288 }
289 
290 // ######################################################################
291 template <class T, size_t Dimensions> inline
292 std::istream& nrt::operator>>(std::istream & in, nrt::Point<T, Dimensions> & p)
293 {
294  std::string stringRep;
295  std::getline( in, stringRep );
296  std::vector<std::string> splitVec = nrt::splitString(stringRep, ',');
297  if(splitVec.size() != Dimensions)
298  throw nrt::exception::Exception(std::string("Could Not Parse Point<" +
299  nrt::demangledName<T>() + "," + boost::lexical_cast<std::string>(Dimensions) +
300  "> From [" + stringRep + "]").c_str());
301 
302  try
303  {
304  std::transform(splitVec.begin(), splitVec.end(),
305  p.begin(),
306  [](std::string & s) { boost::erase_all( s, " " ); return boost::lexical_cast<T>(s); });
307  //@! TODO: make this support NaNs
308  //[](std::string & s) { if( s == "nan" ) return std::numeric_limits<T>::quiet_NaN(); boost::erase_all( s, " " ); return boost::lexical_cast<T>(s); });
309  }
310  catch(boost::bad_lexical_cast &e)
311  {
312  throw nrt::exception::Exception(std::string("Could Not Parse Point<" +
313  nrt::demangledName<T>() + "," + boost::lexical_cast<std::string>(Dimensions) +
314  "> From [" + stringRep + "] : " + e.what()).c_str());
315  }
316 
317  return in;
318 }
319 
320 // ######################################################################
321 template <class T, size_t Dimensions>
322 void nrt::paramValToString(nrt::Point<T, Dimensions> const & val, std::string & result)
323 {
324  std::stringstream out;
325  for (size_t i = 0; i < Dimensions; ++i) { out << result[i]; if(i < Dimensions-1) out << ", "; }
326  result = out.str();
327 }
328 
329 // ######################################################################
330 template <class T, size_t Dimensions>
331  void paramStringToVal(std::string const & valstring, nrt::Point<T, Dimensions> & result)
332 {
333  std::vector<std::string> splitVec = nrt::splitString(valstring, ',');
334  if(splitVec.size() != Dimensions)
335  throw nrt::exception::Exception(std::string("Could Not Parse Point<" +
336  nrt::demangledName<T>() + "," + boost::lexical_cast<std::string>(Dimensions) +
337  "> From [" + valstring + "]").c_str());
338 
339  try
340  {
341  std::transform(splitVec.begin(), splitVec.end(),
342  result.begin(),
343  [](std::string const & s) { return boost::lexical_cast<T>(s); });
344  }
345  catch(boost::bad_lexical_cast &e)
346  {
347  throw nrt::exception::Exception(std::string("Could Not Parse Point<" +
348  nrt::demangledName<T>() + "," + boost::lexical_cast<std::string>(Dimensions) +
349  "> From [" + valstring + "] : " + e.what()).c_str());
350  }
351 }
352 
353 // ######################################################################
354 template <class T, size_t S> inline
355 nrt::Point<T, S> nrt::scale(nrt::Point<T, S> const & src, double const factor)
356 { return src; }
357 
358 // ######################################################################
359 template <class T> inline
360 nrt::Point2D<T> nrt::rotate(nrt::Point2D<T> const & src, double const angle)
361 {
362  double const s = sin(angle);
363  double const c = cos(angle);
364  double const tx = src.x();
365  double const ty = src.y();
366 
367  double const rotatedX = tx * c - ty * s;
368  double const rotatedY = ty * c + tx * s;
369 
370  return nrt::Point2D<T>(nrt::clamped_convert<T>(rotatedX), nrt::clamped_convert<T>(rotatedY));
371 }
372 
373 // ######################################################################
374 template <class T> inline
375 nrt::Point2D<T> nrt::rotateAbout(nrt::Point2D<T> const & src, nrt::Point2D<T> const & p, double const angle)
376 {
377  double const s = sin(angle);
378  double const c = cos(angle);
379  double const tx = src.x();
380  double const ty = src.y();
381  double const px = p.x();
382  double const py = p.y();
383 
384  double const rotatedX = px + (tx - px) * c - (ty - py) * s;
385  double const rotatedY = py + (ty - py) * c + (tx - px) * s;
386 
387  return nrt::Point2D<T>(nrt::clamped_convert<T>(rotatedX), nrt::clamped_convert<T>(rotatedY));
388 }
389 
390 // ######################################################################
391 template <class T, size_t S> inline
392 bool nrt::operator==(nrt::Point<T, S> const & lhs, nrt::Point<T, S> const & rhs)
393 { return std::equal(lhs.begin(), lhs.end(), rhs.begin()); }
394 
395 // ######################################################################
396 template <class T, size_t S> inline
397 bool nrt::operator!=(nrt::Point<T, S> const & lhs, nrt::Point<T, S> const & rhs)
398 { return ! std::equal(lhs.begin(), lhs.end(), rhs.begin()); }
399 
400 // ######################################################################
401 template <class T, size_t S>
403 { nrt::Point<T, S> ret(lhs); ret += rhs; return ret; }
404 
405 // ######################################################################
406 template <class T, size_t S>
408 { nrt::Point<T, S> ret(lhs); ret -= rhs; return ret; }
409 
410 // ######################################################################
411 template <class T, size_t S>
412 nrt::Point<T, S> nrt::operator*(nrt::Point<T, S> const & lhs, double const rhs)
413 { nrt::Point<T, S> ret(lhs); ret *= rhs; return ret; }
414 
415 // ######################################################################
416 template <class T, size_t S>
417 nrt::Point<T, S> nrt::operator*(double const lhs, nrt::Point<T, S> const & rhs)
418 { nrt::Point<T, S> ret(rhs); ret *= lhs; return ret; }
419 
420 // ######################################################################
421 template <class T, size_t S>
422 nrt::Point<T, S> nrt::operator/(nrt::Point<T, S> const & lhs, double const rhs)
423 { nrt::Point<T, S> ret(lhs); ret /= rhs; return ret; }
424 
425 // ######################################################################
426 template <class T, size_t S>
427 nrt::Point<T, S> & nrt::operator*=(nrt::Point<T, S> & lhs, double const rhs)
428 {
429  T * lptr = lhs.begin(); T * lend = lhs.end();
430  while (lptr != lend) { *lptr = nrt::clamped_convert<T>(*lptr * rhs); ++lptr; }
431  return lhs;
432 }
433 
434 // ######################################################################
435 template <class T, size_t S>
436 nrt::Point<T, S> & nrt::operator/=(nrt::Point<T, S> & lhs, double const rhs)
437 {
438  T * lptr = lhs.begin(); T * lend = lhs.end();
439  while (lptr != lend) { *lptr = nrt::clamped_convert<T>(*lptr / rhs); ++lptr; }
440  return lhs;
441 }
442 
443 // ######################################################################
444 template <class T, size_t S>
446 {
447  T * lptr = lhs.begin(); T * lend = lhs.end(); T const * rptr = rhs.begin();
448  while (lptr != lend) { *lptr = nrt::clamped_convert<T>(*lptr + *rptr++); ++lptr; }
449  return lhs;
450 }
451 
452 // ######################################################################
453 template <class T, size_t S>
455 {
456  T * lptr = lhs.begin(); T * lend = lhs.end(); T const * rptr = rhs.begin();
457  while (lptr != lend) { *lptr = nrt::clamped_convert<T>(*lptr - *rptr++); ++lptr; }
458  return lhs;
459 }
460 
461 
462 
463 // ######################################################################
464 template <class T, size_t S>
466 {
467  nrt::Point<T, S> ret(lhs);
468  T * lptr = ret.begin(); T * lend = ret.end(); T const * rptr = rhs.begin();
469  while (lptr != lend) { *lptr = nrt::clamped_convert<T>(*lptr * *rptr++); ++lptr; }
470  return ret;
471 }
472 
473 // ######################################################################
474 template <class T, size_t S>
476 {
477  nrt::Point<T, S> ret(lhs);
478  T * lptr = ret.begin(); T * lend = ret.end(); T const * rptr = rhs.begin();
479  while (lptr != lend) { *lptr = nrt::clamped_convert<T>(*lptr / *rptr++); ++lptr; }
480  return ret;
481 }
482