jeanlemotan
481 lines
16 KiB
C++
481 lines
16 KiB
C++
namespace math
|
|
{
|
|
|
|
template<typename T, typename U> inline constexpr vec2<T> to_vec2(const vec2<U>& v) noexcept { return vec2<T>(T(v.x), T(v.y)); }
|
|
template<typename T> inline constexpr vec2<T> to_vec2(const vec3<T>& v) noexcept { return to_vec2<T, T>(v); }
|
|
template<typename T, typename U> inline constexpr vec2<T> to_vec2(const vec3<U>& v) noexcept { return vec2<T>(T(v.x), T(v.y)); }
|
|
template<typename T> inline constexpr vec2<T> to_vec2(const vec4<T>& v) noexcept { return to_vec2<T, T>(v); }
|
|
template<typename T, typename U> inline constexpr vec2<T> to_vec2(const vec4<U>& v) noexcept { return vec2<T>(T(v.x), T(v.y)); }
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
|
template<typename T, typename U> inline constexpr vec3<T> to_vec3(const vec3<U>& v) noexcept { return vec3<T>(T(v.x), T(v.y), T(v.z)); }
|
|
template<typename T> inline constexpr vec3<T> to_vec3(const vec2<T>& v) noexcept { return to_vec3<T, T>(v); }
|
|
template<typename T, typename U> inline constexpr vec3<T> to_vec3(const vec2<U>& v) noexcept { return vec3<T>(T(v.x), T(v.y), 0); }
|
|
template<typename T> inline constexpr vec3<T> to_vec3(const vec2<T>& v, T z) noexcept { return to_vec3<T, T>(v, z); }
|
|
template<typename T, typename U> inline constexpr vec3<T> to_vec3(const vec2<U>& v, T z) noexcept { return vec3<T>(T(v.x), T(v.y), T(z)); }
|
|
template<typename T> inline constexpr vec3<T> to_vec3(const vec4<T>& v) noexcept { return to_vec3<T, T>(v); }
|
|
template<typename T, typename U> inline constexpr vec3<T> to_vec3(const vec4<U>& v) noexcept { return vec3<T>(T(v.x), T(v.y), T(v.z)); }
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
|
template<typename T, typename U> inline constexpr vec4<T> to_vec4(const vec4<U>& v) noexcept { return vec4<T>(T(v.x), T(v.y), T(v.z), T(v.w)); }
|
|
template<typename T> inline constexpr vec4<T> to_vec4(const vec2<T>& v) noexcept { return to_vec4<T, T>(v); }
|
|
template<typename T, typename U> inline constexpr vec4<T> to_vec4(const vec2<U>& v) noexcept { return vec4<T>(T(v.x), T(v.y), T(0), T(0)); }
|
|
template<typename T> inline constexpr vec4<T> to_vec4(const vec2<T>& v, T z, T w) noexcept { return to_vec4<T, T>(v, z, w); }
|
|
template<typename T, typename U> inline constexpr vec4<T> to_vec4(const vec2<U>& v, T z, T w) noexcept { return vec4<T>(T(v.x), T(v.y), z, w); }
|
|
template<typename T> inline constexpr vec4<T> to_vec4(const vec3<T>& v) noexcept { return to_vec4<T, T>(v); }
|
|
template<typename T, typename U> inline constexpr vec4<T> to_vec4(const vec3<U>& v) noexcept { return vec4<T>(T(v.x), T(v.y), T(v.z), T(0)); }
|
|
template<typename T> inline constexpr vec4<T> to_vec4(const vec3<T>& v, T w) noexcept { return to_vec4<T, T>(v, w); }
|
|
template<typename T, typename U> inline constexpr vec4<T> to_vec4(const vec3<U>& v, T w) noexcept { return vec4<T>(T(v.x), T(v.y), T(v.z), w); }
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat2<T> to_mat2(const mat2<U>& v) noexcept
|
|
{
|
|
return mat2<T>(to_vec2<T>(v.column0),
|
|
to_vec2<T>(v.column1));
|
|
}
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat2<T> to_mat2(const mat3<U>& v) noexcept
|
|
{
|
|
const T* m = v.data();
|
|
return
|
|
{
|
|
T(m[0]), T(m[1]),
|
|
T(m[3]), T(m[4]),
|
|
};
|
|
}
|
|
template<typename T> inline constexpr mat2<T> to_mat2(const mat3<T>& v) noexcept { return to_mat2<T, T>(v); }
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat2<T> to_mat2(const mat4<U>& v) noexcept
|
|
{
|
|
return to_mat2(to_mat3(v));
|
|
}
|
|
template<typename T> inline constexpr mat2<T> to_mat2(const mat4<T>& v) noexcept { return to_mat2<T, T>(v); }
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat3<T> to_mat3(const mat3<U>& v) noexcept
|
|
{
|
|
return mat3<T>(to_vec3<T>(v.column0),
|
|
to_vec3<T>(v.column1),
|
|
to_vec3<T>(v.column2));
|
|
}
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat3<T> to_mat3(const mat4<U>& v) noexcept
|
|
{
|
|
const T* m = v.data();
|
|
return
|
|
{
|
|
T(m[0]), T(m[1]), T(m[2]),
|
|
T(m[4]), T(m[5]), T(m[6]),
|
|
T(m[8]), T(m[9]), T(m[10])
|
|
};
|
|
}
|
|
template<typename T> inline constexpr mat3<T> to_mat3(const mat4<T>& v) noexcept { return to_mat3<T, T>(v); }
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat3<T> to_mat3(const mat2<U>& v) noexcept
|
|
{
|
|
const U* m = v.data();
|
|
return
|
|
{
|
|
T(m[2]), T(m[3]), T(0),
|
|
T(m[0]), T(m[1]), T(0),
|
|
T(0), T(0), T(1),
|
|
};
|
|
}
|
|
template<typename T> inline constexpr mat3<T> to_mat3(const mat2<T>& v) noexcept { return to_mat3<T, T>(v); }
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat3<T> to_mat3(const trans2<U>& trans) noexcept
|
|
{
|
|
const U* m = trans.rotation_scale.data();
|
|
return
|
|
{
|
|
T(m[0]), T(m[1]), T(0),
|
|
T(m[2]), T(m[3]), T(0),
|
|
T(trans.translation.x), T(trans.translation.y), T(1)
|
|
};
|
|
}
|
|
template<typename T> inline constexpr mat3<T> to_mat3(const trans2<T>& trans) noexcept { return to_mat3<T, T>(trans); }
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat3<T> to_mat3(const quat<U>& rotation) noexcept
|
|
{
|
|
T x = T(rotation.x);
|
|
T y = T(rotation.y);
|
|
T z = T(rotation.z);
|
|
T w = T(rotation.w);
|
|
|
|
T const x2 = x * T(2);
|
|
T const y2 = y * T(2);
|
|
T const z2 = z * T(2);
|
|
T const xx2 = x2 * x;
|
|
T const yy2 = y2 * y;
|
|
T const zz2 = z2 * z;
|
|
T const x2y = x2 * y;
|
|
T const x2z = x2 * z;
|
|
T const x2w = x2 * w;
|
|
T const y2z = y2 * z;
|
|
T const y2w = y2 * w;
|
|
T const z2w = z2 * w;
|
|
|
|
return
|
|
{
|
|
1 - yy2 - zz2, x2y + z2w, x2z - y2w,
|
|
x2y - z2w, 1 - xx2 - zz2, y2z + x2w,
|
|
x2z + y2w, y2z - x2w, 1 - yy2 - xx2,
|
|
};
|
|
}
|
|
template<typename T> inline constexpr mat3<T> to_mat3(const quat<T>& rotation) noexcept { return to_mat3<T, T>(rotation); }
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr trans2<T> to_trans2(const mat3<U>& mat) noexcept
|
|
{
|
|
trans2<T> result(uninitialized);
|
|
T* tr = result.rotation_scale.data();
|
|
const U* m = mat.data();
|
|
|
|
tr[0] = T(m[0]);
|
|
tr[1] = T(m[1]);
|
|
TL_PLAIN_ASSERT(is_zero(m[2], U(0.00001)));
|
|
|
|
tr[2] = T(m[3]);
|
|
tr[3] = T(m[4]);
|
|
TL_PLAIN_ASSERT(is_zero(m[5], U(0.00001)));
|
|
|
|
result.translation = { T(m[6]), T(m[7]) };
|
|
TL_PLAIN_ASSERT(is_one(m[8], U(0.00001)));
|
|
|
|
return result;
|
|
}
|
|
template<typename T> inline constexpr trans2<T> to_trans2(const mat3<T>& mat) noexcept { return to_trans2<T, T>(mat); }
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr trans2<T> to_trans2(const trans3<U>& v) noexcept
|
|
{
|
|
trans2<T> result(uninitialized);
|
|
T* tr = result.rotation_scale.data();
|
|
const U* m = v.rotation_scale.data();
|
|
|
|
tr[0] = T(m[0]);
|
|
tr[1] = T(m[1]);
|
|
TL_PLAIN_ASSERT(is_zero(m[2], U(0.00001)));
|
|
|
|
tr[2] = T(m[3]);
|
|
tr[3] = T(m[4]);
|
|
TL_PLAIN_ASSERT(is_zero(m[5], U(0.00001)));
|
|
|
|
TL_PLAIN_ASSERT(is_zero(m[6], U(0.00001)));
|
|
TL_PLAIN_ASSERT(is_zero(m[7], U(0.00001)));
|
|
TL_PLAIN_ASSERT(is_one(m[8], U(0.00001)));
|
|
|
|
result.translation = { T(v.translation.x), T(v.translation.y) };
|
|
TL_PLAIN_ASSERT(is_zero(v.translation.z, U(0.00001)));
|
|
|
|
return result;
|
|
}
|
|
template<typename T> inline constexpr trans2<T> to_trans2(const trans3<T>& v) noexcept { return to_trans2<T, T>(v); }
|
|
|
|
|
|
template<typename T, typename U>
|
|
constexpr trans2<T> to_trans2(const rigid2<U>& v) noexcept
|
|
{
|
|
return trans2<T>(to_vec2<T, U>(v.translation), static_cast<T>(v.rotation));
|
|
}
|
|
template<typename T, typename U>
|
|
constexpr trans2<T> to_trans2(const rigid2<U>& v, const math::vec2<U>& scale) noexcept
|
|
{
|
|
return trans2<T>(to_vec2<T, U>(v.translation), static_cast<T>(v.rotation), to_vec2<T, U>(scale));
|
|
}
|
|
template<typename T> constexpr trans2<T> to_trans2(const rigid2<T>& v) noexcept
|
|
{
|
|
return trans2<T>(v.translation, v.rotation);
|
|
}
|
|
template<typename T> constexpr trans2<T> to_trans2(const rigid2<T>& v, const math::vec2<T>& scale) noexcept
|
|
{
|
|
return trans2<T>(v.translation, v.rotation, scale);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat4<T> to_mat4(const mat4<U>& v) noexcept
|
|
{
|
|
return mat4<T>(to_vec4<T>(v.column0),
|
|
to_vec4<T>(v.column1),
|
|
to_vec4<T>(v.column2),
|
|
to_vec4<T>(v.column3));
|
|
}
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat4<T> to_mat4(const mat3<U>& v) noexcept
|
|
{
|
|
const U* m = v.data();
|
|
return
|
|
{
|
|
T(m[0]), T(m[1]), T(m[2]), T(0),
|
|
T(m[3]), T(m[4]), T(m[5]), T(0),
|
|
T(m[6]), T(m[7]), T(m[8]), T(0),
|
|
T(0), T(0), T(0), T(1),
|
|
};
|
|
}
|
|
template<typename T> inline constexpr mat4<T> to_mat4(const mat3<T>& v) noexcept { return to_mat4<T, T>(v); }
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat4<T> to_mat4(const mat2<U>& v) noexcept
|
|
{
|
|
return to_mat4(to_mat3(v));
|
|
}
|
|
template<typename T> inline constexpr mat4<T> to_mat4(const mat2<T>& v) noexcept { return to_mat4<T, T>(v); }
|
|
|
|
|
|
template<typename T, typename U, class Policy>
|
|
constexpr inline quat<T> to_quat(mat3<U> const& mat) noexcept
|
|
{
|
|
//from: https://en.wikipedia.org/wiki/Rotation_matrix
|
|
// "an M with negative determinant has no uniquely defined closest rotation matrix."
|
|
TL_PLAIN_ASSERT(mat.determinant() >= 0);
|
|
|
|
T x = 0;
|
|
T y = 0;
|
|
T z = 0;
|
|
T w = 0;
|
|
T trace = T(mat.element(0, 0) + mat.element(1, 1) + mat.element(2, 2));
|
|
if (trace > 0)
|
|
{
|
|
// |w| > 1/2, may as well choose w > 1/2
|
|
T fRoot = sqrt<T, Policy>(trace + 1); // 2w
|
|
w = T(0.5) * fRoot;
|
|
fRoot = T(0.5) / fRoot; // 1/(4w)
|
|
x = T(mat.element(1, 2) - mat.element(2, 1)) * fRoot;
|
|
y = T(mat.element(2, 0) - mat.element(0, 2)) * fRoot;
|
|
z = T(mat.element(0, 1) - mat.element(1, 0)) * fRoot;
|
|
}
|
|
else
|
|
{
|
|
// |w| <= 1/2
|
|
int i = 0;
|
|
if (mat.element(2, 2) > mat.element(i, i))
|
|
i = 2;
|
|
else if (mat.element(1, 1) > mat.element(0, 0))
|
|
i = 1;
|
|
|
|
int j = (i + 1) % 3;
|
|
int k = (j + 1) % 3;
|
|
|
|
T fRoot = sqrt<T, Policy>(T(mat.element(i, i) - mat.element(j, j) - mat.element(k, k) + T(1)));
|
|
T* apfQuat[3] = { &x, &y, &z };
|
|
*apfQuat[i] = T(0.5) * fRoot;
|
|
fRoot = T(0.5) / fRoot;
|
|
w = T(mat.element(j, k) - mat.element(k, j)) * fRoot;
|
|
*apfQuat[j] = T(mat.element(i, j) + mat.element(j, i)) * fRoot;
|
|
*apfQuat[k] = T(mat.element(i, k) + mat.element(k, i)) * fRoot;
|
|
}
|
|
return { x, y, z, w };
|
|
}
|
|
template<typename T, class Policy> constexpr inline quat<T> to_quat(mat3<T> const& mat) noexcept { return to_quat<T, T, Policy>(mat); }
|
|
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr mat4<T> to_mat4(const trans3<U>& trans) noexcept
|
|
{
|
|
const U* m = trans.rotation_scale.data();
|
|
return
|
|
{
|
|
T(m[0]), T(m[1]), T(m[2]), T(0),
|
|
T(m[3]), T(m[4]), T(m[5]), T(0),
|
|
T(m[6]), T(m[7]), T(m[8]), T(0),
|
|
trans.translation.x, trans.translation.y, trans.translation.z, T(1),
|
|
};
|
|
}
|
|
template<typename T> inline constexpr mat4<T> to_mat4(const trans3<T>& trans) noexcept { return to_mat4<T, T>(trans); }
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr trans3<T> to_trans3(const mat4<U>& mat) noexcept
|
|
{
|
|
trans3<T> result(uninitialized);
|
|
T* tr = result.rotation_scale.data();
|
|
const U* m = mat.data();
|
|
|
|
tr[0] = T(m[0]);
|
|
tr[1] = T(m[1]);
|
|
tr[2] = T(m[2]);
|
|
TL_PLAIN_ASSERT(is_zero(m[3], U(0.00001)));
|
|
|
|
tr[3] = T(m[4]);
|
|
tr[4] = T(m[5]);
|
|
tr[5] = T(m[6]);
|
|
TL_PLAIN_ASSERT(is_zero(m[7], U(0.00001)));
|
|
|
|
tr[6] = T(m[8]);
|
|
tr[7] = T(m[9]);
|
|
tr[8] = T(m[10]);
|
|
TL_PLAIN_ASSERT(is_zero(m[11], U(0.00001)));
|
|
|
|
result.translation = { T(m[12]), T(m[13]), T(m[14]) };
|
|
TL_PLAIN_ASSERT(is_one(m[15], U(0.00001)));
|
|
|
|
return result;
|
|
}
|
|
template<typename T> inline constexpr trans3<T> to_trans3(const mat4<T>& mat) noexcept { return to_trans3<T, T>(mat); }
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr trans3<T> to_trans3(const trans2<U>& v) noexcept
|
|
{
|
|
trans3<T> result(uninitialized);
|
|
T* tr = result.rotation_scale.data();
|
|
const U* m = v.rotation_scale.data();
|
|
|
|
tr[0] = T(m[0]);
|
|
tr[1] = T(m[1]);
|
|
tr[2] = T(0);
|
|
|
|
tr[3] = T(m[2]);
|
|
tr[4] = T(m[3]);
|
|
tr[5] = T(0);
|
|
|
|
tr[6] = T(0);
|
|
tr[7] = T(0);
|
|
tr[8] = T(1);
|
|
|
|
result.translation = { T(v.translation.x), T(v.translation.y), 0 };
|
|
return result;
|
|
}
|
|
template<typename T> inline constexpr trans3<T> to_trans3(const trans2<T>& v) noexcept { return to_trans3<T, T>(v); }
|
|
|
|
template<typename T, typename U>
|
|
constexpr trans3<T> to_trans3(const rigid3<U>& v) noexcept
|
|
{
|
|
return trans3<T>(to_vec3<T, U>(v.translation), static_cast<T>(v.rotation));
|
|
}
|
|
template<typename T, typename U>
|
|
constexpr trans3<T> to_trans3(const rigid3<U>& v, const math::vec3<U>& scale) noexcept
|
|
{
|
|
return trans3<T>(to_vec3<T, U>(v.translation), static_cast<T>(v.rotation), to_vec3<T, U>(scale));
|
|
}
|
|
template<typename T> constexpr trans3<T> to_trans3(const rigid3<T>& v) noexcept
|
|
{
|
|
return trans3<T>(v.translation, v.rotation);
|
|
}
|
|
template<typename T> constexpr trans3<T> to_trans3(const rigid3<T>& v, const math::vec3<T>& scale) noexcept
|
|
{
|
|
return trans3<T>(v.translation, v.rotation, scale);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
|
template<typename T, typename U>
|
|
inline constexpr line3<T> to_line3(const segment3<U>& v) noexcept
|
|
{
|
|
return { to_vec3<T>(v.start), to_vec3<T>(v.get_direction()) };
|
|
}
|
|
template<typename T> inline constexpr line3<T> to_line3(const segment3<T>& v) noexcept { return to_line3<T, T>(v); }
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
|
namespace detail
|
|
{
|
|
|
|
template<typename To, typename From>
|
|
struct caster
|
|
{
|
|
To operator()(const From& v) const { return static_cast<To>(v); }
|
|
};
|
|
|
|
|
|
template<typename T>
|
|
struct caster<T, vec2<T>>
|
|
{
|
|
T operator()(const vec2<T>& v) const { return v[0]; }
|
|
};
|
|
template<typename T>
|
|
struct caster<T, vec3<T>>
|
|
{
|
|
T operator()(const vec3<T>& v) const { return v[0]; }
|
|
};
|
|
template<typename T>
|
|
struct caster<T, vec4<T>>
|
|
{
|
|
T operator()(const vec4<T>& v) const { return v[0]; }
|
|
};
|
|
|
|
|
|
template<typename T, typename From>
|
|
struct caster<vec2<T>, From>
|
|
{
|
|
vec2<T> operator()(const From& v) const { return to_vec2<T>(v); }
|
|
};
|
|
template<typename T, typename From>
|
|
struct caster<vec3<T>, From>
|
|
{
|
|
vec3<T> operator()(const From& v) const { return to_vec3<T>(v); }
|
|
};
|
|
template<typename T, typename From>
|
|
struct caster<vec4<T>, From>
|
|
{
|
|
vec4<T> operator()(const From& v) const { return to_vec4<T>(v); }
|
|
};
|
|
template<typename T, typename From>
|
|
struct caster<quat<T>, From>
|
|
{
|
|
quat<T> operator()(const From& v) const { return to_quat<T>(v); }
|
|
};
|
|
|
|
|
|
template<typename T, typename From>
|
|
struct caster<mat2<T>, From>
|
|
{
|
|
mat2<T> operator()(const From& v) const { return to_mat2<T>(v); }
|
|
};
|
|
template<typename T, typename From>
|
|
struct caster<mat3<T>, From>
|
|
{
|
|
mat3<T> operator()(const From& v) const { return to_mat3<T>(v); }
|
|
};
|
|
template<typename T, typename From>
|
|
struct caster<mat4<T>, From>
|
|
{
|
|
mat4<T> operator()(const From& v) const { return to_mat4<T>(v); }
|
|
};
|
|
|
|
|
|
template<typename T, typename From>
|
|
struct caster<trans2<T>, From>
|
|
{
|
|
trans2<T> operator()(const From& v) const { return to_trans2<T>(v); }
|
|
};
|
|
template<typename T, typename From>
|
|
struct caster<trans3<T>, From>
|
|
{
|
|
trans3<T> operator()(const From& v) const { return to_trans3<T>(v); }
|
|
};
|
|
|
|
|
|
template<typename T, typename From>
|
|
struct caster<line3<T>, From>
|
|
{
|
|
line3<T> operator()(const From& v) const { return to_line3<T>(v); }
|
|
};
|
|
|
|
}
|
|
|
|
|
|
template<typename To, typename From> inline constexpr To cast(const From& v) noexcept
|
|
{
|
|
return detail::caster<To, From>()(v);
|
|
}
|
|
//template<typename T> inline constexpr T cast(const T& v) noexcept { return v; }
|
|
|
|
} |