277 lines
13 KiB
C++
277 lines
13 KiB
C++
//----------------------------------------------------------------------------//
|
|
// //
|
|
// ozz-animation is hosted at http://github.com/guillaumeblanc/ozz-animation //
|
|
// and distributed under the MIT License (MIT). //
|
|
// //
|
|
// Copyright (c) Guillaume Blanc //
|
|
// //
|
|
// Permission is hereby granted, free of charge, to any person obtaining a //
|
|
// copy of this software and associated documentation files (the "Software"), //
|
|
// to deal in the Software without restriction, including without limitation //
|
|
// the rights to use, copy, modify, merge, publish, distribute, sublicense, //
|
|
// and/or sell copies of the Software, and to permit persons to whom the //
|
|
// Software is furnished to do so, subject to the following conditions: //
|
|
// //
|
|
// The above copyright notice and this permission notice shall be included in //
|
|
// all copies or substantial portions of the Software. //
|
|
// //
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR //
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, //
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL //
|
|
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER //
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING //
|
|
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER //
|
|
// DEALINGS IN THE SOFTWARE. //
|
|
// //
|
|
//----------------------------------------------------------------------------//
|
|
|
|
#ifndef OZZ_OZZ_BASE_MATHS_SOA_FLOAT4X4_H_
|
|
#define OZZ_OZZ_BASE_MATHS_SOA_FLOAT4X4_H_
|
|
|
|
#include <cassert>
|
|
|
|
#include "ozz/base/maths/soa_float.h"
|
|
#include "ozz/base/maths/soa_quaternion.h"
|
|
#include "ozz/base/platform.h"
|
|
|
|
namespace ozz {
|
|
namespace math {
|
|
|
|
// Declare the 4x4 soa matrix type. Uses the column major convention where the
|
|
// matrix-times-vector is written v'=Mv:
|
|
// [ m.cols[0].x m.cols[1].x m.cols[2].x m.cols[3].x ] {v.x}
|
|
// | m.cols[0].y m.cols[1].y m.cols[2].y m.cols[3].y | * {v.y}
|
|
// | m.cols[0].z m.cols[1].y m.cols[2].y m.cols[3].y | {v.z}
|
|
// [ m.cols[0].w m.cols[1].w m.cols[2].w m.cols[3].w ] {v.1}
|
|
struct SoaFloat4x4 {
|
|
// Soa matrix columns.
|
|
SoaFloat4 cols[4];
|
|
|
|
// Returns the identity matrix.
|
|
static OZZ_INLINE SoaFloat4x4 identity() {
|
|
const SimdFloat4 zero = simd_float4::zero();
|
|
const SimdFloat4 one = simd_float4::one();
|
|
SoaFloat4x4 ret = {{{one, zero, zero, zero},
|
|
{zero, one, zero, zero},
|
|
{zero, zero, one, zero},
|
|
{zero, zero, zero, one}}};
|
|
return ret;
|
|
}
|
|
|
|
// Returns a scaling matrix that scales along _v.
|
|
// _v.w is ignored.
|
|
static OZZ_INLINE SoaFloat4x4 Scaling(const SoaFloat4& _v) {
|
|
const SimdFloat4 zero = simd_float4::zero();
|
|
const SimdFloat4 one = simd_float4::one();
|
|
const SoaFloat4x4 ret = {{{_v.x, zero, zero, zero},
|
|
{zero, _v.y, zero, zero},
|
|
{zero, zero, _v.z, zero},
|
|
{zero, zero, zero, one}}};
|
|
return ret;
|
|
}
|
|
|
|
// Returns the rotation matrix built from quaternion defined by x, y, z and w
|
|
// components of _v.
|
|
static OZZ_INLINE SoaFloat4x4 FromQuaternion(const SoaQuaternion& _q) {
|
|
assert(AreAllTrue(IsNormalizedEst(_q)));
|
|
|
|
const SimdFloat4 zero = simd_float4::zero();
|
|
const SimdFloat4 one = simd_float4::one();
|
|
const SimdFloat4 two = one + one;
|
|
|
|
const SimdFloat4 xx = _q.x * _q.x;
|
|
const SimdFloat4 xy = _q.x * _q.y;
|
|
const SimdFloat4 xz = _q.x * _q.z;
|
|
const SimdFloat4 xw = _q.x * _q.w;
|
|
const SimdFloat4 yy = _q.y * _q.y;
|
|
const SimdFloat4 yz = _q.y * _q.z;
|
|
const SimdFloat4 yw = _q.y * _q.w;
|
|
const SimdFloat4 zz = _q.z * _q.z;
|
|
const SimdFloat4 zw = _q.z * _q.w;
|
|
|
|
const SoaFloat4x4 ret = {
|
|
{{one - two * (yy + zz), two * (xy + zw), two * (xz - yw), zero},
|
|
{two * (xy - zw), one - two * (xx + zz), two * (yz + xw), zero},
|
|
{two * (xz + yw), two * (yz - xw), one - two * (xx + yy), zero},
|
|
{zero, zero, zero, one}}};
|
|
return ret;
|
|
}
|
|
|
|
// Returns the affine transformation matrix built from split translation,
|
|
// rotation (quaternion) and scale.
|
|
static OZZ_INLINE SoaFloat4x4 FromAffine(const SoaFloat3& _translation,
|
|
const SoaQuaternion& _quaternion,
|
|
const SoaFloat3& _scale) {
|
|
assert(AreAllTrue(IsNormalizedEst(_quaternion)));
|
|
|
|
const SimdFloat4 zero = simd_float4::zero();
|
|
const SimdFloat4 one = simd_float4::one();
|
|
const SimdFloat4 two = one + one;
|
|
|
|
const SimdFloat4 xx = _quaternion.x * _quaternion.x;
|
|
const SimdFloat4 xy = _quaternion.x * _quaternion.y;
|
|
const SimdFloat4 xz = _quaternion.x * _quaternion.z;
|
|
const SimdFloat4 xw = _quaternion.x * _quaternion.w;
|
|
const SimdFloat4 yy = _quaternion.y * _quaternion.y;
|
|
const SimdFloat4 yz = _quaternion.y * _quaternion.z;
|
|
const SimdFloat4 yw = _quaternion.y * _quaternion.w;
|
|
const SimdFloat4 zz = _quaternion.z * _quaternion.z;
|
|
const SimdFloat4 zw = _quaternion.z * _quaternion.w;
|
|
|
|
const SoaFloat4x4 ret = {
|
|
{{_scale.x * (one - two * (yy + zz)), _scale.x * two * (xy + zw),
|
|
_scale.x * two * (xz - yw), zero},
|
|
{_scale.y * two * (xy - zw), _scale.y * (one - two * (xx + zz)),
|
|
_scale.y * two * (yz + xw), zero},
|
|
{_scale.z * two * (xz + yw), _scale.z * two * (yz - xw),
|
|
_scale.z * (one - two * (xx + yy)), zero},
|
|
{_translation.x, _translation.y, _translation.z, one}}};
|
|
return ret;
|
|
}
|
|
};
|
|
|
|
// Returns the transpose of matrix _m.
|
|
OZZ_INLINE SoaFloat4x4 Transpose(const SoaFloat4x4& _m) {
|
|
const SoaFloat4x4 ret = {
|
|
{{_m.cols[0].x, _m.cols[1].x, _m.cols[2].x, _m.cols[3].x},
|
|
{_m.cols[0].y, _m.cols[1].y, _m.cols[2].y, _m.cols[3].y},
|
|
{_m.cols[0].z, _m.cols[1].z, _m.cols[2].z, _m.cols[3].z},
|
|
{_m.cols[0].w, _m.cols[1].w, _m.cols[2].w, _m.cols[3].w}}};
|
|
return ret;
|
|
}
|
|
|
|
// Returns the inverse of matrix _m.
|
|
// If _invertible is not nullptr, each component will be set to true if its
|
|
// respective matrix is invertible. If _invertible is nullptr, then an assert is
|
|
// triggered in case any of the 4 matrices isn't invertible.
|
|
OZZ_INLINE SoaFloat4x4 Invert(const SoaFloat4x4& _m,
|
|
SimdInt4* _invertible = nullptr) {
|
|
const SoaFloat4* cols = _m.cols;
|
|
const SimdFloat4 a00 = cols[2].z * cols[3].w - cols[3].z * cols[2].w;
|
|
const SimdFloat4 a01 = cols[2].y * cols[3].w - cols[3].y * cols[2].w;
|
|
const SimdFloat4 a02 = cols[2].y * cols[3].z - cols[3].y * cols[2].z;
|
|
const SimdFloat4 a03 = cols[2].x * cols[3].w - cols[3].x * cols[2].w;
|
|
const SimdFloat4 a04 = cols[2].x * cols[3].z - cols[3].x * cols[2].z;
|
|
const SimdFloat4 a05 = cols[2].x * cols[3].y - cols[3].x * cols[2].y;
|
|
const SimdFloat4 a06 = cols[1].z * cols[3].w - cols[3].z * cols[1].w;
|
|
const SimdFloat4 a07 = cols[1].y * cols[3].w - cols[3].y * cols[1].w;
|
|
const SimdFloat4 a08 = cols[1].y * cols[3].z - cols[3].y * cols[1].z;
|
|
const SimdFloat4 a09 = cols[1].x * cols[3].w - cols[3].x * cols[1].w;
|
|
const SimdFloat4 a10 = cols[1].x * cols[3].z - cols[3].x * cols[1].z;
|
|
const SimdFloat4 a11 = cols[1].y * cols[3].w - cols[3].y * cols[1].w;
|
|
const SimdFloat4 a12 = cols[1].x * cols[3].y - cols[3].x * cols[1].y;
|
|
const SimdFloat4 a13 = cols[1].z * cols[2].w - cols[2].z * cols[1].w;
|
|
const SimdFloat4 a14 = cols[1].y * cols[2].w - cols[2].y * cols[1].w;
|
|
const SimdFloat4 a15 = cols[1].y * cols[2].z - cols[2].y * cols[1].z;
|
|
const SimdFloat4 a16 = cols[1].x * cols[2].w - cols[2].x * cols[1].w;
|
|
const SimdFloat4 a17 = cols[1].x * cols[2].z - cols[2].x * cols[1].z;
|
|
const SimdFloat4 a18 = cols[1].x * cols[2].y - cols[2].x * cols[1].y;
|
|
|
|
const SimdFloat4 b0x = cols[1].y * a00 - cols[1].z * a01 + cols[1].w * a02;
|
|
const SimdFloat4 b1x = -cols[1].x * a00 + cols[1].z * a03 - cols[1].w * a04;
|
|
const SimdFloat4 b2x = cols[1].x * a01 - cols[1].y * a03 + cols[1].w * a05;
|
|
const SimdFloat4 b3x = -cols[1].x * a02 + cols[1].y * a04 - cols[1].z * a05;
|
|
|
|
const SimdFloat4 b0y = -cols[0].y * a00 + cols[0].z * a01 - cols[0].w * a02;
|
|
const SimdFloat4 b1y = cols[0].x * a00 - cols[0].z * a03 + cols[0].w * a04;
|
|
const SimdFloat4 b2y = -cols[0].x * a01 + cols[0].y * a03 - cols[0].w * a05;
|
|
const SimdFloat4 b3y = cols[0].x * a02 - cols[0].y * a04 + cols[0].z * a05;
|
|
|
|
const SimdFloat4 b0z = cols[0].y * a06 - cols[0].z * a07 + cols[0].w * a08;
|
|
const SimdFloat4 b1z = -cols[0].x * a06 + cols[0].z * a09 - cols[0].w * a10;
|
|
const SimdFloat4 b2z = cols[0].x * a11 - cols[0].y * a09 + cols[0].w * a12;
|
|
const SimdFloat4 b3z = -cols[0].x * a08 + cols[0].y * a10 - cols[0].z * a12;
|
|
|
|
const SimdFloat4 b0w = -cols[0].y * a13 + cols[0].z * a14 - cols[0].w * a15;
|
|
const SimdFloat4 b1w = cols[0].x * a13 - cols[0].z * a16 + cols[0].w * a17;
|
|
const SimdFloat4 b2w = -cols[0].x * a14 + cols[0].y * a16 - cols[0].w * a18;
|
|
const SimdFloat4 b3w = cols[0].x * a15 - cols[0].y * a17 + cols[0].z * a18;
|
|
|
|
const SimdFloat4 det =
|
|
cols[0].x * b0x + cols[0].y * b1x + cols[0].z * b2x + cols[0].w * b3x;
|
|
const SimdInt4 invertible = CmpNe(det, simd_float4::zero());
|
|
assert((_invertible || AreAllTrue(invertible)) && "Matrix is not invertible");
|
|
if (_invertible != nullptr) {
|
|
*_invertible = invertible;
|
|
}
|
|
const SimdFloat4 inv_det =
|
|
Select(invertible, RcpEstNR(det), simd_float4::zero());
|
|
|
|
const SoaFloat4x4 ret = {
|
|
{{b0x * inv_det, b0y * inv_det, b0z * inv_det, b0w * inv_det},
|
|
{b1x * inv_det, b1y * inv_det, b1z * inv_det, b1w * inv_det},
|
|
{b2x * inv_det, b2y * inv_det, b2z * inv_det, b2w * inv_det},
|
|
{b3x * inv_det, b3y * inv_det, b3z * inv_det, b3w * inv_det}}};
|
|
|
|
return ret;
|
|
}
|
|
|
|
// Scales matrix _m along the axis defined by _v components.
|
|
// _v.w is ignored.
|
|
OZZ_INLINE SoaFloat4x4 Scale(const SoaFloat4x4& _m, const SoaFloat4& _v) {
|
|
const SoaFloat4x4 ret = {{{_m.cols[0].x * _v.x, _m.cols[0].y * _v.x,
|
|
_m.cols[0].z * _v.x, _m.cols[0].w * _v.x},
|
|
{_m.cols[1].x * _v.y, _m.cols[1].y * _v.y,
|
|
_m.cols[1].z * _v.y, _m.cols[1].w * _v.y},
|
|
{_m.cols[2].x * _v.z, _m.cols[2].y * _v.z,
|
|
_m.cols[2].z * _v.z, _m.cols[2].w * _v.z},
|
|
_m.cols[3]}};
|
|
return ret;
|
|
}
|
|
} // namespace math
|
|
} // namespace ozz
|
|
|
|
// Computes the multiplication of matrix Float4x4 and vector _v.
|
|
OZZ_INLINE ozz::math::SoaFloat4 operator*(const ozz::math::SoaFloat4x4& _m,
|
|
const ozz::math::SoaFloat4& _v) {
|
|
const ozz::math::SoaFloat4 ret = {
|
|
_m.cols[0].x * _v.x + _m.cols[1].x * _v.y + _m.cols[2].x * _v.z +
|
|
_m.cols[3].x * _v.w,
|
|
_m.cols[0].y * _v.x + _m.cols[1].y * _v.y + _m.cols[2].y * _v.z +
|
|
_m.cols[3].y * _v.w,
|
|
_m.cols[0].z * _v.x + _m.cols[1].z * _v.y + _m.cols[2].z * _v.z +
|
|
_m.cols[3].z * _v.w,
|
|
_m.cols[0].w * _v.x + _m.cols[1].w * _v.y + _m.cols[2].w * _v.z +
|
|
_m.cols[3].w * _v.w};
|
|
return ret;
|
|
}
|
|
|
|
// Computes the multiplication of two matrices _a and _b.
|
|
OZZ_INLINE ozz::math::SoaFloat4x4 operator*(const ozz::math::SoaFloat4x4& _a,
|
|
const ozz::math::SoaFloat4x4& _b) {
|
|
const ozz::math::SoaFloat4x4 ret = {
|
|
{_a * _b.cols[0], _a * _b.cols[1], _a * _b.cols[2], _a * _b.cols[3]}};
|
|
return ret;
|
|
}
|
|
|
|
// Computes the per element addition of two matrices _a and _b.
|
|
OZZ_INLINE ozz::math::SoaFloat4x4 operator+(const ozz::math::SoaFloat4x4& _a,
|
|
const ozz::math::SoaFloat4x4& _b) {
|
|
const ozz::math::SoaFloat4x4 ret = {
|
|
{{_a.cols[0].x + _b.cols[0].x, _a.cols[0].y + _b.cols[0].y,
|
|
_a.cols[0].z + _b.cols[0].z, _a.cols[0].w + _b.cols[0].w},
|
|
{_a.cols[1].x + _b.cols[1].x, _a.cols[1].y + _b.cols[1].y,
|
|
_a.cols[1].z + _b.cols[1].z, _a.cols[1].w + _b.cols[1].w},
|
|
{_a.cols[2].x + _b.cols[2].x, _a.cols[2].y + _b.cols[2].y,
|
|
_a.cols[2].z + _b.cols[2].z, _a.cols[2].w + _b.cols[2].w},
|
|
{_a.cols[3].x + _b.cols[3].x, _a.cols[3].y + _b.cols[3].y,
|
|
_a.cols[3].z + _b.cols[3].z, _a.cols[3].w + _b.cols[3].w}}};
|
|
return ret;
|
|
}
|
|
|
|
// Computes the per element subtraction of two matrices _a and _b.
|
|
OZZ_INLINE ozz::math::SoaFloat4x4 operator-(const ozz::math::SoaFloat4x4& _a,
|
|
const ozz::math::SoaFloat4x4& _b) {
|
|
const ozz::math::SoaFloat4x4 ret = {
|
|
{{_a.cols[0].x - _b.cols[0].x, _a.cols[0].y - _b.cols[0].y,
|
|
_a.cols[0].z - _b.cols[0].z, _a.cols[0].w - _b.cols[0].w},
|
|
{_a.cols[1].x - _b.cols[1].x, _a.cols[1].y - _b.cols[1].y,
|
|
_a.cols[1].z - _b.cols[1].z, _a.cols[1].w - _b.cols[1].w},
|
|
{_a.cols[2].x - _b.cols[2].x, _a.cols[2].y - _b.cols[2].y,
|
|
_a.cols[2].z - _b.cols[2].z, _a.cols[2].w - _b.cols[2].w},
|
|
{_a.cols[3].x - _b.cols[3].x, _a.cols[3].y - _b.cols[3].y,
|
|
_a.cols[3].z - _b.cols[3].z, _a.cols[3].w - _b.cols[3].w}}};
|
|
return ret;
|
|
}
|
|
#endif // OZZ_OZZ_BASE_MATHS_SOA_FLOAT4X4_H_
|