vector3D.h

Go to the documentation of this file.

#ifndef MECACELL_VECTOR3D_H
#define MECACELL_VECTOR3D_H
 
#include <array>
#include <cmath>
#include <functional>
#include <iostream>
#include "../utilities/config.hpp"
#include "../utilities/exportable.hpp"
#include "../utilities/utils.hpp"
#include "basis.hpp"
#include "quaternion.h"
#include "rotation.h"
 
namespace MecaCell {
 
class Vector3D {
 public:
    std::array<double, 3> coords;
 
    static const int dimension = 3;
    inline Vector3D(double a, double b, double c) : coords{{a, b, c}} {}
    //template <typename T>
    //Vector3D(const T &otherV) : coords{{otherV.x(), otherV.y(), otherV.z()}} {}
    inline Vector3D() : coords{{0, 0, 0}} {}
    inline explicit Vector3D(double a) : coords{{a, a, a}} {}
    inline explicit Vector3D(std::array<double, 3> c) : coords(c) {}
    inline Vector3D(const Vector3D &v) : coords(v.coords) {}
    inline Vector3D(Vector3D &&v) : coords(std::move(v.coords)) {}
 
    Vector3D &operator=(const Vector3D &other) {
        if (&other == this) return *this;
        coords = other.coords;
        return *this;
    }
 
    inline double dot(const Vector3D &v) const {
        return coords[0] * v.coords[0] + coords[1] * v.coords[1] + coords[2] * v.coords[2];
    }
 
    inline const Vector3D cross(const Vector3D &v) const {
        return Vector3D(coords[1] * v.coords[2] - coords[2] * v.coords[1],
                        coords[2] * v.coords[0] - coords[0] * v.coords[2],
                        coords[0] * v.coords[1] - coords[1] * v.coords[0]);
    }
 
    inline double &xRef() { return coords[0]; }
    inline double &yRef() { return coords[1]; }
    inline double &zRef() { return coords[2]; }
    inline double x() const { return coords[0]; }
    inline double y() const { return coords[1]; }
    inline double z() const { return coords[2]; }
 
    inline void setX(const double f) { coords[0] = f; }
    inline void setY(const double f) { coords[1] = f; }
    inline void setZ(const double f) { coords[2] = f; }
 
    void random() {
        std::normal_distribution<double> nDist(0.0, 1.0);
        coords = {{nDist(Config::globalRand()), nDist(Config::globalRand()),
                   nDist(Config::globalRand())}};
        normalize();
    }
 
    static inline Vector3D randomUnit() {
        Vector3D v;
        v.random();
        return v;
    }
 
    Vector3D deltaDirection(double amount) {
        std::normal_distribution<double> nDist(0.0, amount);
        return Vector3D(coords[0] + nDist(Config::globalRand()),
                        coords[1] + nDist(Config::globalRand()),
                        coords[2] + nDist(Config::globalRand()))
            .normalized();
    }
 
    static inline Vector3D zero() { return Vector3D(0.0, 0.0, 0.0); }
 
    inline bool isZero() const {
        return (coords[0] == 0.0 && coords[1] == 0.0 && coords[2] == 0.0);
    }
 
    inline Vector3D &operator*=(double d) {
        coords[0] *= d;
        coords[1] *= d;
        coords[2] *= d;
        return *this;
    };
 
    inline Vector3D &operator/=(double d) {
        coords[0] /= d;
        coords[1] /= d;
        coords[2] /= d;
        return *this;
    };
 
    inline Vector3D &operator+=(const Vector3D &v) {
        coords[0] += v.coords[0];
        coords[1] += v.coords[1];
        coords[2] += v.coords[2];
        return *this;
    }
 
    inline Vector3D &operator-=(const Vector3D &v) {
        coords[0] -= v.coords[0];
        coords[1] -= v.coords[1];
        coords[2] -= v.coords[2];
        return *this;
    }
 
    friend inline bool operator==(const Vector3D &v1, const Vector3D &v2);
    friend inline bool operator!=(const Vector3D &v1, const Vector3D &v2);
    friend inline Vector3D operator+(const Vector3D &v1, const Vector3D &v2);
    friend inline Vector3D operator+(const Vector3D &v1, double f);
    friend inline Vector3D operator+(double f, const Vector3D &v1);
    friend inline Vector3D operator-(const Vector3D &v1, const Vector3D &v2);
    friend inline Vector3D operator-(const Vector3D &v1, double f);
    friend inline Vector3D operator-(double f, const Vector3D &v1);
    friend inline Vector3D operator*(double factor, const Vector3D &vector);
    friend inline Vector3D operator*(const Vector3D &vector, double factor);
    friend inline Vector3D operator*(const Vector3D &v1, const Vector3D &v2);
    friend inline Vector3D operator-(const Vector3D &vector);
    friend inline Vector3D operator/(const Vector3D &vector, double divisor);
    friend inline ostream &operator<<(ostream &out, const Vector3D &v);
 
