tiny_vector3.h

/*
 * Copyright 2020 Google LLC
 *
 * 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 TINY_VECTOR3_H
#define TINY_VECTOR3_H

#include <assert.h>
#include <stdio.h>

#include <string>

typedef void (*TinySubmitProfileTiming)(const std::string& profileName);

template <typename TinyScalar, typename TinyConstants>
struct TinyVector3 {
  TinyScalar m_x;
  TinyScalar m_y;
  TinyScalar m_z;

  int m_size{3};

  // TODO(ericheiden) remove unused rows argument (used for MatrixXxX)
  explicit TinyVector3(int unused = 0) {
    set_zero();
  }

  TinyVector3(const TinyVector3& rhs) {
    m_x = rhs.m_x;
    m_y = rhs.m_y;
    m_z = rhs.m_z;
  }

  TinyVector3& operator=(const TinyVector3& rhs) {
    m_x = rhs.m_x;
    m_y = rhs.m_y;
    m_z = rhs.m_z;
    return *this;
  }

  TinyVector3(TinyScalar x, TinyScalar y, TinyScalar z)
      : m_x(x), m_y(y), m_z(z) {}

  TinyScalar x() const { return m_x; }

  TinyScalar getX() const { return m_x; }

  void setX(TinyScalar x) { m_x = x; }

  TinyScalar y() const { return m_y; }

  TinyScalar getY() const { return m_y; }
  void setY(TinyScalar y) { m_y = y; }

  TinyScalar z() const { return m_z; }
  TinyScalar getZ() const { return m_z; }
  void setZ(TinyScalar z) { m_z = z; }
  void setValue(const TinyScalar& x, const TinyScalar& y, const TinyScalar& z) {
    m_x = x;
    m_y = y;
    m_z = z;
  }

  void set_zero() {
    setValue(TinyConstants::zero(), TinyConstants::zero(),
             TinyConstants::zero());
  }

  static TinyVector3 zero() {
    TinyVector3 res(TinyConstants::zero(), TinyConstants::zero(),
                    TinyConstants::zero());
    return res;
  }

  static TinyVector3 makeUnitZ() {
    TinyVector3 res;
    res.setValue(TinyConstants::zero(), TinyConstants::zero(),
                 TinyConstants::one());
    return res;
  }
  static TinyVector3 makeUnitX() {
    TinyVector3 res;
    res.setValue(TinyConstants::one(), TinyConstants::zero(),
                 TinyConstants::zero());
    return res;
  }

  static TinyVector3 makeUnitY() {
    TinyVector3 res;
    res.setValue(TinyConstants::zero(), TinyConstants::one(),
                 TinyConstants::zero());
    return res;
  }
  static TinyVector3 create(const TinyScalar& x, const TinyScalar& y,
                            const TinyScalar& z) {
    TinyVector3<TinyScalar, TinyConstants> res;
    res.setValue(x, y, z);
    return res;
  }

  inline TinyScalar dot(const TinyVector3& other) const {
    TinyScalar res =
        getX() * other.getX() + getY() * other.getY() + getZ() * other.getZ();
    return res;
  }

  static TinyScalar dot2(const TinyVector3& a, const TinyVector3& other) {
    TinyScalar res = a.getX() * other.getX() + a.getY() * other.getY() +
                     a.getZ() * other.getZ();
    return res;
  }

  inline TinyVector3 cross(const TinyVector3& v) const {
    return TinyVector3::create(m_y * v.m_z - m_z * v.m_y,
                               m_z * v.m_x - m_x * v.m_z,
                               m_x * v.m_y - m_y * v.m_x);
  }

  static TinyVector3 cross2(const TinyVector3& a, const TinyVector3& v) {
    return TinyVector3::create(a.m_y * v.m_z - a.m_z * v.m_y,
                               a.m_z * v.m_x - a.m_x * v.m_z,
                               a.m_x * v.m_y - a.m_y * v.m_x);
  }

  inline TinyScalar length() const {
    TinyScalar res = (*this).dot(*this);
    res = TinyConstants::sqrt1(res);
    return res;
  }

  inline TinyVector3 adj_length(const TinyScalar& R) const {
    TinyVector3 res;
    TinyScalar l = length();
    TinyScalar Rl2 = TinyConstants::half() / l * R;
    res.set_zero();
    res.m_x = 2*m_x*Rl2;
    res.m_y = 2*m_y*Rl2;
    res.m_z = 2*m_z*Rl2;
    return res;

  }

  inline TinyScalar length_squared() const {
      TinyScalar res = (*this).dot(*this);
      return res;
  }

  inline void normalize() { *this = *this * (TinyConstants::one() / length()); }

  inline TinyVector3 normalized() const {
    TinyVector3 tmp = *this;
    tmp.normalize();
    return tmp;
  }

  inline TinyScalar sqnorm() const { return m_x * m_x + m_y * m_y + m_z * m_z; }

  inline TinyVector3& operator+=(const TinyVector3& v) {
    m_x = m_x + v.m_x;
    m_y = m_y + v.m_y;
    m_z = m_z + v.m_z;
    return *this;
  }

  inline TinyVector3& operator-=(const TinyVector3& v) {
    m_x = m_x - v.m_x;
    m_y = m_y - v.m_y;
    m_z = m_z - v.m_z;
    return *this;
  }

  inline TinyVector3& operator*=(TinyScalar x) {
    m_x = m_x * x;
    m_y = m_y * x;
    m_z = m_z * x;
    return *this;
  }

  inline TinyVector3 operator-() const {
    TinyVector3 v = TinyVector3::create(-getX(), -getY(), -getZ());
    return v;
  }

  inline TinyScalar& operator[](int i) {
    switch (i) {
      case 0: {
        return m_x;
      }
      case 1: {
        return m_y;
      }
      case 2: {
        return m_z;
      }
      default: {
      }
    }
    assert(0);
    return m_x;
  }

  inline const TinyScalar& operator[](int i) const {
    switch (i) {
      case 0: {
        return m_x;
      }
      case 1: {
        return m_y;
      }
      case 2: {
        return m_z;
      }
      default: {
      }
    }
    assert(0);
    return m_x;
  }

  /**
   * Treat this vector as normal vector of a plane and compute two
   * orthogonal direction vectors of that plane.
   * p and q will be unit vectors, the normal vector does not need to be unit
   * length.
   */
  inline void plane_space(TinyVector3& p, TinyVector3& q) const {
    const TinyVector3& n = *this;
    if (TinyConstants::getBool(
      n.m_z * n.m_z > TinyConstants::half())) {
      // choose p in y-z plane
      TinyScalar a = n.m_y * n.m_y + n.m_z * n.m_z;
      TinyScalar k = TinyConstants::sqrt1(a);
      p.m_x = TinyConstants::zero();
      p.m_y = -n.m_z * k;
      p.m_z = n.m_y * k;
      // set q = n x p
      q.m_x = a * k;
      q.m_y = -n.m_x * p.m_z;
      q.m_z = n.m_x * p.m_y;
    } else {
      // choose p in x-y plane
      TinyScalar a = n.m_x * n.m_x + n.m_y * n.m_y;
      TinyScalar k = TinyConstants::sqrt1(a);
      p.m_x = -n.m_y * k;
      p.m_y = n.m_x * k;
      p.m_z = TinyConstants::zero();
      // set q = n x p
      q.m_x = -n.m_z * p.m_y;
      q.m_y = n.m_z * p.m_x;
      q.m_z = a * k;
    }
  }

  inline void adj_plane_space(
              const TinyVector3& Rp,
              const TinyVector3& Rq,
              const TinyVector3& p, 
              const TinyVector3& q,
              TinyVector3& R)  {
    const TinyVector3& n = *this;
    if (TinyConstants::getBool(
      n.m_z * n.m_z > TinyConstants::half())) {
      // choose p in y-z plane
      TinyScalar a = n.m_y * n.m_y + n.m_z * n.m_z;
      TinyScalar k = TinyConstants::sqrt1(a);


      TinyVector3 new_Rp  = Rp;
      R.m_x += p.m_y * Rq.m_z;
      new_Rp.m_y += n.m_x * Rq.m_z;
      R.m_x += - p.m_z * Rq.m_x;
      new_Rp.m_z += - n.m_x * Rq.m_y;
      TinyScalar Ra = k * Rq.m_x;
      TinyScalar Rk = a * Rq.m_x;
      R.m_y+= k * new_Rp.m_z;
      Rk += n.m_y* Rp.m_z;
      R.m_z += -k * new_Rp.m_y;
      Rk += - n.m_z  * new_Rp.m_y;
      Ra += 0.5 * Rk / TinyConstants::sqrt1(a);
      R.m_y += 2 * n.m_y * Ra;
      R.m_z += 2 * n.m_z * Ra;

    } else {
      // choose p in x-y plane
      TinyScalar a = n.m_x * n.m_x + n.m_y * n.m_y;
      TinyScalar k = TinyConstants::sqrt1(a);
      TinyVector3 new_Rp = Rp;

      TinyScalar Ra = Rq.m_z * k;
      TinyScalar Rk = Rq.m_z * a;
      R.m_z += Rq.m_y * p.m_x;
      new_Rp.m_x  += Rq.m_y * n.m_z;
      R.m_z += -Rq.m_x * p.m_y;
      new_Rp.m_y += -n.m_z * Rq.m_x;
      R.m_x += k * new_Rp.m_y;
      Rk += n.m_x * new_Rp.m_y;
      R.m_y += -k*new_Rp.m_x;
      Rk += -n.m_y * new_Rp.m_x;
      Ra += 0.5 * Rk / TinyConstants::sqrt1(a);
      R.m_x += 2 * n.m_x * Ra;
      R.m_y += 2 * n.m_y * Ra;



    }
    
  }

  void print(const char* txt) const {
    printf("%s\n", txt);
    for (int c = 0; c < 3; c++) {
      TinyScalar val = (*this)[c];
      double v = TinyConstants::getDouble(val);
      printf("%f, ", v);
    }
    printf("\n");
  }
};

