LinearMotionArithmetic.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_MATH_LINEARMOTIONARITHMETIC_H
17 #define SURGSIM_MATH_LINEARMOTIONARITHMETIC_H
18 
19 #include <array>
20 #include <iostream>
21 
22 #include <Eigen/Core>
23 
26 #include "SurgSim/Math/Vector.h"
27 
28 namespace SurgSim
29 {
30 namespace Math
31 {
32 
49 template <typename T>
51 {
52 public:
54  LinearMotion();
55 
60  LinearMotion(const T& start, const T& end);
61 
64  LinearMotion(const LinearMotion<T>& m);
65 
69 
73 
77 
80  Interval<T> toInterval() const;
81 
86 
88  bool containsZero() const;
89 
93  bool isApprox(const LinearMotion<T>& i, const T& epsilon) const;
94 
97  bool operator==(const LinearMotion<T>& m) const;
98 
101  bool operator!=(const LinearMotion<T>& m) const;
102 
105  LinearMotion<T> operator+(const LinearMotion<T>& m) const;
107  LinearMotion<T> operator-(const LinearMotion<T>& m) const;
110 
115  Interval<T> operator*(const LinearMotion<T>& m) const;
116 
122  Interval<T> operator/(const LinearMotion<T>& m) const;
123 
125  T getStart() const;
126 
128  T getEnd() const;
129 
132  T atTime(const T& t) const;
133 
135  LinearMotion<T> firstHalf() const;
136 
138  LinearMotion<T> secondHalf() const;
139 
140 private:
143 
145  T m_end;
146 };
147 
155 template <class T, int N>
157 {
158  static_assert(N > 0, "LinearMotion must have dimensionality > 0.");
159 
160 public:
162  LinearMotionND();
163 
166  explicit LinearMotionND(const std::array<LinearMotion<T>, N>& x);
167 
170  LinearMotionND(const LinearMotionND<T, N>& motion);
171 
175 
179  LinearMotionND(const std::array<T, N>& a, const std::array<T, N>& b);
180 
184 
188 
192 
197  bool isApprox(const LinearMotionND<T, N>& motion, const T& epsilon) const;
198 
201  bool operator==(const LinearMotionND<T, N>& motion) const;
202 
205  bool operator!=(const LinearMotionND<T, N>& motion) const;
206 
214 
220 
227 
230  Interval<T> dotProduct(const LinearMotionND<T, N>& motion) const;
231 
235  const LinearMotion<T>& getAxis(int i) const;
236 
238  void getStart(std::array<T, N>* start) const;
239 
241  void getEnd(std::array<T, N>* end) const;
242 
245 
248 
249 private:
251  std::array<LinearMotion<T>, N> m_motion;
252 };
253 
257 template <class T>
258 class LinearMotionND<T, 3>
259 {
260 public:
262  typedef Eigen::Matrix<T, 3, 1> Vector3;
263 
265  LinearMotionND();
266 
269  explicit LinearMotionND(const std::array<LinearMotion<T>, 3>& x);
270 
275  LinearMotionND(const LinearMotion<T>& a, const LinearMotion<T>& b, const LinearMotion<T>& c);
276 
279  LinearMotionND(const LinearMotionND<T, 3>& motion);
280 
284 
288  LinearMotionND(const std::array<T, 3>& start, const std::array<T, 3>& end);
289 
293  LinearMotionND(const Vector3& start, const Vector3& end);
294 
298 
302 
306 
311  bool isApprox(const LinearMotionND<T, 3>& motion, const T& epsilon) const;
312 
315  bool operator==(const LinearMotionND<T, 3>& motion) const;
316 
319  bool operator!=(const LinearMotionND<T, 3>& motion) const;
320 
328 
334 
341 
345  Interval<T> dotProduct(const LinearMotionND<T, 3>& motion, const Interval<T>& range) const;
346 
350  IntervalND<T, 3> crossProduct(const LinearMotionND<T, 3>& motion, const Interval<T>& range) const;
351 
353  Interval<T> magnitudeSquared(const Interval<T>& range) const;
354 
356  Interval<T> magnitude(const Interval<T>& range) const;
357 
361  const LinearMotion<T>& getAxis(int i) const;
362 
364  void getStart(std::array<T, 3>* start) const;
365 
367  void getEnd(std::array<T, 3>* end) const;
368 
370  Vector3 getStart() const;
371 
373  Vector3 getEnd() const;
374 
377  Vector3 atTime(const T& t) const;
378 
381 
384 
385 private:
387  std::array<LinearMotion<T>, 3> m_motion;
388 };
389 
390 // Linear motion utilities
391 
397 template <typename T>
398 std::ostream& operator<<(std::ostream& o, const LinearMotion<T>& motion);
399 
400 // Linear motion ND utilities
401 
408 template <typename T, int N>
409 std::ostream& operator<<(std::ostream& o, const LinearMotionND<T, N>& motion);
410 
411 // Linear motion 3D utilities
412 
418 template <class T>
420 
427 template <class T, int A>
429 
435 template <class T>
437 
443 template <class T>
445 
451 template <class T>
453 
461 template <class T>
463  Polynomial<T, 2>* resultXAxis, Polynomial<T, 2>* resultYAxis, Polynomial<T, 2>* resultZAxis);
464 
472 template <class T>
474  const LinearMotionND<T, 3>& c);
475 
483 template <class T>
485  const LinearMotionND<T, 3>& c, const Interval<T>& range);
486 
491 template <class T>
493 
494 }; // Math
495 }; // SurgSim
496 
498 
499 #endif // SURGSIM_MATH_LINEARMOTIONARITHMETIC_H
Definition: CompoundShapeToGraphics.cpp:29
Polynomial< T, 2 > analyticMagnitudeSquared(const LinearMotionND< T, 3 > &motion)
Calculate the magnitude squared of a linear motion 3 group as a polynomial.
Definition: LinearMotionArithmetic-inl.h:697
bool isApprox(const LinearMotion< T > &i, const T &epsilon) const
Definition: LinearMotionArithmetic-inl.h:74
T getEnd() const
Definition: LinearMotionArithmetic-inl.h:138
std::array< LinearMotion< T >, 3 > m_motion
The 3 dimensional group of linear motions.
Definition: LinearMotionArithmetic.h:387
IntervalND<T,3> defines the concept of a group of mathematical intervals specialized to 3 intervals a...
Definition: IntervalArithmetic.h:307
LinearMotionND<T, 3> specializes the LinearMotionND<T, N> class for 3 dimensions. ...
Definition: LinearMotionArithmetic.h:258
Polynomial<T, 3> specializes the Polynomial class for degree 3 (cubic polynomials) ...
Definition: Polynomial.h:255
IntervalND defines the concept of a group of mathematical intervals and provides operations on them i...
Definition: IntervalArithmetic.h:199
Polynomial< T, 2 > analyticCrossProductYAxis(const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b)
Calculate the Y axis of an analytic cross product as a Polynomial.
Definition: LinearMotionArithmetic-inl.h:653
std::array< LinearMotion< T >, N > m_motion
The N dimensional group of linear motions.
Definition: LinearMotionArithmetic.h:251
LinearMotion< T > firstHalf() const
Definition: LinearMotionArithmetic-inl.h:150
Polynomial<T, 2> specializes the Polynomial class for degree 2 (quadratic polynomials) ...
Definition: Polynomial.h:183
Polynomial< T, 2 > analyticDotProduct(const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b)
Calculate an analytic dot product as a Polynomial.
Definition: LinearMotionArithmetic-inl.h:625
LinearMotion< T > & operator-=(const LinearMotion< T > &m)
Definition: LinearMotionArithmetic-inl.h:112
LinearMotion< T > secondHalf() const
Definition: LinearMotionArithmetic-inl.h:156
Polynomial< T, 2 > analyticCrossProductXAxis(const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b)
Calculate the X axis of an analytic cross product as a Polynomial.
Definition: LinearMotionArithmetic-inl.h:647
void analyticCrossProduct(const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b, Polynomial< T, 2 > *resultXAxis, Polynomial< T, 2 > *resultYAxis, Polynomial< T, 2 > *resultZAxis)
Calculate an analytic cross product as a Polynomial.
Definition: LinearMotionArithmetic-inl.h:665
T getStart() const
Definition: LinearMotionArithmetic-inl.h:132
Polynomial< T, 2 > analyticCrossProductAxis(const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b)
Calculate a single axis of an analytic cross product as a Polynomial.
Definition: LinearMotionArithmetic-inl.h:633
T m_end
The end point of the linear motion.
Definition: LinearMotionArithmetic.h:145
LinearMotion< T > operator-(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:106
bool containsZero() const
Definition: LinearMotionArithmetic-inl.h:68
Definitions of small fixed-size vector types.
Interval< T > operator/(const LinearMotion< T > &m) const
Standard arithmetic operators extended to interval groups.
Definition: LinearMotionArithmetic-inl.h:126
Interval< T > tripleProduct(const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b, const LinearMotionND< T, 3 > &c, const Interval< T > &range)
Calculate the triple product, as an interval.
LinearMotion< T > & operator=(const LinearMotion< T > &m)
Assignment operator.
Definition: LinearMotionArithmetic-inl.h:40
Eigen::Matrix< T, 3, 1 > Vector3
Typedef for a vector 3 return.
Definition: LinearMotionArithmetic.h:262
Polynomial< T, 2 > analyticCrossProductZAxis(const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b)
Calculate the Z axis of an analytic cross product as a Polynomial.
Definition: LinearMotionArithmetic-inl.h:659
bool operator!=(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:86
LinearMotion()
Constructor.
Definition: LinearMotionArithmetic-inl.h:25
Polynomial< T, 3 > analyticTripleProduct(const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b, const LinearMotionND< T, 3 > &c)
Calculate an analytic cross product as a Polynomial, as a polynomial whose value for t=0...
Definition: LinearMotionArithmetic-inl.h:674
Interval defines the concept of a mathematical interval and provides operations on it including arith...
Definition: IntervalArithmetic.h:34
Interval< T > operator*(const LinearMotion< T > &m) const
Standard arithmetic operators extended to interval groups.
Definition: LinearMotionArithmetic-inl.h:120
LinearMotion< T > operator+(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:92
Interval< T > toInterval() const
Convert from LinearMotion to an Interval.
Definition: LinearMotionArithmetic-inl.h:56
LinearMotionND<T, N> defines the concept of a group of linear motions and provides operations on them...
Definition: LinearMotionArithmetic.h:156
Polynomial< T, 1 > toPolynomial() const
Returns a linear expression (degree-1 polynomial) whose value for t=0..1 progresses from `start&#39; to `...
Definition: LinearMotionArithmetic-inl.h:62
Polynomial<T, 1> specializes the Polynomial class for degree 1 (linear polynomials) ...
Definition: Polynomial.h:117
T atTime(const T &t) const
Definition: LinearMotionArithmetic-inl.h:144
bool operator==(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:80
LinearMotion is (intentionally) a lot like Interval, but it deals with linear motion where all operan...
Definition: LinearMotionArithmetic.h:50
LinearMotion< T > & operator+=(const LinearMotion< T > &m)
Definition: LinearMotionArithmetic-inl.h:98
T m_start
The start point of the linear motion.
Definition: LinearMotionArithmetic.h:142