LinearMotionArithmetic.h
Go to the documentation of this file.
// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SURGSIM_MATH_LINEARMOTIONARITHMETIC_H
#define SURGSIM_MATH_LINEARMOTIONARITHMETIC_H
#include <array>
#include <iostream>
#include <Eigen/Core>
#include "SurgSim/Math/IntervalArithmetic.h"
#include "SurgSim/Math/PolynomialValues.h"
#include "SurgSim/Math/Vector.h"
namespace SurgSim
{
namespace Math
{
template <typename T>
class LinearMotion
{
public:
LinearMotion();
LinearMotion(const T& start, const T& end);
LinearMotion(const LinearMotion<T>& m);
LinearMotion(LinearMotion<T>&& m);
LinearMotion<T>& operator=(const LinearMotion<T>& m);
LinearMotion<T>& operator=(LinearMotion<T>&& m);
Interval<T> toInterval() const;
Polynomial<T, 1> toPolynomial() const;
bool containsZero() const;
bool isApprox(const LinearMotion<T>& i, const T& epsilon) const;
bool operator==(const LinearMotion<T>& m) const;
bool operator!=(const LinearMotion<T>& m) const;
LinearMotion<T> operator+(const LinearMotion<T>& m) const;
LinearMotion<T>& operator+=(const LinearMotion<T>& m);
LinearMotion<T> operator-(const LinearMotion<T>& m) const;
LinearMotion<T>& operator-=(const LinearMotion<T>& m);
Interval<T> operator*(const LinearMotion<T>& m) const;
Interval<T> operator/(const LinearMotion<T>& m) const;
T getStart() const;
T getEnd() const;
T atTime(const T& t) const;
LinearMotion<T> firstHalf() const;
LinearMotion<T> secondHalf() const;
private:
T m_start;
T m_end;
};
template <class T, int N>
class LinearMotionND
{
static_assert(N > 0, "LinearMotion must have dimensionality > 0.");
public:
LinearMotionND();
explicit LinearMotionND(const std::array<LinearMotion<T>, N>& x);
LinearMotionND(const LinearMotionND<T, N>& motion);
LinearMotionND(LinearMotionND<T, N>&& motion);
LinearMotionND(const std::array<T, N>& a, const std::array<T, N>& b);
LinearMotionND<T, N>& operator=(const LinearMotionND<T, N>& motion);
LinearMotionND<T, N>& operator=(LinearMotionND<T, N>&& motion);
IntervalND<T, N> toInterval() const;
bool isApprox(const LinearMotionND<T, N>& motion, const T& epsilon) const;
bool operator==(const LinearMotionND<T, N>& motion) const;
bool operator!=(const LinearMotionND<T, N>& motion) const;
LinearMotionND<T, N> operator+(const LinearMotionND<T, N>& m) const;
LinearMotionND<T, N>& operator+=(const LinearMotionND<T, N>& m);
LinearMotionND<T, N> operator-(const LinearMotionND<T, N>& m) const;
LinearMotionND<T, N>& operator-=(const LinearMotionND<T, N>& m);
IntervalND<T, N> operator*(const LinearMotionND<T, N>& m) const;
IntervalND<T, N> operator/(const LinearMotionND<T, N>& m) const;
Interval<T> dotProduct(const LinearMotionND<T, N>& motion) const;
const LinearMotion<T>& getAxis(int i) const;
void getStart(std::array<T, N>* start) const;
void getEnd(std::array<T, N>* end) const;
LinearMotionND<T, N> firstHalf() const;
LinearMotionND<T, N> secondHalf() const;
private:
std::array<LinearMotion<T>, N> m_motion;
};
template <class T>
class LinearMotionND<T, 3>
{
public:
typedef Eigen::Matrix<T, 3, 1> Vector3;
LinearMotionND();
explicit LinearMotionND(const std::array<LinearMotion<T>, 3>& x);
LinearMotionND(const LinearMotion<T>& a, const LinearMotion<T>& b, const LinearMotion<T>& c);
LinearMotionND(const LinearMotionND<T, 3>& motion);
LinearMotionND(LinearMotionND<T, 3>&& motion);
LinearMotionND(const std::array<T, 3>& start, const std::array<T, 3>& end);
LinearMotionND(const Vector3& start, const Vector3& end);
LinearMotionND<T, 3>& operator=(const LinearMotionND<T, 3>& motion);
LinearMotionND<T, 3>& operator=(LinearMotionND<T, 3>&& motion);
IntervalND<T, 3> toInterval() const;
bool isApprox(const LinearMotionND<T, 3>& motion, const T& epsilon) const;
bool operator==(const LinearMotionND<T, 3>& motion) const;
bool operator!=(const LinearMotionND<T, 3>& motion) const;
LinearMotionND<T, 3> operator+(const LinearMotionND<T, 3>& m) const;
LinearMotionND<T, 3>& operator+=(const LinearMotionND<T, 3>& m);
LinearMotionND<T, 3> operator-(const LinearMotionND<T, 3>& m) const;
LinearMotionND<T, 3>& operator-=(const LinearMotionND<T, 3>& m);
IntervalND<T, 3> operator*(const LinearMotionND<T, 3>& m) const;
IntervalND<T, 3> operator/(const LinearMotionND<T, 3>& m) const;
Interval<T> dotProduct(const LinearMotionND<T, 3>& motion, const Interval<T>& range) const;
IntervalND<T, 3> crossProduct(const LinearMotionND<T, 3>& motion, const Interval<T>& range) const;
Interval<T> magnitudeSquared(const Interval<T>& range) const;
Interval<T> magnitude(const Interval<T>& range) const;
const LinearMotion<T>& getAxis(int i) const;
void getStart(std::array<T, 3>* start) const;
void getEnd(std::array<T, 3>* end) const;
Vector3 getStart() const;
Vector3 getEnd() const;
Vector3 atTime(const T& t) const;
LinearMotionND<T, 3> firstHalf() const;
LinearMotionND<T, 3> secondHalf() const;
private:
std::array<LinearMotion<T>, 3> m_motion;
};
// Linear motion utilities
template <typename T>
std::ostream& operator<<(std::ostream& o, const LinearMotion<T>& motion);
// Linear motion ND utilities
template <typename T, int N>
std::ostream& operator<<(std::ostream& o, const LinearMotionND<T, N>& motion);
// Linear motion 3D utilities
template <class T>
Polynomial<T, 2> analyticDotProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b);
template <class T, int A>
Polynomial<T, 2> analyticCrossProductAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b);
template <class T>
Polynomial<T, 2> analyticCrossProductXAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b);
template <class T>
Polynomial<T, 2> analyticCrossProductYAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b);
template <class T>
Polynomial<T, 2> analyticCrossProductZAxis(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b);
template <class T>
void analyticCrossProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b,
Polynomial<T, 2>* resultXAxis, Polynomial<T, 2>* resultYAxis, Polynomial<T, 2>* resultZAxis);
template <class T>
Polynomial<T, 3> analyticTripleProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b,
const LinearMotionND<T, 3>& c);
template <class T>
Interval<T> tripleProduct(const LinearMotionND<T, 3>& a, const LinearMotionND<T, 3>& b,
const LinearMotionND<T, 3>& c, const Interval<T>& range);
template <class T>
Polynomial<T, 2> analyticMagnitudeSquared(const LinearMotionND<T, 3>& motion);
}; // Math
}; // SurgSim
#include "SurgSim/Math/LinearMotionArithmetic-inl.h"
#endif // SURGSIM_MATH_LINEARMOTIONARITHMETIC_H
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
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::isApprox
bool isApprox(const LinearMotion< T > &i, const T &epsilon) const
Definition: LinearMotionArithmetic-inl.h:74
SurgSim::Math::LinearMotion::getEnd
T getEnd() const
Definition: LinearMotionArithmetic-inl.h:138
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::IntervalND< T, 3 >
IntervalND<T,3> defines the concept of a group of mathematical intervals specialized to 3 intervals a...
Definition: IntervalArithmetic.h:307
SurgSim::Math::LinearMotionND< T, 3 >
LinearMotionND<T, 3> specializes the LinearMotionND<T, N> class for 3 dimensions. ...
Definition: LinearMotionArithmetic.h:258
PolynomialValues.h
SurgSim::Math::Polynomial< T, 3 >
Polynomial<T, 3> specializes the Polynomial class for degree 3 (cubic polynomials) ...
Definition: Polynomial.h:255
SurgSim::Math::IntervalND
IntervalND defines the concept of a group of mathematical intervals and provides operations on them i...
Definition: IntervalArithmetic.h:199
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::LinearMotionND::m_motion
std::array< LinearMotion< T >, N > m_motion
The N dimensional group of linear motions.
Definition: LinearMotionArithmetic.h:251
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:183
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::LinearMotion::operator-=
LinearMotion< T > & operator-=(const LinearMotion< T > &m)
Definition: LinearMotionArithmetic-inl.h:112
SurgSim::Math::LinearMotion::secondHalf
LinearMotion< T > secondHalf() const
Definition: LinearMotionArithmetic-inl.h:156
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::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::LinearMotion::getStart
T getStart() const
Definition: LinearMotionArithmetic-inl.h:132
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::LinearMotion::m_end
T m_end
The end point of the linear motion.
Definition: LinearMotionArithmetic.h:145
SurgSim::Math::LinearMotion::operator-
LinearMotion< T > operator-(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:106
SurgSim::Math::LinearMotion::containsZero
bool containsZero() const
Definition: LinearMotionArithmetic-inl.h:68
Vector.h
Definitions of small fixed-size vector types.
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::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::LinearMotion::operator=
LinearMotion< T > & operator=(const LinearMotion< T > &m)
Assignment operator.
Definition: LinearMotionArithmetic-inl.h:40
LinearMotionArithmetic-inl.h
SurgSim::Math::LinearMotionND< T, 3 >::Vector3
Eigen::Matrix< T, 3, 1 > Vector3
Typedef for a vector 3 return.
Definition: LinearMotionArithmetic.h:262
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::operator!=
bool operator!=(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:86
SurgSim::Math::LinearMotion::LinearMotion
LinearMotion()
Constructor.
Definition: LinearMotionArithmetic-inl.h:25
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::Interval
Interval defines the concept of a mathematical interval and provides operations on it including arith...
Definition: IntervalArithmetic.h:34
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::LinearMotion::operator+
LinearMotion< T > operator+(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:92
SurgSim::Math::LinearMotion::toInterval
Interval< T > toInterval() const
Convert from LinearMotion to an Interval.
Definition: LinearMotionArithmetic-inl.h:56
SurgSim::Math::LinearMotionND
LinearMotionND<T, N> defines the concept of a group of linear motions and provides operations on them...
Definition: LinearMotionArithmetic.h:156
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&#39; to `...
Definition: LinearMotionArithmetic-inl.h:62
SurgSim::Math::Polynomial< T, 1 >
Polynomial<T, 1> specializes the Polynomial class for degree 1 (linear polynomials) ...
Definition: Polynomial.h:117
SurgSim::Math::LinearMotion::atTime
T atTime(const T &t) const
Definition: LinearMotionArithmetic-inl.h:144
SurgSim::Math::LinearMotion::operator==
bool operator==(const LinearMotion< T > &m) const
Definition: LinearMotionArithmetic-inl.h:80
IntervalArithmetic.h
SurgSim::Math::LinearMotion
LinearMotion is (intentionally) a lot like Interval, but it deals with linear motion where all operan...
Definition: LinearMotionArithmetic.h:50
SurgSim::Math::LinearMotion::operator+=
LinearMotion< T > & operator+=(const LinearMotion< T > &m)
Definition: LinearMotionArithmetic-inl.h:98
SurgSim::Math::LinearMotion::m_start
T m_start
The start point of the linear motion.
Definition: LinearMotionArithmetic.h:142