template <typename TinyScalar, typename TinyConstants>
inline TinyVector3<TinyScalar, TinyConstants> operator*(
    const TinyVector3<TinyScalar, TinyConstants>& a,
    const TinyVector3<TinyScalar, TinyConstants>& b) {
  TinyVector3<TinyScalar, TinyConstants> res =
      TinyVector3<TinyScalar, TinyConstants>::create(
          a.getX() * b.getX(), a.getY() * b.getY(), a.getZ() * b.getZ());
  return res;
}

template <typename TinyScalar, typename TinyConstants>
inline TinyVector3<TinyScalar, TinyConstants> operator*(
    const TinyScalar& a, const TinyVector3<TinyScalar, TinyConstants>& b) {
  TinyVector3<TinyScalar, TinyConstants> res =
      TinyVector3<TinyScalar, TinyConstants>::create(a * b.getX(), a * b.getY(),
                                                     a * b.getZ());
  return res;
}

template <typename TinyScalar, typename TinyConstants>
inline TinyVector3<TinyScalar, TinyConstants> operator*(
    const TinyVector3<TinyScalar, TinyConstants>& b, const TinyScalar& a) {
  TinyVector3<TinyScalar, TinyConstants> res =
      TinyVector3<TinyScalar, TinyConstants>::create(a * b.getX(), a * b.getY(),
                                                     a * b.getZ());
  return res;
}

