BLI_math_quaternion_types.hh

Go to the documentation of this file.

/* SPDX-FileCopyrightText: 2023 Blender Authors
 *
 * SPDX-License-Identifier: GPL-2.0-or-later */
 
#pragma once

 
#include <ostream>
 
#include "BLI_math_angle_types.hh"
#include "BLI_math_base.hh"
#include "BLI_math_basis_types.hh"
#include "BLI_math_matrix_types.hh"
#include "BLI_math_vector_types.hh"
#include "BLI_struct_equality_utils.hh"
 
namespace blender::math {
 
/* -------------------------------------------------------------------- */

template<typename T> struct QuaternionBase {
  T w, x, y, z;
 
  QuaternionBase() = default;
 
  QuaternionBase(const T &new_w, const T &new_x, const T &new_y, const T &new_z)
      : w(new_w), x(new_x), y(new_y), z(new_z) {};

  explicit QuaternionBase(const VecBase<T, 4> &vec) : QuaternionBase(UNPACK4(vec)) {};

  QuaternionBase(const T &real, const VecBase<T, 3> &imaginary)
      : QuaternionBase(real, UNPACK3(imaginary)) {};

 
  static QuaternionBase identity()
  {
    return {1, 0, 0, 0};
  }

  static QuaternionBase zero()
  {
    return {0, 0, 0, 0};
  }

  static QuaternionBase expmap(const VecBase<T, 3> &expmap);

 
  explicit operator VecBase<T, 4>() const
  {
    return {this->w, this->x, this->y, this->z};
  }

  VecBase<T, 3> expmap() const;

  AngleRadianBase<T> twist_angle(const Axis axis) const;

  QuaternionBase twist(const Axis axis) const;

  QuaternionBase swing(const Axis axis) const;

  const VecBase<T, 3> &imaginary_part() const
  {
    return *reinterpret_cast<const VecBase<T, 3> *>(&x);
  }
  VecBase<T, 3> &imaginary_part()
  {
    return *reinterpret_cast<VecBase<T, 3> *>(&x);
  }


  QuaternionBase wrapped_around(const QuaternionBase &reference) const;

 
  friend QuaternionBase operator*(const QuaternionBase &a, const QuaternionBase &b)
  {
    return {a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
            a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
            a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
            a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x};
  }
 
  QuaternionBase &operator*=(const QuaternionBase &b) &
  {
    *this = *this * b;
    return *this;
  }
 
  /* Scalar product. */
  friend QuaternionBase operator*(const QuaternionBase &a, const T &b)
  {
    return {a.w * b, a.x * b, a.y * b, a.z * b};
  }
 
  /* Negate the quaternion. */
  friend QuaternionBase operator-(const QuaternionBase &a)
  {
    return {-a.w, -a.x, -a.y, -a.z};
  }
 
  BLI_STRUCT_EQUALITY_OPERATORS_4(QuaternionBase, w, x, y, z)
 
  uint64_t hash() const
  {
    return VecBase<T, 4>(*this).hash();
  }
 
  friend std::ostream &operator<<(std::ostream &stream, const QuaternionBase &rot)
  {
    return stream << "Quaternion" << static_cast<VecBase<T, 4>>(rot);
  }
};

 
/* -------------------------------------------------------------------- */

template<typename T> struct DualQuaternionBase {
  QuaternionBase<T> quat;
  QuaternionBase<T> trans;

  /* TODO(fclem): Can this be replaced by a Mat3x3 ?
   * It currently holds some translation in some cases. Is this wanted?
   * This would save some flops all along the way. */
  MatBase<T, 4, 4> scale;
  T scale_weight;
  T quat_weight;
 
  DualQuaternionBase() = delete;

  DualQuaternionBase(const QuaternionBase<T> &non_dual, const QuaternionBase<T> &dual);

  DualQuaternionBase(const QuaternionBase<T> &non_dual,
                     const QuaternionBase<T> &dual,
                     const MatBase<T, 4, 4> &scale_mat);

 
  static DualQuaternionBase identity()
  {
    return DualQuaternionBase(QuaternionBase<T>::identity(), QuaternionBase<T>::zero());
  }



  DualQuaternionBase &operator*=(const T &t) &;

  DualQuaternionBase &operator+=(const DualQuaternionBase &b) &;

  friend DualQuaternionBase operator*(const DualQuaternionBase &a, const T &t)
  {
    DualQuaternionBase dq = a;
    dq *= t;
    return dq;
  }

  friend DualQuaternionBase operator*(const T &t, const DualQuaternionBase &a)
  {
    DualQuaternionBase dq = a;
    dq *= t;
    return dq;
  }

  friend DualQuaternionBase operator+(const DualQuaternionBase &a, const DualQuaternionBase &b)
  {
    DualQuaternionBase dq = a;
    dq += b;
    return dq;
  }
 
  BLI_STRUCT_EQUALITY_OPERATORS_5(
      DualQuaternionBase, quat, trans, quat_weight, scale_weight, scale)
 
  friend std::ostream &operator<<(std::ostream &stream, const DualQuaternionBase &rot)
  {
    stream << "DualQuaternion(\n";
    stream << "  .quat  = " << rot.quat << "\n";
    stream << "  .trans = " << rot.trans << "\n";
    if (rot.scale_weight != T(0)) {
      stream << "  .scale = " << rot.scale;
      stream << "  .scale_weight = " << rot.scale_weight << "\n";
    }
    stream << "  .quat_weight = " << rot.quat_weight << "\n)\n";
    return stream;
  }
};

template<typename T> [[nodiscard]] inline bool is_normalized(const DualQuaternionBase<T> &dq)
{
  return dq.quat_weight == T(1);
}

 
/* -------------------------------------------------------------------- */
 
template<typename U> struct AssertUnitEpsilon<QuaternionBase<U>> {
  static constexpr U value = AssertUnitEpsilon<U>::value * 10;
};

 
/* -------------------------------------------------------------------- */
 
template<typename T> struct TypeTraits {
  using DoublePrecision = T;
};
template<> struct TypeTraits<float> {
  using DoublePrecision = double;
};
 
using Quaternion = QuaternionBase<float>;
using DualQuaternion = DualQuaternionBase<float>;

 
}  // namespace blender::math