point.h

//  Copyright (c) 2019 Prashant K. Jha
//  Copyright (c) 2019 Patrick Diehl
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 
#ifndef UTIL_POINT_H
#define UTIL_POINT_H
 
#include "matrixBlaze.h"
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>
#include <stdlib.h>
 
namespace util {
 
struct Point3 {
 
  double d_x;
 
  double d_y;
 
  double d_z;
 
  Point3() : d_x(0.), d_y(0.), d_z(0.){};
 
  template <class T>
  Point3(T x, T y, T z) : d_x(x), d_y(y), d_z(z){};
 
  template <class T>
  explicit Point3(T x[3]) : d_x(x[0]), d_y(x[1]), d_z(x[2]){};
 
  explicit Point3(const std::vector<double> &p) {
 
    if (p.empty())
      return;
    else if (p.size() == 1)
      d_x = p[0];
    else if (p.size() == 2) {
      d_x = p[0];
      d_y = p[1];
    } else if (p.size() == 3) {
      d_x = p[0];
      d_y = p[1];
      d_z = p[2];
    }
  }
 
  Point3(const Point3 &p) : d_x(p.d_x), d_y(p.d_y), d_z(p.d_z) {};
 
  std::string printStr(int nt = 0, int lvl = 0) const {
 
    std::string tabS = "";
    for (int i = 0; i < nt; i++)
      tabS += "\t";
 
    std::ostringstream oss;
    oss << tabS << "(" << d_x << ", " << d_y << ", " << d_z << ")";
 
    return oss.str();
  }
 
 
  void print(int nt = 0, int lvl = 0) const { std::cout << printStr(nt, lvl); }
 
  double length() const { return std::sqrt(d_x * d_x + d_y * d_y + d_z * d_z); }
 
  double lengthSq() const { return d_x * d_x + d_y * d_y + d_z *
  d_z; }
 
  Point3 unit() {
    auto l = this->length();
    if (l < 1.0E-10)
      return {};
    else
      return {d_x/l, d_y/l, d_z/l};
  }
 
  Point3 unit() const {
    auto l = this->length();
    if (l < 1.0E-10)
      return {};
    else
      return {d_x/l, d_y/l, d_z/l};
  }
 
  double dot(const Point3 &b) const { return d_x * b.d_x + d_y * b.d_y + d_z * b
  .d_z; }
 
  double dist(const Point3 &b) const {
    return std::sqrt((d_x - b.d_x) * (d_x - b.d_x) +
                     (d_y - b.d_y) * (d_y - b.d_y) +
                     (d_z - b.d_z) * (d_z - b.d_z));
  }
 
  Point3 cross(const Point3 &b) const {
    return {-d_z * b.d_y + d_y * b.d_z,
            d_z * b.d_x - d_x * b.d_z,
            -d_y * b.d_x + d_x * b.d_y};
  }
 
  Point3 project(const Point3 &b, bool is_unit = false) const {
    auto l_sq = (is_unit ? 1. : this->length() * this->length());
    auto dot = this->dot(b);
    return {dot * d_x / l_sq, dot * d_y / l_sq, dot * d_z / l_sq};
  }
 
  Point3 projectNormal(const Point3 &b, bool is_unit = false) const {
    auto l_sq = (is_unit ? 1. : this->length() * this->length());
    auto dot = this->dot(b);
    return b - Point3(dot * d_x / l_sq, dot * d_y / l_sq, dot * d_z / l_sq);
  }
 
 
  double angle(Point3 b) const {
 
    auto ahat = this->unit();
    auto bhat = b.unit();
    return std::acos(ahat.dot(bhat));
  }
 
  util::Matrix33 toMatrix() {
 
    util::Matrix33 tmp = util::Matrix33();
 
    tmp(0, 0) = this->d_x * this->d_x;
    tmp(0, 1) = this->d_x * this->d_y;
    tmp(0, 2) = this->d_x * this->d_z;
 
    tmp(1, 0) = this->d_y * this->d_x;
    tmp(1, 1) = this->d_y * this->d_y;
    tmp(1, 2) = this->d_y * this->d_z;
 
    tmp(2, 0) = this->d_z * this->d_x;
    tmp(2, 1) = this->d_z * this->d_y;
    tmp(2, 2) = this->d_z * this->d_z;
 
    return tmp;
  }
 