template <typename TinyScalar, typename TinyConstants>
inline TinyVector3<TinyScalar, TinyConstants> operator-(
    const TinyVector3<TinyScalar, TinyConstants>& a,
    const TinyVector3<TinyScalar, TinyConstants>& b) {
  TinyVector3<TinyScalar, TinyConstants> res =
      TinyVector3<TinyScalar, TinyConstants>::create(
          a.getX() - b.getX(), a.getY() - b.getY(), a.getZ() - b.getZ());
  return res;
}

template <typename TinyScalar, typename TinyConstants>
inline TinyVector3<TinyScalar, TinyConstants> operator+(
    const TinyVector3<TinyScalar, TinyConstants>& a,
    const TinyVector3<TinyScalar, TinyConstants>& b) {
  TinyVector3<TinyScalar, TinyConstants> res =
      TinyVector3<TinyScalar, TinyConstants>::create(
          a.getX() + b.getX(), a.getY() + b.getY(), a.getZ() + b.getZ());
  return res;
}

template <typename TinyScalar, typename TinyConstants>
inline TinyVector3<TinyScalar, TinyConstants> copySignPerElem(
    const TinyVector3<TinyScalar, TinyConstants>& vec0,
    const TinyVector3<TinyScalar, TinyConstants>& vec1) {
  return TinyVector3<TinyScalar, TinyConstants>::create(
      (vec1.getX() < TinyConstants::zero()) ? -Fix64Abs(vec0.getX())
                                            : Fix64Abs(vec0.getX()),
      (vec1.getY() < TinyConstants::zero()) ? -Fix64Abs(vec0.getY())
                                            : Fix64Abs(vec0.getY()),
      (vec1.getZ() < TinyConstants::zero()) ? -Fix64Abs(vec0.getZ())
                                            : Fix64Abs(vec0.getZ()));
}

template <typename TinyScalar, typename TinyConstants>
inline TinyVector3<TinyScalar, TinyConstants> mulPerElem(
    const TinyVector3<TinyScalar, TinyConstants>& vec0,
    const TinyVector3<TinyScalar, TinyConstants>& vec1) {
  return TinyVector3<TinyScalar, TinyConstants>::create(
      (vec0.getX() * vec1.getX()), (vec0.getY() * vec1.getY()),
      (vec0.getZ() * vec1.getZ()));
}

template <typename TinyScalar, typename TinyConstants>
inline TinyVector3<TinyScalar, TinyConstants> absPerElem(
    const TinyVector3<TinyScalar, TinyConstants>& vec) {
  return TinyVector3<TinyScalar, TinyConstants>::create(
      Fix64Abs(vec.getX()), Fix64Abs(vec.getY()), Fix64Abs(vec.getZ()));
}

#endif  // TINY_VECTOR3_H