SuperMarioOdysseyOnline/include/sead/math/seadVectorCalcCommon.hpp

251 lines
5.3 KiB
C++
Raw Normal View History

2022-06-16 21:33:18 +00:00
#pragma once
#ifdef cafe
#include <cafe.h>
#endif // cafe
#include <math/seadMathCalcCommon.h>
#ifndef SEAD_MATH_VECTOR_CALC_COMMON_H_
#include <math/seadVectorCalcCommon.h>
#endif
namespace sead
{
template <typename T>
inline void Vector2CalcCommon<T>::add(Base& o, const Base& a, const Base& b)
{
o.x = a.x + b.x;
o.y = a.y + b.y;
}
template <typename T>
inline void Vector2CalcCommon<T>::sub(Base& o, const Base& a, const Base& b)
{
o.x = a.x - b.x;
o.y = a.y - b.y;
}
template <typename T>
inline void Vector2CalcCommon<T>::set(Base& o, const Base& v)
{
o.x = v.x;
o.y = v.y;
}
template <typename T>
inline void Vector2CalcCommon<T>::set(Base& v, T x, T y)
{
v.x = x;
v.y = y;
}
template <typename T>
inline void Vector3CalcCommon<T>::add(Base& o, const Base& a, const Base& b)
{
o.x = a.x + b.x;
o.y = a.y + b.y;
o.z = a.z + b.z;
}
#ifdef cafe
template <>
inline void Vector3CalcCommon<f32>::add(Base& o, const Base& a, const Base& b)
{
// TODO: Implement using intrinsics
ASM_VECAdd((const Vec*)&a, (const Vec*)&b, (Vec*)&o);
}
#endif // cafe
template <typename T>
inline void Vector3CalcCommon<T>::sub(Base& o, const Base& a, const Base& b)
{
o.x = a.x - b.x;
o.y = a.y - b.y;
o.z = a.z - b.z;
}
#ifdef cafe
template <>
inline void Vector3CalcCommon<f32>::sub(Base& o, const Base& a, const Base& b)
{
// TODO: Implement using intrinsics
ASM_VECSubtract((const Vec*)&a, (const Vec*)&b, (Vec*)&o);
}
#endif // cafe
template <typename T>
inline void Vector3CalcCommon<T>::mul(Base& o, const Mtx33& m, const Base& a)
{
const Base tmp = a;
o.x = m.m[0][0] * tmp.x + m.m[0][1] * tmp.y + m.m[0][2] * tmp.z;
o.y = m.m[1][0] * tmp.x + m.m[1][1] * tmp.y + m.m[1][2] * tmp.z;
o.z = m.m[2][0] * tmp.x + m.m[2][1] * tmp.y + m.m[2][2] * tmp.z;
}
template <typename T>
inline void Vector3CalcCommon<T>::mul(Base& o, const Mtx34& m, const Base& a)
{
const Base tmp = a;
o.x = m.m[0][0] * tmp.x + m.m[0][1] * tmp.y + m.m[0][2] * tmp.z + m.m[0][3];
o.y = m.m[1][0] * tmp.x + m.m[1][1] * tmp.y + m.m[1][2] * tmp.z + m.m[1][3];
o.z = m.m[2][0] * tmp.x + m.m[2][1] * tmp.y + m.m[2][2] * tmp.z + m.m[2][3];
}
template <typename T>
inline void Vector3CalcCommon<T>::cross(Base& o, const Base& a, const Base& b)
{
Vector3CalcCommon<T>::set(o, (a.y * b.z) - (a.z * b.y), (a.z * b.x) - (a.x * b.z),
(a.x * b.y) - (a.y * b.x));
}
#ifdef cafe
template <>
inline void Vector3CalcCommon<f32>::cross(Base& o, const Base& a, const Base& b)
{
ASM_VECCrossProduct((const Vec*)&a, (const Vec*)&b, (Vec*)&o);
}
#endif // cafe
template <typename T>
inline T Vector3CalcCommon<T>::dot(const Base& a, const Base& b)
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
#ifdef cafe
template <>
inline f32 Vector3CalcCommon<f32>::dot(const Base& a, const Base& b)
{
// TODO: Implement using intrinsics
return ASM_VECDotProduct((const Vec*)&a, (const Vec*)&b);
}
#endif // cafe
template <typename T>
inline T Vector3CalcCommon<T>::squaredLength(const Base& v)
{
return v.x * v.x + v.y * v.y + v.z * v.z;
}
template <typename T>
inline T Vector3CalcCommon<T>::length(const Base& v)
{
return MathCalcCommon<T>::sqrt(squaredLength(v));
}
#ifdef cafe
template <>
inline f32 Vector3CalcCommon<f32>::length(const Base& v)
{
return ASM_VECMag((const Vec*)&v);
}
#endif // cafe
template <typename T>
inline bool Vector3CalcCommon<T>::equals(const Base& lhs, const Base& rhs, T epsilon)
{
return MathCalcCommon<T>::equalsEpsilon(lhs.x, rhs.x, epsilon) &&
MathCalcCommon<T>::equalsEpsilon(lhs.y, rhs.y, epsilon) &&
MathCalcCommon<T>::equalsEpsilon(lhs.z, rhs.z, epsilon);
}
template <typename T>
inline void Vector3CalcCommon<T>::multScalar(Base& o, const Base& v, T t)
{
o.x = v.x * t;
o.y = v.y * t;
o.z = v.z * t;
}
#ifdef cafe
template <>
inline void Vector3CalcCommon<f32>::multScalar(Base& o, const Base& v, f32 t)
{
// TODO: Implement using intrinsics
ASM_VECScale((const Vec*)&v, (Vec*)&o, t);
}
#endif // cafe
template <typename T>
inline void Vector3CalcCommon<T>::multScalarAdd(Base& o, T t, const Base& a, const Base& b)
{
o.x = a.x * t + b.x;
o.y = a.y * t + b.y;
o.z = a.z * t + b.z;
}
#ifdef cafe
template <>
inline void Vector3CalcCommon<f32>::multScalarAdd(Base& o, f32 t, const Base& a, const Base& b)
{
// TODO: Implement using intrinsics
ASM_VECScale((const Vec*)&a, (Vec*)&o, t);
ASM_VECAdd((const Vec*)&o, (const Vec*)&b, (Vec*)&o);
}
#endif // cafe
template <typename T>
T Vector3CalcCommon<T>::normalize(Base& v)
{
const T len = length(v);
if (len > 0)
{
const T inv_len = 1 / len;
v.x *= inv_len;
v.y *= inv_len;
v.z *= inv_len;
}
return len;
}
template <typename T>
inline void Vector3CalcCommon<T>::set(Base& o, const Base& v)
{
o.x = v.x;
o.y = v.y;
o.z = v.z;
}
template <typename T>
inline void Vector3CalcCommon<T>::set(Base& v, T x, T y, T z)
{
v.x = x;
v.y = y;
v.z = z;
}
template <typename T>
inline void Vector4CalcCommon<T>::set(Base& o, const Base& v)
{
o.x = v.x;
o.y = v.y;
o.z = v.z;
o.w = v.w;
}
template <typename T>
inline void Vector4CalcCommon<T>::set(Base& v, T x, T y, T z, T w)
{
v.x = x;
v.y = y;
v.z = z;
v.w = w;
}
} // namespace sead