275 lines
12 KiB
C
275 lines
12 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_SIMD_QUATERNION_H_
|
||
|
|
#define OZZ_OZZ_BASE_MATHS_SIMD_QUATERNION_H_
|
||
|
|
|
||
|
|
#include "ozz/base/maths/simd_math.h"
|
||
|
|
|
||
|
|
#include <cmath>
|
||
|
|
|
||
|
|
// Implement simd quaternion.
|
||
|
|
namespace ozz {
|
||
|
|
namespace math {
|
||
|
|
// Declare the Quaternion type.
|
||
|
|
struct SimdQuaternion {
|
||
|
|
SimdFloat4 xyzw;
|
||
|
|
|
||
|
|
// Returns the identity quaternion.
|
||
|
|
static OZZ_INLINE SimdQuaternion identity() {
|
||
|
|
const SimdQuaternion quat = {simd_float4::w_axis()};
|
||
|
|
return quat;
|
||
|
|
}
|
||
|
|
|
||
|
|
// the angle in radian.
|
||
|
|
static OZZ_INLINE SimdQuaternion FromAxisAngle(_SimdFloat4 _axis,
|
||
|
|
_SimdFloat4 _angle);
|
||
|
|
|
||
|
|
// Returns a normalized quaternion initialized from an axis and angle cosine
|
||
|
|
// representation.
|
||
|
|
// Assumes the axis part (x, y, z) of _axis_angle is normalized.
|
||
|
|
// _angle.x is the angle cosine in radian, it must be within [-1,1] range.
|
||
|
|
static OZZ_INLINE SimdQuaternion FromAxisCosAngle(_SimdFloat4 _axis,
|
||
|
|
_SimdFloat4 _cos);
|
||
|
|
|
||
|
|
// Returns the quaternion that will rotate vector _from into vector _to,
|
||
|
|
// around their plan perpendicular axis.The input vectors don't need to be
|
||
|
|
// normalized, they can be null also.
|
||
|
|
static OZZ_INLINE SimdQuaternion FromVectors(_SimdFloat4 _from,
|
||
|
|
_SimdFloat4 _to);
|
||
|
|
|
||
|
|
// Returns the quaternion that will rotate vector _from into vector _to,
|
||
|
|
// around their plan perpendicular axis. The input vectors must be normalized.
|
||
|
|
static OZZ_INLINE SimdQuaternion FromUnitVectors(_SimdFloat4 _from,
|
||
|
|
_SimdFloat4 _to);
|
||
|
|
|
||
|
|
// Returns a normalized quaternion initialized from an axis angle
|
||
|
|
// representation.
|
||
|
|
// Assumes the axis part (x, y, z) of _axis_angle is normalized. _angle.x is
|
||
|
|
};
|
||
|
|
|
||
|
|
// Returns the multiplication of _a and _b. If both _a and _b are normalized,
|
||
|
|
// then the result is normalized.
|
||
|
|
OZZ_INLINE SimdQuaternion operator*(const SimdQuaternion& _a,
|
||
|
|
const SimdQuaternion& _b) {
|
||
|
|
// Original quaternion multiplication can be swizzled in a simd friendly way
|
||
|
|
// if w is negated, and some w multiplications parts (1st/last) are swaped.
|
||
|
|
//
|
||
|
|
// p1 p2 p3 p4
|
||
|
|
// _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
|
||
|
|
// _a.w * _b.w - _a.x * _b.x - _a.y * _b.y - _a.z * _b.z
|
||
|
|
// ... becomes ->
|
||
|
|
// _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
|
||
|
|
// - (_a.z * _b.z + _a.x * _b.x + _a.y * _b.y - _a.w * _b.w)
|
||
|
|
const SimdFloat4 p1 =
|
||
|
|
Swizzle<3, 3, 3, 2>(_a.xyzw) * Swizzle<0, 1, 2, 2>(_b.xyzw);
|
||
|
|
const SimdFloat4 p2 =
|
||
|
|
Swizzle<0, 1, 2, 0>(_a.xyzw) * Swizzle<3, 3, 3, 0>(_b.xyzw);
|
||
|
|
const SimdFloat4 p13 =
|
||
|
|
MAdd(Swizzle<1, 2, 0, 1>(_a.xyzw), Swizzle<2, 0, 1, 1>(_b.xyzw), p1);
|
||
|
|
const SimdFloat4 p24 =
|
||
|
|
NMAdd(Swizzle<2, 0, 1, 3>(_a.xyzw), Swizzle<1, 2, 0, 3>(_b.xyzw), p2);
|
||
|
|
const SimdQuaternion quat = {Xor(p13 + p24, simd_int4::mask_sign_w())};
|
||
|
|
return quat;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Returns the conjugate of _q. This is the same as the inverse if _q is
|
||
|
|
// normalized. Otherwise the magnitude of the inverse is 1.f/|_q|.
|
||
|
|
OZZ_INLINE SimdQuaternion Conjugate(const SimdQuaternion& _q) {
|
||
|
|
const SimdQuaternion quat = {Xor(_q.xyzw, simd_int4::mask_sign_xyz())};
|
||
|
|
return quat;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Returns the negate of _q. This represent the same rotation as q.
|
||
|
|
OZZ_INLINE SimdQuaternion operator-(const SimdQuaternion& _q) {
|
||
|
|
const SimdQuaternion quat = {Xor(_q.xyzw, simd_int4::mask_sign())};
|
||
|
|
return quat;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Returns the normalized quaternion _q.
|
||
|
|
OZZ_INLINE SimdQuaternion Normalize(const SimdQuaternion& _q) {
|
||
|
|
const SimdQuaternion quat = {Normalize4(_q.xyzw)};
|
||
|
|
return quat;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Returns the normalized quaternion _q if the norm of _q is not 0.
|
||
|
|
// Otherwise returns _safer.
|
||
|
|
OZZ_INLINE SimdQuaternion NormalizeSafe(const SimdQuaternion& _q,
|
||
|
|
const SimdQuaternion& _safer) {
|
||
|
|
const SimdQuaternion quat = {NormalizeSafe4(_q.xyzw, _safer.xyzw)};
|
||
|
|
return quat;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Returns the estimated normalized quaternion _q.
|
||
|
|
OZZ_INLINE SimdQuaternion NormalizeEst(const SimdQuaternion& _q) {
|
||
|
|
const SimdQuaternion quat = {NormalizeEst4(_q.xyzw)};
|
||
|
|
return quat;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Returns the estimated normalized quaternion _q if the norm of _q is not 0.
|
||
|
|
// Otherwise returns _safer.
|
||
|
|
OZZ_INLINE SimdQuaternion NormalizeSafeEst(const SimdQuaternion& _q,
|
||
|
|
const SimdQuaternion& _safer) {
|
||
|
|
const SimdQuaternion quat = {NormalizeSafeEst4(_q.xyzw, _safer.xyzw)};
|
||
|
|
return quat;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Tests if the _q is a normalized quaternion.
|
||
|
|
// Returns the result in the x component of the returned vector. y, z and w are
|
||
|
|
// set to 0.
|
||
|
|
OZZ_INLINE SimdInt4 IsNormalized(const SimdQuaternion& _q) {
|
||
|
|
return IsNormalized4(_q.xyzw);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Tests if the _q is a normalized quaternion.
|
||
|
|
// Uses the estimated normalization coefficient, that matches estimated math
|
||
|
|
// functions (RecpEst, MormalizeEst...).
|
||
|
|
// Returns the result in the x component of the returned vector. y, z and w are
|
||
|
|
// set to 0.
|
||
|
|
OZZ_INLINE SimdInt4 IsNormalizedEst(const SimdQuaternion& _q) {
|
||
|
|
return IsNormalizedEst4(_q.xyzw);
|
||
|
|
}
|
||
|
|
|
||
|
|
OZZ_INLINE SimdQuaternion SimdQuaternion::FromAxisAngle(_SimdFloat4 _axis,
|
||
|
|
_SimdFloat4 _angle) {
|
||
|
|
assert(AreAllTrue1(IsNormalizedEst3(_axis)) && "axis is not normalized.");
|
||
|
|
const SimdFloat4 half_angle = _angle * simd_float4::Load1(.5f);
|
||
|
|
const SimdFloat4 half_sin = SinX(half_angle);
|
||
|
|
const SimdFloat4 half_cos = CosX(half_angle);
|
||
|
|
const SimdQuaternion quat = {SetW(_axis * SplatX(half_sin), half_cos)};
|
||
|
|
return quat;
|
||
|
|
}
|
||
|
|
|
||
|
|
OZZ_INLINE SimdQuaternion SimdQuaternion::FromAxisCosAngle(_SimdFloat4 _axis,
|
||
|
|
_SimdFloat4 _cos) {
|
||
|
|
const SimdFloat4 one = simd_float4::one();
|
||
|
|
const SimdFloat4 half = simd_float4::Load1(.5f);
|
||
|
|
|
||
|
|
assert(AreAllTrue1(IsNormalizedEst3(_axis)) && "axis is not normalized.");
|
||
|
|
assert(AreAllTrue1(And(CmpGe(_cos, -one), CmpLe(_cos, one))) &&
|
||
|
|
"cos is not in [-1,1] range.");
|
||
|
|
|
||
|
|
const SimdFloat4 half_cos2 = (one + _cos) * half;
|
||
|
|
const SimdFloat4 half_sin2 = one - half_cos2;
|
||
|
|
const SimdFloat4 half_sincos2 = SetY(half_cos2, half_sin2);
|
||
|
|
const SimdFloat4 half_sincos = Sqrt(half_sincos2);
|
||
|
|
const SimdFloat4 half_sin = SplatY(half_sincos);
|
||
|
|
const SimdQuaternion quat = {SetW(_axis * half_sin, half_sincos)};
|
||
|
|
return quat;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Returns to an axis angle representation of quaternion _q.
|
||
|
|
// Assumes quaternion _q is normalized.
|
||
|
|
OZZ_INLINE SimdFloat4 ToAxisAngle(const SimdQuaternion& _q) {
|
||
|
|
assert(AreAllTrue1(IsNormalizedEst4(_q.xyzw)) && "_q is not normalized.");
|
||
|
|
const SimdFloat4 x_axis = simd_float4::x_axis();
|
||
|
|
const SimdFloat4 clamped_w = Clamp(-x_axis, SplatW(_q.xyzw), x_axis);
|
||
|
|
const SimdFloat4 half_angle = ACosX(clamped_w);
|
||
|
|
|
||
|
|
// Assuming quaternion is normalized then s always positive.
|
||
|
|
const SimdFloat4 s = SplatX(SqrtX(NMAdd(clamped_w, clamped_w, x_axis)));
|
||
|
|
// If s is close to zero then direction of axis is not important.
|
||
|
|
const SimdInt4 low = CmpLt(s, simd_float4::Load1(1e-3f));
|
||
|
|
return Select(low, x_axis,
|
||
|
|
SetW(_q.xyzw * RcpEstNR(s), half_angle + half_angle));
|
||
|
|
}
|
||
|
|
|
||
|
|
OZZ_INLINE SimdQuaternion SimdQuaternion::FromVectors(_SimdFloat4 _from,
|
||
|
|
_SimdFloat4 _to) {
|
||
|
|
// http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
|
||
|
|
const SimdFloat4 norm_from_norm_to =
|
||
|
|
SqrtX(Length3Sqr(_from) * Length3Sqr(_to));
|
||
|
|
const float norm_from_norm_to_x = GetX(norm_from_norm_to);
|
||
|
|
if (norm_from_norm_to_x < 1.e-6f) {
|
||
|
|
return SimdQuaternion::identity();
|
||
|
|
}
|
||
|
|
|
||
|
|
const SimdFloat4 real_part = norm_from_norm_to + Dot3(_from, _to);
|
||
|
|
SimdQuaternion quat;
|
||
|
|
if (GetX(real_part) < 1.e-6f * norm_from_norm_to_x) {
|
||
|
|
// If _from and _to are exactly opposite, rotate 180 degrees around an
|
||
|
|
// arbitrary orthogonal axis. Axis normalization can happen later, when we
|
||
|
|
// normalize the quaternion.
|
||
|
|
float from[4];
|
||
|
|
ozz::math::StorePtrU(_from, from);
|
||
|
|
quat.xyzw = std::abs(from[0]) > std::abs(from[2])
|
||
|
|
? ozz::math::simd_float4::Load(-from[1], from[0], 0.f, 0.f)
|
||
|
|
: ozz::math::simd_float4::Load(0.f, -from[2], from[1], 0.f);
|
||
|
|
} else {
|
||
|
|
// This is the general code path.
|
||
|
|
quat.xyzw = SetW(Cross3(_from, _to), real_part);
|
||
|
|
}
|
||
|
|
return Normalize(quat);
|
||
|
|
}
|
||
|
|
|
||
|
|
OZZ_INLINE SimdQuaternion SimdQuaternion::FromUnitVectors(_SimdFloat4 _from,
|
||
|
|
_SimdFloat4 _to) {
|
||
|
|
// http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
|
||
|
|
assert(ozz::math::AreAllTrue1(
|
||
|
|
And(IsNormalizedEst3(_from), IsNormalizedEst3(_to))) &&
|
||
|
|
"Input vectors must be normalized.");
|
||
|
|
|
||
|
|
const SimdFloat4 real_part =
|
||
|
|
ozz::math::simd_float4::x_axis() + Dot3(_from, _to);
|
||
|
|
if (GetX(real_part) < 1.e-6f) {
|
||
|
|
// If _from and _to are exactly opposite, rotate 180 degrees around an
|
||
|
|
// arbitrary orthogonal axis.
|
||
|
|
// Normalization isn't needed, as from is already.
|
||
|
|
float from[4];
|
||
|
|
ozz::math::StorePtrU(_from, from);
|
||
|
|
SimdQuaternion quat = {
|
||
|
|
std::abs(from[0]) > std::abs(from[2])
|
||
|
|
? ozz::math::simd_float4::Load(-from[1], from[0], 0.f, 0.f)
|
||
|
|
: ozz::math::simd_float4::Load(0.f, -from[2], from[1], 0.f)};
|
||
|
|
return quat;
|
||
|
|
} else {
|
||
|
|
// This is the general code path.
|
||
|
|
SimdQuaternion quat = {SetW(Cross3(_from, _to), real_part)};
|
||
|
|
return Normalize(quat);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Computes the transformation of a Quaternion and a vector _v.
|
||
|
|
// This is equivalent to carrying out the quaternion multiplications:
|
||
|
|
// _q.conjugate() * (*this) * _q
|
||
|
|
// w component of the returned vector is undefined.
|
||
|
|
OZZ_INLINE SimdFloat4 TransformVector(const SimdQuaternion& _q,
|
||
|
|
_SimdFloat4 _v) {
|
||
|
|
// http://www.neil.dantam.name/note/dantam-quaternion.pdf
|
||
|
|
// _v + 2.f * cross(_q.xyz, cross(_q.xyz, _v) + _q.w * _v)
|
||
|
|
const SimdFloat4 cross1 = MAdd(SplatW(_q.xyzw), _v, Cross3(_q.xyzw, _v));
|
||
|
|
const SimdFloat4 cross2 = Cross3(_q.xyzw, cross1);
|
||
|
|
return _v + cross2 + cross2;
|
||
|
|
}
|
||
|
|
} // namespace math
|
||
|
|
} // namespace ozz
|
||
|
|
#endif // OZZ_OZZ_BASE_MATHS_SIMD_QUATERNION_H_
|