  util::Matrix33 toMatrix(const util::Point3 b) {
 
    util::Matrix33 tmp = util::Matrix33();
 
    tmp(0, 0) = this->d_x * b.d_x;
    tmp(0, 1) = this->d_x * b.d_y;
    tmp(0, 2) = this->d_x * b.d_z;
 
    tmp(1, 0) = this->d_y * b.d_x;
    tmp(1, 1) = this->d_y * b.d_y;
    tmp(1, 2) = this->d_y * b.d_z;
 
    tmp(2, 0) = this->d_z * b.d_x;
    tmp(2, 1) = this->d_z * b.d_y;
    tmp(2, 2) = this->d_z * b.d_z;
 
    return tmp;
  }
 
  util::Vector3 toVector() {
 
    return util::Vector3{this->d_x, this->d_y, this->d_z};
  }
 
  friend Point3 operator+(Point3 lhs, const Point3 &rhs) {
    lhs += rhs;
    return lhs;
  }
 
  friend Point3 operator-(Point3 lhs, const Point3 &rhs) {
    lhs -= rhs;
    return lhs;
  }
 
  friend double operator*(Point3 lhs, const Point3 rhs) {
    return lhs.d_x * rhs.d_x + lhs.d_y * rhs.d_y + lhs.d_z * rhs.d_z;
  }
 
  friend Point3 operator*(Point3 lhs, const double rhs) {
    lhs *= rhs;
    return lhs;
  }
 
  friend Point3 operator+(Point3 lhs, const double rhs) {
    return {lhs.d_x + rhs, lhs.d_y + rhs, lhs.d_z + rhs};
  }
 
  friend Point3 operator+(const double lhs, Point3 rhs) {
    return {lhs + rhs.d_x, lhs + rhs.d_y, lhs + rhs.d_z};
  }
 
  friend Point3 operator-(Point3 lhs, const double rhs) {
    return {lhs.d_x - rhs, lhs.d_y - rhs, lhs.d_z - rhs};
  }
 
  friend Point3 operator-(const double lhs, Point3 rhs) {
    return {lhs - rhs.d_x, lhs - rhs.d_y, lhs - rhs.d_z};
  }
 
  friend Point3 operator*(const double lhs, Point3 rhs) {
    rhs *= lhs;
    return rhs;
  }
 
  friend Point3 operator/(Point3 lhs, const double rhs) {
    lhs /= rhs;
    return lhs;
  }
 
  Point3 &operator+=(const double b) {
 
    d_x += b;
    d_y += b;
    d_z += b;
    return *this;
  }
 
  Point3 &operator-=(const double b) {
 
    d_x -= b;
    d_y -= b;
    d_z -= b;
    return *this;
  }
 
  Point3 &operator*=(const double b) {
 
    d_x *= b;
    d_y *= b;
    d_z *= b;
    return *this;
  }
 
  Point3 &operator+=(const Point3 &b) {
 
    d_x += b.d_x;
    d_y += b.d_y;
    d_z += b.d_z;
    return *this;
  }
 
  Point3 &operator-=(const Point3 &b) {
 
    d_x -= b.d_x;
    d_y -= b.d_y;
    d_z -= b.d_z;
    return *this;
  }
 
  Point3 &operator*=(const Point3 &b) {
 
    d_x *= b.d_x;
    d_y *= b.d_y;
    d_z *= b.d_z;
    return *this;
  }
 
  Point3 &operator/=(const double b) {
 
    d_x /= b;
    d_y /= b;
    d_z /= b;
    return *this;
  }
 
  double &operator[](size_t i) {
 
    if (i == 0)
      return d_x;
    else if (i == 1)
      return d_y;
    else
      return d_z;
  }
 
  const double &operator[](size_t i) const {
 
    if (i == 0)
      return d_x;
    else if (i == 1)
      return d_y;
    else
      return d_z;
  }
  
  friend std::ostream &operator<<(std::ostream &os, const Point3 p);
 
};
 
inline std::ostream &operator<<(std::ostream &os, const Point3 p) {
  os << p.d_x << " " << p.d_y << " " << p.d_z << std::endl;
  return os;
}
 
} // namespace util
 
#endif