Code/Engine/Foundation/SimdMath/Implementation/SimdQuat_inl.h - dev - redgpu/ezEngine

Code/Engine/Foundation/SimdMath/Implementation/SimdQuat_inl.h
145 строк4 KB
Trevor Cash
Update (Templatize) Primative Type Tests to test both float and double versions. (#1785)
27 янв 2026, 09:34
27 янв 2026, 09:346a0df83
#pragma once

EZ_ALWAYS_INLINE ezSimdQuat::ezSimdQuat() = default;

EZ_ALWAYS_INLINE ezSimdQuat::ezSimdQuat(const ezSimdVec4f& v)
  : m_v(v)
{
}

EZ_ALWAYS_INLINE const ezSimdQuat ezSimdQuat::MakeIdentity()
{
  return ezSimdQuat(ezSimdVec4f(0.0f, 0.0f, 0.0f, 1.0f));
}

EZ_ALWAYS_INLINE ezSimdQuat ezSimdQuat::MakeFromElements(ezSimdFloat x, ezSimdFloat y, ezSimdFloat z, ezSimdFloat w)
{
  return ezSimdQuat(ezSimdVec4f(x, y, z, w));
}

inline ezSimdQuat ezSimdQuat::MakeFromAxisAndAngle(const ezSimdVec4f& vRotationAxis, const ezSimdFloat& fAngle)
{
  ///\todo optimize
  const ezAngle halfAngle = ezAngle::MakeFromRadian(fAngle) * 0.5f;
  float s = ezMath::Sin(halfAngle);
  float c = ezMath::Cos(halfAngle);

  ezSimdQuat res;
  res.m_v = vRotationAxis * s;
  res.m_v.SetW(c);
  return res;
}

EZ_ALWAYS_INLINE void ezSimdQuat::Normalize()
{
  m_v.Normalize<4>();
}

inline ezResult ezSimdQuat::GetRotationAxisAndAngle(ezSimdVec4f& ref_vAxis, ezSimdFloat& ref_fAngle, const ezSimdFloat& fEpsilon) const
{
  ///\todo optimize
  const ezAngle acos = ezMath::ACos(float(m_v.w().Max(-1).Min(1)));
  const float d = ezMath::Sin(acos);

  if (d < fEpsilon)
  {
    ref_vAxis.Set(1.0f, 0.0f, 0.0f, 0.0f);
  }
  else
  {
    ref_vAxis = m_v / d;
  }

  ref_fAngle = acos * 2.0f;

  return EZ_SUCCESS;
}

EZ_ALWAYS_INLINE ezSimdMat4f ezSimdQuat::GetAsMat4() const
{
  const ezSimdVec4f xyz = m_v;
  const ezSimdVec4f x2y2z2 = xyz + xyz;
  const ezSimdVec4f xx2yy2zz2 = x2y2z2.CompMul(xyz);

  // diagonal terms
  // 1 - (yy2 + zz2)
  // 1 - (xx2 + zz2)
  // 1 - (xx2 + yy2)
  const ezSimdVec4f yy2_xx2_xx2 = xx2yy2zz2.Get<ezSwizzle::YXXX>();
  const ezSimdVec4f zz2_zz2_yy2 = xx2yy2zz2.Get<ezSwizzle::ZZYX>();
  ezSimdVec4f diagonal = ezSimdVec4f(1.0f) - (yy2_xx2_xx2 + zz2_zz2_yy2);
  diagonal.SetW(ezSimdFloat::MakeZero());

  // non diagonal terms
  // xy2 +- wz2
  // yz2 +- wx2
  // xz2 +- wy2
  const ezSimdVec4f x_y_x = xyz.Get<ezSwizzle::XYXX>();
  const ezSimdVec4f y2_z2_z2 = x2y2z2.Get<ezSwizzle::YZZX>();
  const ezSimdVec4f base = x_y_x.CompMul(y2_z2_z2);

  const ezSimdVec4f z2_x2_y2 = x2y2z2.Get<ezSwizzle::ZXYX>();
  const ezSimdVec4f offset = z2_x2_y2 * m_v.w();

  const ezSimdVec4f adds = base + offset;
  const ezSimdVec4f subs = base - offset;

  // final matrix layout
  // col0 = (diaX, addX, subZ, diaW)
  const ezSimdVec4f addX_u_diaX_u = adds.GetCombined<ezSwizzle::XXXX>(diagonal);
  const ezSimdVec4f subZ_u_diaW_u = subs.GetCombined<ezSwizzle::ZXWX>(diagonal);
  const ezSimdVec4f col0 = addX_u_diaX_u.GetCombined<ezSwizzle::ZXXZ>(subZ_u_diaW_u);

  // col1 = (subX, diaY, addY, diaW)
  const ezSimdVec4f subX_u_diaY_u = subs.GetCombined<ezSwizzle::XXYX>(diagonal);
  const ezSimdVec4f addY_u_diaW_u = adds.GetCombined<ezSwizzle::YXWX>(diagonal);
  const ezSimdVec4f col1 = subX_u_diaY_u.GetCombined<ezSwizzle::XZXZ>(addY_u_diaW_u);

  // col2 = (addZ, subY, diaZ, diaW)
  const ezSimdVec4f addZ_u_subY_u = adds.GetCombined<ezSwizzle::ZXYX>(subs);
  const ezSimdVec4f col2 = addZ_u_subY_u.GetCombined<ezSwizzle::XZZW>(diagonal);

  return ezSimdMat4f::MakeFromColumns(col0, col1, col2, ezSimdVec4f(0, 0, 0, 1));
}

EZ_ALWAYS_INLINE bool ezSimdQuat::IsValid(const ezSimdFloat& fEpsilon) const
{
  return m_v.IsNormalized<4>(fEpsilon);
}

EZ_ALWAYS_INLINE bool ezSimdQuat::IsNaN() const
{
  return m_v.IsNaN<4>();
}

EZ_ALWAYS_INLINE ezSimdQuat ezSimdQuat::operator-() const
{
  return ezSimdQuat(m_v.FlipSign(ezSimdVec4b(true, true, true, false)));
}

EZ_ALWAYS_INLINE ezSimdVec4f ezSimdQuat::operator*(const ezSimdVec4f& v) const
{
  ezSimdVec4f t = m_v.CrossRH(v);
  t += t;
  return v + t * m_v.w() + m_v.CrossRH(t);
}

EZ_ALWAYS_INLINE ezSimdQuat ezSimdQuat::operator*(const ezSimdQuat& q2) const
{
  ezSimdQuat q;

  q.m_v = q2.m_v * m_v.w() + m_v * q2.m_v.w() + m_v.CrossRH(q2.m_v);
  q.m_v.SetW(m_v.w() * q2.m_v.w() - m_v.Dot<3>(q2.m_v));

  return q;
}

EZ_ALWAYS_INLINE bool ezSimdQuat::operator==(const ezSimdQuat& q2) const
{
  return (m_v == q2.m_v).AllSet<4>();
}

EZ_ALWAYS_INLINE bool ezSimdQuat::operator!=(const ezSimdQuat& q2) const
{
  return (m_v != q2.m_v).AnySet<4>();
}