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
SurgSim::Math::LinearMotion::operator!=
bool operator!=(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:86
SurgSim::Math::LinearMotion::operator=
LinearMotion< T > & operator=(const LinearMotion< T > &m)
Assignment operator.
Definition: LinearMotionArithmetic-inl.h:40
SurgSim::Math::IntervalND< T, 3 >
IntervalND<T,3> defines the concept of a group of mathematical intervals specialized to 3 intervals a...
Definition: IntervalArithmetic.h:308
SurgSim::Math::LinearMotion::toPolynomial
Polynomial< T, 1 > toPolynomial() const
Returns a linear expression (degree-1 polynomial) whose value for t=0..1 progresses from ‘start’ to ‘...
Definition: LinearMotionArithmetic-inl.h:62
SurgSim::Math::analyticMagnitudeSquared
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
SurgSim::Math::LinearMotion
LinearMotion is (intentionally) a lot like Interval, but it deals with linear motion where all operan...
Definition: LinearMotionArithmetic.h:51
SurgSim::Math::LinearMotion::LinearMotion
LinearMotion()
Constructor.
Definition: LinearMotionArithmetic-inl.h:25
Vector.h
Definitions of small fixed-size vector types.
SurgSim::Math::LinearMotionND::getEnd
void getEnd(std::array< T, N > *end) const
Definition: LinearMotionArithmetic-inl.h:325
SurgSim::Math::LinearMotionND::m_motion
std::array< LinearMotion< T >, N > m_motion
The N dimensional group of linear motions.
Definition: LinearMotionArithmetic.h:251
SurgSim::Math::LinearMotionND< T, 3 >
LinearMotionND<T, 3> specializes the LinearMotionND<T, N> class for 3 dimensions.
Definition: LinearMotionArithmetic.h:259
SurgSim::Math::tripleProduct
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.
SurgSim::Math::LinearMotionND::operator+=
LinearMotionND< T, N > & operator+=(const LinearMotionND< T, N > &m)
Definition: LinearMotionArithmetic-inl.h:258
SurgSim::Math::LinearMotion::m_end
T m_end
The end point of the linear motion.
Definition: LinearMotionArithmetic.h:145
SurgSim::Math::LinearMotionND::operator*
IntervalND< T, N > operator*(const LinearMotionND< T, N > &m) const
Standard arithmetic operators extended to interval groups.
Definition: LinearMotionArithmetic-inl.h:286
SurgSim::Math::LinearMotion::containsZero
bool containsZero() const
Definition: LinearMotionArithmetic-inl.h:68
IntervalArithmetic.h
SurgSim::Math::LinearMotionND
LinearMotionND<T, N> defines the concept of a group of linear motions and provides operations on them...
Definition: LinearMotionArithmetic.h:157
SurgSim::Math::LinearMotion::operator*
Interval< T > operator*(const LinearMotion< T > &m) const
Standard arithmetic operators extended to interval groups.
Definition: LinearMotionArithmetic-inl.h:120
SurgSim::Math::LinearMotionND::operator-
LinearMotionND< T, N > operator-(const LinearMotionND< T, N > &m) const
Definition: LinearMotionArithmetic-inl.h:268
SurgSim::Math::analyticCrossProductXAxis
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
SurgSim::Math::LinearMotionND::operator==
bool operator==(const LinearMotionND< T, N > &motion) const
Definition: LinearMotionArithmetic-inl.h:238
SurgSim
Definition: CompoundShapeToGraphics.cpp:30
SurgSim::Math::LinearMotion::secondHalf
LinearMotion< T > secondHalf() const
Definition: LinearMotionArithmetic-inl.h:156
SurgSim::Math::LinearMotion::operator-
LinearMotion< T > operator-(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:106
SurgSim::Math::LinearMotionND::getAxis
const LinearMotion< T > & getAxis(int i) const
Definition: LinearMotionArithmetic-inl.h:309
SurgSim::Math::LinearMotionND::operator/
IntervalND< T, N > operator/(const LinearMotionND< T, N > &m) const
Standard arithmetic operators extended to interval groups.
Definition: LinearMotionArithmetic-inl.h:292
LinearMotionArithmetic-inl.h
SurgSim::Math::analyticCrossProduct
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
SurgSim::Math::LinearMotionND::getStart
void getStart(std::array< T, N > *start) const
Definition: LinearMotionArithmetic-inl.h:316
SurgSim::Math::Interval
Interval defines the concept of a mathematical interval and provides operations on it including arith...
Definition: IntervalArithmetic.h:35
SurgSim::Math::analyticTripleProduct
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
SurgSim::Math::operator<<
std::ostream & operator<<(std::ostream &o, const Interval< T > &interval)
Write a textual version of the interval to an output stream.
Definition: IntervalArithmetic-inl.h:839
SurgSim::Math::LinearMotionND::operator=
LinearMotionND< T, N > & operator=(const LinearMotionND< T, N > &motion)
Assignment operator.
Definition: LinearMotionArithmetic-inl.h:195
SurgSim::Math::LinearMotionND::LinearMotionND
LinearMotionND()
Constructor.
Definition: LinearMotionArithmetic-inl.h:163
SurgSim::Math::LinearMotion::operator/
Interval< T > operator/(const LinearMotion< T > &m) const
Standard arithmetic operators extended to interval groups.
Definition: LinearMotionArithmetic-inl.h:126
SurgSim::Math::LinearMotionND::toInterval
IntervalND< T, N > toInterval() const
Convert from LinearMotion to an Interval.
Definition: LinearMotionArithmetic-inl.h:213
SurgSim::Math::LinearMotionND::firstHalf
LinearMotionND< T, N > firstHalf() const
Definition: LinearMotionArithmetic-inl.h:334
SurgSim::Math::analyticCrossProductAxis
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
SurgSim::Math::Polynomial< T, 1 >
Polynomial<T, 1> specializes the Polynomial class for degree 1 (linear polynomials)
Definition: Polynomial.h:118
SurgSim::Math::LinearMotionND::secondHalf
LinearMotionND< T, N > secondHalf() const
Definition: LinearMotionArithmetic-inl.h:345
SurgSim::Math::LinearMotionND::operator-=
LinearMotionND< T, N > & operator-=(const LinearMotionND< T, N > &m)
Definition: LinearMotionArithmetic-inl.h:276
SurgSim::Math::IntervalND
IntervalND defines the concept of a group of mathematical intervals and provides operations on them i...
Definition: IntervalArithmetic.h:200
SurgSim::Math::analyticDotProduct
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
SurgSim::Math::LinearMotionND< T, 3 >::m_motion
std::array< LinearMotion< T >, 3 > m_motion
The 3 dimensional group of linear motions.
Definition: LinearMotionArithmetic.h:387
SurgSim::Math::LinearMotionND::isApprox
bool isApprox(const LinearMotionND< T, N > &motion, const T &epsilon) const
Definition: LinearMotionArithmetic-inl.h:225
SurgSim::Math::LinearMotionND::dotProduct
Interval< T > dotProduct(const LinearMotionND< T, N > &motion) const
Definition: LinearMotionArithmetic-inl.h:298
SurgSim::Math::Polynomial< T, 3 >
Polynomial<T, 3> specializes the Polynomial class for degree 3 (cubic polynomials)
Definition: Polynomial.h:256
SurgSim::Math::LinearMotionND::operator+
LinearMotionND< T, N > operator+(const LinearMotionND< T, N > &m) const
Definition: LinearMotionArithmetic-inl.h:250
SurgSim::Math::analyticCrossProductZAxis
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
SurgSim::Math::LinearMotion::isApprox
bool isApprox(const LinearMotion< T > &i, const T &epsilon) const
Definition: LinearMotionArithmetic-inl.h:74
SurgSim::Math::LinearMotionND< T, 3 >::Vector3
Eigen::Matrix< T, 3, 1 > Vector3
Typedef for a vector 3 return.
Definition: LinearMotionArithmetic.h:262
SurgSim::Math::LinearMotionND::operator!=
bool operator!=(const LinearMotionND< T, N > &motion) const
Definition: LinearMotionArithmetic-inl.h:244
SurgSim::Math::LinearMotion::m_start
T m_start
The start point of the linear motion.
Definition: LinearMotionArithmetic.h:142
SurgSim::Math::LinearMotion::operator==
bool operator==(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:80
SurgSim::Math::LinearMotion::operator+
LinearMotion< T > operator+(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:92
SurgSim::Math::LinearMotion::getStart
T getStart() const
Definition: LinearMotionArithmetic-inl.h:132
PolynomialValues.h
SurgSim::Math::LinearMotion::operator+=
LinearMotion< T > & operator+=(const LinearMotion< T > &m)
Definition: LinearMotionArithmetic-inl.h:98
SurgSim::Math::LinearMotion::toInterval
Interval< T > toInterval() const
Convert from LinearMotion to an Interval.
Definition: LinearMotionArithmetic-inl.h:56
SurgSim::Math::LinearMotion::operator-=
LinearMotion< T > & operator-=(const LinearMotion< T > &m)
Definition: LinearMotionArithmetic-inl.h:112
SurgSim::Math::analyticCrossProductYAxis
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
SurgSim::Math::LinearMotion::firstHalf
LinearMotion< T > firstHalf() const
Definition: LinearMotionArithmetic-inl.h:150
SurgSim::Math::Polynomial< T, 2 >
Polynomial<T, 2> specializes the Polynomial class for degree 2 (quadratic polynomials)
Definition: Polynomial.h:184
SurgSim::Math::LinearMotion::getEnd
T getEnd() const
Definition: LinearMotionArithmetic-inl.h:138
SurgSim::Math::LinearMotion::atTime
T atTime(const T &t) const
Definition: LinearMotionArithmetic-inl.h:144