vectorutils.h

Go to the documentation of this file.

#ifndef ION_MATH_VECTORUTILS_H_
#define ION_MATH_VECTORUTILS_H_


#include <algorithm>
#include <limits>

#include "ion/base/logging.h"
#include "ion/math/utils.h"
#include "ion/math/vector.h"

namespace ion {
namespace math {

template <int Dimension, typename T>
T Dot(const Vector<Dimension, T>& v0, const Vector<Dimension, T>& v1) {
  T dot = static_cast<T>(0);
  for (int i = 0; i < Dimension; ++i)
    dot += (v0[i] * v1[i]);
  return dot;
}

template <typename T>
Vector<3, T> Cross(const Vector<3, T>& v0, const Vector<3, T>& v1) {
  return Vector<3, T>(v0[1] * v1[2] - v0[2] * v1[1],
                      v0[2] * v1[0] - v0[0] * v1[2],
                      v0[0] * v1[1] - v0[1] * v1[0]);
}

template <typename T>
T Cross(const Vector<2, T>& v0, const Vector<2, T>& v1) {
  return (v0[0] * v1[1] - v0[1] * v1[0]);
}

template <int Dimension, typename T>
T LengthSquared(const Vector<Dimension, T>& v) {
  return Dot(v, v);
}

template <int Dimension, typename T>
T Length(const Vector<Dimension, T>& v) {
  return static_cast<T>(Sqrt(LengthSquared(v)));
}

template <int Dimension, typename T>
T DistanceSquared(const Point<Dimension, T>& p0,
                  const Point<Dimension, T>& p1) {
  return LengthSquared(p0 - p1);
}

template <int Dimension, typename T>
T Distance(const Point<Dimension, T>& p0, const Point<Dimension, T>& p1) {
  return Length(p0 - p1);
}

template <int Dimension, typename T>
bool Normalize(Vector<Dimension, T>* v) {
  DCHECK(v);
  const T len = Length(*v);
  if (len == static_cast<T>(0)) {
    return false;
  } else {
    (*v) /= len;
    return true;
  }
}

template <int Dimension, typename T>
const Vector<Dimension, T> Normalized(const Vector<Dimension, T>& v) {
  Vector<Dimension, T> result = v;
  if (Normalize(&result))
    return result;
  else
    return Vector<Dimension, T>::Zero();
}

template <typename T>
const Vector<2, T> Orthogonal(const Vector<2, T>& v) {
  return Vector<2, T>(-v[1], v[0]);
}

template <typename T>
const Vector<3, T> Orthogonal(const Vector<3, T>& v) {
  static const T kTolerance = static_cast<T>(0.0001);
  Vector<3, T> n = Cross(v, Vector<3, T>::AxisX());
  if (Length(n) < kTolerance) {
    n = Cross(v, Vector<3, T>::AxisY());
    if (Length(n) < kTolerance)
      n = Cross(v, Vector<3, T>::AxisZ());
  }
  return n;
}

template <int Dimension, typename T>
const Vector<Dimension, T> Orthonormal(const Vector<Dimension, T>& v) {
  return Normalized(Orthogonal(v));
}

template <int Dimension, typename T>
const Vector<Dimension, T> Projection(const Vector<Dimension, T>& v,
                                      const Vector<Dimension, T>& onto_v) {
  const T len_squared = LengthSquared(onto_v);
  return len_squared == static_cast<T>(0) ? Vector<Dimension, T>::Zero() :
      (Dot(v, onto_v) / len_squared) * onto_v;
}

template <int Dimension, typename T>
const Vector<Dimension, T> Rescale(const Vector<Dimension, T>& v, T length) {
  return Normalized(v) * length;
}

template <int Dimension, typename T>
bool VectorsAlmostEqual(const Vector<Dimension, T>& v0,
                        const Vector<Dimension, T>& v1, T tolerance) {
  for (int i = 0; i < Dimension; ++i) {
    if (Abs(v0[i] - v1[i]) > Abs(tolerance))
      return false;
  }
  return true;
}

template <int Dimension, typename T>
const Point<Dimension, T> MinBoundPoint(const Point<Dimension, T>& p0,
                                        const Point<Dimension, T>& p1) {
  Point<Dimension, T> min_point;
  for (int i = 0; i < Dimension; ++i)
    min_point[i] = std::min(p0[i], p1[i]);
  return min_point;
}

template <int Dimension, typename T>
const Point<Dimension, T> MaxBoundPoint(const Point<Dimension, T>& p0,
                                        const Point<Dimension, T>& p1) {
  Point<Dimension, T> max_point;
  for (int i = 0; i < Dimension; ++i)
    max_point[i] = std::max(p0[i], p1[i]);
  return max_point;
}

template <int Dimension, typename T>
Point<Dimension, T> ClosestPointOnSegment(const Point<Dimension, T>& p,
                                          const Point<Dimension, T>& start,
                                          const Point<Dimension, T>& end) {
  const Vector<Dimension, T> diff = end - start;
  if (LengthSquared(diff) == static_cast<T>(0))
    return start;

  const Vector<Dimension, T> to_min = p - start;
  const double projection = Dot(to_min, diff);
  const double length_squared = Dot(diff, diff);
  if (projection <= static_cast<T>(0)) {
    return start;
  } else if (length_squared <= projection) {
    return end;
  } else {
    const T t = projection / length_squared;
    return start + t * diff;
  }
}

template <int Dimension, typename T>
const T DistanceSquaredToSegment(const Point<Dimension, T>& p,
                                 const Point<Dimension, T>& start,
                                 const Point<Dimension, T>& end) {
  return DistanceSquared(p, ClosestPointOnSegment(p, start, end));
}

template <int Dimension, typename T>
const T DistanceToSegment(const Point<Dimension, T>& p,
                          const Point<Dimension, T>& start,
                          const Point<Dimension, T>& end) {
  return Distance(p, ClosestPointOnSegment(p, start, end));
}

template <int Dimension, typename T>
bool PointsAlmostEqual(const Point<Dimension, T>& v0,
                       const Point<Dimension, T>& v1, T tolerance) {
  for (int i = 0; i < Dimension; ++i) {
    if (Abs(v0[i] - v1[i]) > Abs(tolerance))
      return false;
  }
  return true;
}

template <int InDimension, int OutDimension, typename T>
bool Swizzle(const VectorBase<InDimension, T>& input,
             const char* swizzle_string, VectorBase<OutDimension, T>* output) {
  for (int i = 0; i < OutDimension; ++i) {
    int dim;
    switch (swizzle_string[i]) {
      case 'x': case 'r' : case 's':
      case 'X': case 'R' : case 'S':
        dim = 0;
        break;
      case 'y': case 'g' : case 't':
      case 'Y': case 'G' : case 'T':
        dim = 1;
        break;
      case 'z': case 'b' : case 'p':
      case 'Z': case 'B' : case 'P':
        dim = 2;
        break;
      case 'w': case 'a' : case 'q':
      case 'W': case 'A' : case 'Q':
        dim = 3;
        break;
      default:
        return false;
    }
    if (dim >= InDimension)
      return false;
    (*output)[i] = input[dim];
  }
  return true;
}

template <int Dimension, typename T>
bool IsVectorFinite(const VectorBase<Dimension, T>& v) {
  for (int i = 0; i < Dimension; ++i) {
    if (!IsFinite(v[i]))
      return false;
  }
  return true;
}

}  // namespace math
}  // namespace ion

#endif  // ION_MATH_VECTORUTILS_H_