    double length() const {
        return sqrt(coords[0] * coords[0] + coords[1] * coords[1] + coords[2] * coords[2]);
    }
    double sqlength() const {
        return coords[0] * coords[0] + coords[1] * coords[1] + coords[2] * coords[2];
    }
 
    Vector3D rotated(double angle, const Vector3D &vec) const {
        double halfangle = angle * 0.5;
        Vector3D v = vec * sin(halfangle);
        Vector3D vcV = 2.0 * v.cross(*this);
        return *this + cos(halfangle) * vcV + v.cross(vcV);
    }
 
    Vector3D rotated(const Rotation<Vector3D> &r) const {
        double halfangle = r.teta * 0.5;
        Vector3D v = r.n * sin(halfangle);
        Vector3D vcV = 2.0 * v.cross(*this);
        return *this + cos(halfangle) * vcV + v.cross(vcV);
    }
 
    static void addAsAngularVelocity(const Vector3D &v, Rotation<Vector3D> &r) {
        double dTeta = v.length();
        Vector3D n0(0, 1, 0);
        if (dTeta > 0) {
            n0 = v / dTeta;
        }
        r = addRotations(r, Rotation<Vector3D>(n0, dTeta));
    }
 
    static Rotation<Vector3D> getRotation(const Vector3D &v0, const Vector3D &v1) {
        Rotation<Vector3D> res;
        res.teta = acos(min<double>(1.0, max<double>(-1.0, v0.dot(v1))));
        Vector3D cross = v0.cross(v1);
        if (cross.sqlength() == 0) {
            cross = Vector3D(0, 1, 0);
        }
        res.n = cross.normalized();
        return res;
    }
 
    static double rayCast(const Vector3D &o, const Vector3D &n, const Vector3D &p,
                          const Vector3D &r) {
        double nr = n.dot(r);
        return (nr == 0) ? 0 : n.dot(o - p) / nr;
    }
 
    static Vector3D getProjectionOnPlane(const Vector3D &o, const Vector3D &n,
                                         const Vector3D &p) {
        return p - (n.dot(p - o) * n);
    }
 
    static Rotation<Vector3D> rotateRotation(const Rotation<Vector3D> &start,
                                             const Rotation<Vector3D> &offset) {
        return Rotation<Vector3D>(start.n.rotated(offset), start.teta);
    }
    static Rotation<Vector3D> addRotations(const Rotation<Vector3D> &R0,
                                           const Rotation<Vector3D> &R1) {
        auto q2 = Quaternion<Vector3D>(R1.teta, R1.n) * Quaternion<Vector3D>(R0.teta, R0.n);
        q2.normalize();
        return q2.toAxisAngle();
    }
 
    static Rotation<Vector3D> getRotation(const Vector3D &X0, const Vector3D &Y0,
                                          const Vector3D &X1, const Vector3D &Y1) {
        Quaternion<Vector3D> q0(X0.normalized(), X1.normalized());
        Vector3D Ytmp = q0 * Y0;
        Ytmp.normalize();
        auto qres = Quaternion<Vector3D>(Ytmp, Y1.normalized()) * q0;
        qres.normalize();
        return qres.toAxisAngle();
    }
    static Rotation<Vector3D> getRotation(const Basis<Vector3D> &b0,
                                          const Basis<Vector3D> &b1) {
        return getRotation(b0.X, b0.Y, b1.X, b1.Y);
    }
 
    static Vector3D getProjection(const Vector3D &A, const Vector3D &B, const Vector3D &P) {
        Vector3D a = B - A;
        return A + a * (a.dot(P - A) / a.sqlength());
    }
 
    void normalize() { *this /= length(); }
 
    inline Vector3D normalized() const {
        double l = length();
        return l > 0 ? *this / l : zero();
    }
 
    std::string toString() const {
        std::stringstream s;
        s.precision(500);
        s << "(" << coords[0] << " , " << coords[1] << ", " << coords[2] << ")";
        return s.str();
    }
    inline int getHash(const int a, const int b) const {
        unsigned int A = (unsigned int)(a >= 0 ? 2 * a : -2 * a - 1);
        unsigned int B = (unsigned int)(b >= 0 ? 2 * b : -2 * b - 1);
        int C = ((A >= B ? A * A + A + B : A + B * B) / 2);
        return (a < 0 && b < 0) || (a >= 0 && b >= 0) ? C : -C - 1;
    }
 
