array3.h

Go to the documentation of this file.

#ifndef MATH_ARRAY3_H_INCLUDED
#define MATH_ARRAY3_H_INCLUDED
/*
 * Copyright 2011-2016 Universiteit Antwerpen
 *
 * Licensed under the EUPL, Version 1.1 or  as soon they will be approved by
 * the European Commission - subsequent versions of the EUPL (the "Licence");
 * You may not use this work except in compliance with the Licence.
 * You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl5
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the Licence is distributed on an "AS IS" basis,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the Licence for the specific language governing
 * permissions and limitations under the Licence.
 */
#include "constants.h"
#include "signum.h"

#include <array>
#include <cmath>
#include <ostream>

namespace std {


// ---------------------------------------------------------------------------------------------------
// Array's norm.
// ---------------------------------------------------------------------------------------------------

inline double Norm(const std::array<double, 3>& u)
{
        return std::sqrt( u[0] * u[0] + u[1] * u[1] + u[2] * u[2] );
}

inline double Norm2(const std::array<double, 3>& u)
{
        return u[0] * u[0] + u[1] * u[1] + u[2] * u[2];
}

// ---------------------------------------------------------------------------------------------------
// Unary, basic arithmetic.
// ---------------------------------------------------------------------------------------------------

inline std::array<double, 3>& operator+=(std::array<double, 3>& u, const std::array<double, 3>& v)
{
        u[0] += v[0];
        u[1] += v[1];
        u[2] += v[2];
        return u;
}

inline std::array<double, 3>& operator-=(std::array<double, 3>& u, const std::array<double, 3>& v)
{
        u[0] -= v[0];
        u[1] -= v[1];
        u[2] -= v[2];
        return u;
}

inline std::array<double, 3>& operator*=(std::array<double, 3>& u, double m)
{
        u[0] *= m;
        u[1] *= m;
        u[2] *= m;
        return u;
}

inline std::array<double, 3>& operator/=(std::array<double, 3>& u, double m)
{
        u[0] /= m;
        u[1] /= m;
        u[2] /= m;
        return u;
}

// ---------------------------------------------------------------------------------------------------
// Binary, basic arithmetic.
// ---------------------------------------------------------------------------------------------------

inline std::array<double, 3> operator+(const std::array<double, 3>& u, const std::array<double, 3>& v)
{
        return {{ u[0] + v[0], u[1] + v[1], u[2] + v[2] }};
}

inline std::array<double, 3> operator-(const std::array<double, 3>& u, const std::array<double, 3>& v)
{
        return {{ u[0] - v[0], u[1] - v[1], u[2] - v[2] }};
}

inline std::array<double, 3> operator* (const std::array<double, 3>& u, double m)
{
        return {{u[0]*m, u[1]*m, u[2]*m}};
}

inline std::array<double, 3> operator* (double m, const std::array<double, 3>& v)
{
        return v * m;
}

inline std::array<double, 3> operator/ (const std::array<double, 3>& u, double d)
{
        return {{u[0]/d, u[1]/d, u[2]/d}};
}

// ---------------------------------------------------------------------------------------------------
// Geometric ops.
// ---------------------------------------------------------------------------------------------------

inline std::array<double, 3> Normalize(const std::array<double, 3>& u)
{
        return u / Norm(u);
}


inline std::array<double, 3> CrossProduct(const std::array<double, 3>& u, const std::array<double, 3>& v)
{
        return {{ u[1]*v[2] - u[2]*v[1], u[2]*v[0] - u[0]*v[2], u[0]*v[1] - u[1]*v[0] }};
}

inline double InnerProduct(const std::array<double, 3>& u, const std::array<double, 3>& v)
{
        return (u[0] * v[0] + u[1] * v[1] + u[2] * v[2]);
}

inline std::array<double, 3> Orthogonalize(const std::array<double, 3>& u)
{
        return {{ u[1], -u[0], 0.0 }};
}

inline double Angle(const std::array<double, 3>& u, const std::array<double, 3>& v)
{
        return std::acos(std::min(1.0, std::max(-1.0, InnerProduct(u, v) / (Norm(u) * Norm(v)))));
}

inline bool IsSameDirection(const std::array<double, 3>& u, const std::array<double, 3>& v)
{
        return (Angle(u, v) < 0.5 * SimPT_Sim::Util::pi());
}

inline double SignedAngle(const std::array<double, 3>& u, const std::array<double, 3>& v)
{
        double cos_angle = InnerProduct(u, v) / (Norm(u) * Norm(v));
        double angle = 0.0;
        // check for computational inaccuracies
        if (cos_angle <= -1.0) {
                angle = SimPT_Sim::Util::pi();
        }
        else if (cos_angle >= 1.0) {
                angle = 0.0;
        } else {
                angle = acos(cos_angle) * SimPT_Sim::Util::signum(InnerProduct(Orthogonalize(u), v));
        }
        return angle;
}

// ---------------------------------------------------------------------------------------------------
// I/O.
// ---------------------------------------------------------------------------------------------------

inline std::ostream& operator<<(std::ostream& os, const std::array<double, 3>& v)
{
        os << "(" << v[0] << ", " << v[1] << ", " << v[2] << ")";
        return os;
}

} // namespace std

#endif // include guard