LinearMotionArithmetic.h

// 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