    int getHash() const {
        return getHash(
            static_cast<int>(floor(coords[0])),
            getHash(static_cast<int>(floor(coords[1])), static_cast<int>(floor(coords[2]))));
    }
 
    inline void iterateTo(const Vector3D &v,
                          const std::function<void(const Vector3D &)> &fun,
                          const double inc = 1) const {
        Vector3D base(floor(min(coords[0], v.coords[0])), floor(min(coords[1], v.coords[1])),
                      floor(min(coords[2], v.coords[2])));
        Vector3D nxt(ceil(max(coords[0], v.coords[0])), ceil(max(coords[1], v.coords[1])),
                     ceil(max(coords[2], v.coords[2])));
        for (auto i = base.x(); i <= nxt.x(); i += inc) {
            for (auto j = base.y(); j <= nxt.y(); j += inc) {
                for (auto k = base.z(); k <= nxt.z(); k += inc) {
                    fun(Vector3D(i, j, k));
                }
            }
        }
    }
 
    Vector3D ortho() const {
        if (coords[1] == 0 && coords[0] == 0) {
            return Vector3D(0.0, 1.0, 0.0);
        }
        return Vector3D(-coords[1], coords[0], 0.0);
    }
 
    Vector3D ortho(const Vector3D &v) const {
        if ((v - *this).sqlength() > 0.000000000001) {
            Vector3D res = cross(v);
            if (res.sqlength() > 0.0000000000001) return res;
        }
        return ortho();
    }
 
    EXPORTABLE(Vector3D, KV(coords));
};
 
inline bool operator==(const Vector3D &v1, const Vector3D &v2) {
    return v1.coords[0] == v2.coords[0] && v1.coords[1] == v2.coords[1] &&
           v1.coords[2] == v2.coords[2];
}
inline bool operator!=(const Vector3D &v1, const Vector3D &v2) {
    return v1.coords[0] != v2.coords[0] && v1.coords[1] != v2.coords[1] &&
           v1.coords[2] != v2.coords[2];
}
 
inline Vector3D operator+(const Vector3D &v1, const Vector3D &v2) {
    return Vector3D(v1.coords[0] + v2.coords[0], v1.coords[1] + v2.coords[1],
                    v1.coords[2] + v2.coords[2]);
}
inline Vector3D operator+(const double f, const Vector3D &v) {
    return Vector3D(v.coords[0] + f, v.coords[1] + f, v.coords[2] + f);
}
inline Vector3D operator+(const Vector3D &v, const double f) {
    return Vector3D(v.coords[0] + f, v.coords[1] + f, v.coords[2] + f);
}
 
inline Vector3D operator-(const Vector3D &v1, const Vector3D &v2) {
    return Vector3D(v1.coords[0] - v2.coords[0], v1.coords[1] - v2.coords[1],
                    v1.coords[2] - v2.coords[2]);
}
inline Vector3D operator*(const Vector3D &v1, const Vector3D &v2) {
    return Vector3D(v1.coords[0] * v2.coords[0], v1.coords[1] * v2.coords[1],
                    v1.coords[2] * v2.coords[2]);
}
 
inline Vector3D operator*(const double f, const Vector3D &v) {
    return Vector3D(v.coords[0] * f, v.coords[1] * f, v.coords[2] * f);
}
inline Vector3D operator*(const Vector3D &v, const double f) {
    return Vector3D(v.coords[0] * f, v.coords[1] * f, v.coords[2] * f);
}
 
inline Vector3D operator-(const double f, const Vector3D &v) {
    return Vector3D(v.coords[0] - f, v.coords[1] - f, v.coords[2] - f);
}
inline Vector3D operator-(const Vector3D &v, const double f) {
    return Vector3D(v.coords[0] - f, v.coords[1] - f, v.coords[2] - f);
}
 
inline Vector3D operator-(const Vector3D &v) {
    return Vector3D(-v.coords[0], -v.coords[1], -v.coords[2]);
}
 
inline Vector3D operator/(const Vector3D &v, const double f) {
    return Vector3D(v.coords[0] / f, v.coords[1] / f, v.coords[2] / f);
}
inline ostream &operator<<(ostream &out, const Vector3D &v) {
    out << "(" << v.coords[0] << ", " << v.coords[1] << ", " << v.coords[2] << ")";
    return out;
}
}  // namespace MecaCell
namespace std {
template <> struct hash<MecaCell::Vector3D> {
    int operator()(const MecaCell::Vector3D &v) const { return v.getHash(); }
};
}  // namespace std
#endif  // VECTOR3D_H