OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Time.hpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Modified and adapted by
5  * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
6  *
7  * Original Copyright (c) 2008, Willow Garage, Inc.
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of Willow Garage, Inc. nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *********************************************************************/
37 /*
38  * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
39  * Modified: Andreas Forster (an.forster@gmail.com)
40  */
41 
51 #ifndef INCLUDE_OKVIS_IMPLEMENTATION_TIME_HPP_
52 #define INCLUDE_OKVIS_IMPLEMENTATION_TIME_HPP_
53 
54 /*********************************************************************
55  ** Headers
56  *********************************************************************/
57 
58 //#include <ros/platform.h>
59 #include <iostream>
60 #include <cmath>
61 //#include <ros/exception.h>
62 
63 /*********************************************************************
64  ** Cross Platform Headers
65  *********************************************************************/
66 
67 #ifdef WIN32
68 #include <sys/timeb.h>
69 #else
70 #include <sys/time.h>
71 #endif
72 
74 namespace okvis {
75 
76 template<class T, class D>
77 T& TimeBase<T, D>::fromNSec(uint64_t t) {
78  sec = (int32_t)(t / 1000000000);
79  nsec = (int32_t)(t % 1000000000);
80 
81  normalizeSecNSec(sec, nsec);
82 
83  return *static_cast<T*>(this);
84 }
85 
86 template<class T, class D>
87 D TimeBase<T, D>::operator-(const T &rhs) const {
88  return D((int32_t) sec - (int32_t) rhs.sec,
89  (int32_t) nsec - (int32_t) rhs.nsec); // carry handled in ctor
90 }
91 
92 template<class T, class D>
93 T TimeBase<T, D>::operator-(const D &rhs) const {
94  return *static_cast<const T*>(this) + (-rhs);
95 }
96 
97 template<class T, class D>
98 T TimeBase<T, D>::operator+(const D &rhs) const {
99  int64_t sec_sum = (int64_t) sec + (int64_t) rhs.sec;
100  int64_t nsec_sum = (int64_t) nsec + (int64_t) rhs.nsec;
101 
102  // Throws an exception if we go out of 32-bit range
103  normalizeSecNSecUnsigned(sec_sum, nsec_sum);
104 
105  // now, it's safe to downcast back to uint32 bits
106  return T((uint32_t) sec_sum, (uint32_t) nsec_sum);
107 }
108 
109 template<class T, class D>
110 T& TimeBase<T, D>::operator+=(const D &rhs) {
111  *this = *this + rhs;
112  return *static_cast<T*>(this);
113 }
114 
115 template<class T, class D>
116 T& TimeBase<T, D>::operator-=(const D &rhs) {
117  *this += (-rhs);
118  return *static_cast<T*>(this);
119 }
120 
121 template<class T, class D>
122 bool TimeBase<T, D>::operator==(const T &rhs) const {
123  return sec == rhs.sec && nsec == rhs.nsec;
124 }
125 
126 template<class T, class D>
127 bool TimeBase<T, D>::operator<(const T &rhs) const {
128  if (sec < rhs.sec)
129  return true;
130  else if (sec == rhs.sec && nsec < rhs.nsec)
131  return true;
132  return false;
133 }
134 
135 template<class T, class D>
136 bool TimeBase<T, D>::operator>(const T &rhs) const {
137  if (sec > rhs.sec)
138  return true;
139  else if (sec == rhs.sec && nsec > rhs.nsec)
140  return true;
141  return false;
142 }
143 
144 template<class T, class D>
145 bool TimeBase<T, D>::operator<=(const T &rhs) const {
146  if (sec < rhs.sec)
147  return true;
148  else if (sec == rhs.sec && nsec <= rhs.nsec)
149  return true;
150  return false;
151 }
152 
153 template<class T, class D>
154 bool TimeBase<T, D>::operator>=(const T &rhs) const {
155  if (sec > rhs.sec)
156  return true;
157  else if (sec == rhs.sec && nsec >= rhs.nsec)
158  return true;
159  return false;
160 }
161 
162 }
163 
164 #endif // INCLUDE_OKVIS_IMPLEMENTATION_TIME_HPP_
165 
void normalizeSecNSecUnsigned(int64_t &sec, int64_t &nsec)
Definition: Time.cpp:391
void normalizeSecNSec(uint64_t &sec, uint64_t &nsec)
Definition: Time.cpp:370
T & fromNSec(uint64_t t)
Definition: Time.hpp:77
bool operator>=(const T &rhs) const
Definition: Time.hpp:154
T & operator+=(const D &rhs)
Definition: Time.hpp:110
bool operator<(const T &rhs) const
Definition: Time.hpp:127
bool operator==(const T &rhs) const
Definition: Time.hpp:122
bool operator<=(const T &rhs) const
Definition: Time.hpp:145
T operator+(const D &rhs) const
Definition: Time.hpp:98
D operator-(const T &rhs) const
Definition: Time.hpp:87
bool operator>(const T &rhs) const
Definition: Time.hpp:136
T & operator-=(const D &rhs)
Definition: Time.hpp:116