diff options
Diffstat (limited to 'Runtime/mecanim/math')
| -rw-r--r-- | Runtime/mecanim/math/axes.h | 128 | ||||
| -rw-r--r-- | Runtime/mecanim/math/collider.h | 55 |
2 files changed, 183 insertions, 0 deletions
diff --git a/Runtime/mecanim/math/axes.h b/Runtime/mecanim/math/axes.h new file mode 100644 index 0000000..b495ea7 --- /dev/null +++ b/Runtime/mecanim/math/axes.h @@ -0,0 +1,128 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/Math/Simd/math.h" +#include "Runtime/Math/Simd/quaternion.h" + +namespace math +{ + struct Limit + { + DEFINE_GET_TYPESTRING(Limit) + + float4 m_Min; // m_Min > 0 -> free, m_Min == 0 -> lock, m_Min < 0 -> limit + float4 m_Max; // m_Max < 0 -> free, m_Max == 0 -> lock, m_Max > 0 -> limit + + inline Limit() : m_Min(1,1,1,1), m_Max(-1,-1,-1,-1) {} + inline Limit(float4 const& aMin, float4 const& aMax, float aRange) { m_Min = aMin; m_Max = aMax; } + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_Min); + TRANSFER(m_Max); + } + }; + + enum AxesType { kFull, kZYRoll, kRollZY, kEulerXYZ }; + + struct Axes + { + DEFINE_GET_TYPESTRING(Axes) + + float4 m_PreQ; + float4 m_PostQ; + float4 m_Sgn; + Limit m_Limit; + float m_Length; + mecanim::uint32_t m_Type; // AxesType + + inline Axes() : m_PreQ(0,0,0,1), m_PostQ(0,0,0,1), m_Sgn(1,1,1,1), m_Length(1), m_Type(kEulerXYZ) {} + inline Axes(float4 const& aPreQ, float4 const& aPostQ, float4 const& aSgn, float const &aLength, AxesType const& aType) { m_PreQ = aPreQ; m_PostQ = aPostQ; m_Sgn = aSgn; m_Length = aLength; m_Type = aType; } + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_PreQ); + TRANSFER(m_PostQ); + TRANSFER(m_Sgn); + TRANSFER(m_Limit); + TRANSFER(m_Length); + TRANSFER(m_Type); + } + }; + + STATIC_INLINE float4 LimitSmootClamp(Limit const& l, float4 const& v, float1 const& smoothRange) + { + const float4 min = cond(l.m_Min<float1::zero(), -smoothClamp(-v,float4::one(),smoothRange), cond(l.m_Min>float1::zero(),v,float4::zero())); + const float4 max = cond(l.m_Max>float1::zero(), smoothClamp(v,float4::one(),smoothRange), cond(l.m_Max<float1::zero(),v,float4::zero())); + return math::cond(v<float1::zero(),min,max); + } + + STATIC_INLINE float LimitProject(float min, float max, float v) + { + float i = min < 0 ? -v / min : min > 0 ? v : 0; + float a = max > 0 ? +v / max : max < 0 ? v : 0; + return v < 0 ? i : a; + } + + STATIC_INLINE float LimitUnproject(float min, float max, float v) + { + float i = min < 0 ? -v * min : min > 0 ? v : 0; + float a = max > 0 ? +v * max : max < 0 ? v : 0; + return v < 0 ? i : a; + } + + STATIC_INLINE float4 LimitProject(Limit const& l, float4 const& v) + { + const float4 min = cond(l.m_Min<float1::zero(),-v/l.m_Min,cond(l.m_Min>float1::zero(),v,float4::zero())); + const float4 max = cond(l.m_Max>float1::zero(),+v/l.m_Max,cond(l.m_Max<float1::zero(),v,float4::zero())); + return math::cond(v<float1::zero(),min,max); + } + + STATIC_INLINE float4 LimitUnproject(Limit const& l, float4 const& v) + { + const float4 min = cond(l.m_Min<float1::zero(),-v*l.m_Min,cond(l.m_Min>float1::zero(),v,float4::zero())); + const float4 max = cond(l.m_Max>float1::zero(),+v*l.m_Max,cond(l.m_Max<float1::zero(),v,float4::zero())); + return math::cond(v<float1::zero(),min,max); + } + + STATIC_INLINE float4 AxesProject(Axes const& a, float4 const& q) + { + return normalize(quatMul(quatConj(a.m_PreQ),quatMul(q,a.m_PostQ))); + } + + STATIC_INLINE float4 AxesUnproject(Axes const& a, float4 const& q) + { + return normalize(quatMul(a.m_PreQ,quatMul(q,quatConj(a.m_PostQ)))); + } + + STATIC_INLINE float4 ToAxes(Axes const& a, float4 const& q) + { + const float4 qp = AxesProject(a,q); + float4 xyz; + switch(a.m_Type) + { + case kEulerXYZ: xyz = LimitProject(a.m_Limit, quatQuatToEuler(qp)); break; + case kZYRoll: xyz = LimitProject(a.m_Limit,doubleAtan(quat2ZYRoll(qp)*sgn(a.m_Sgn))); break; + case kRollZY: xyz = LimitProject(a.m_Limit,doubleAtan(quat2RollZY(qp)*sgn(a.m_Sgn))); break; + default: xyz = LimitProject(a.m_Limit,doubleAtan(quat2Qtan(qp)*sgn(a.m_Sgn))); break; + }; + return xyz; + } + + STATIC_INLINE float4 FromAxes(Axes const& a, float4 const& uvw) + { + float4 q; + switch(a.m_Type) + { + case kEulerXYZ: q = quatEulerToQuat(uvw); break; + case kZYRoll: q = ZYRoll2Quat(halfTan(LimitUnproject(a.m_Limit,uvw))*sgn(a.m_Sgn)); break; + case kRollZY: q = RollZY2Quat(halfTan(LimitUnproject(a.m_Limit,uvw))*sgn(a.m_Sgn)); break; + default: q = qtan2Quat(halfTan(LimitUnproject(a.m_Limit,uvw))*sgn(a.m_Sgn)); break; + }; + + return AxesUnproject(a,q); + } +} diff --git a/Runtime/mecanim/math/collider.h b/Runtime/mecanim/math/collider.h new file mode 100644 index 0000000..234281a --- /dev/null +++ b/Runtime/mecanim/math/collider.h @@ -0,0 +1,55 @@ +#pragma once + +#include "Runtime/mecanim/defs.h" +#include "Runtime/mecanim/types.h" +#include "Runtime/Math/Simd/xform.h" + +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" + +namespace math +{ + enum ColliderType { kNone = 0, kCube, kSphere, kCylinder, kCapsule }; + enum JointType { kIgnored = 0, kLocked, kLimited }; + + struct Collider + { + DEFINE_GET_TYPESTRING(Collider) + + xform m_X; + mecanim::uint32_t m_Type; // ColliderType + + inline Collider() : m_Type(kCube) {} + + // Joint information + mecanim::uint32_t m_XMotionType; + mecanim::uint32_t m_YMotionType; + mecanim::uint32_t m_ZMotionType; + float m_MinLimitX; + float m_MaxLimitX; + float m_MaxLimitY; + float m_MaxLimitZ; + + template<class TransferFunction> + inline void Transfer (TransferFunction& transfer) + { + TRANSFER(m_X); + TRANSFER(m_Type); + TRANSFER(m_XMotionType); + TRANSFER(m_YMotionType); + TRANSFER(m_ZMotionType); + TRANSFER(m_MinLimitX); + TRANSFER(m_MaxLimitX); + TRANSFER(m_MaxLimitY); + TRANSFER(m_MaxLimitZ); + } + }; + + STATIC_INLINE float4 SphereCollide(math::xform const &sphereX,math::float4 const &pos) + { + math::float4 ret = math::xformInvMulVec(sphereX,pos); + + ret *= math::rcp(math::length(ret)); + + return math::xformMulVec(sphereX,ret); + } +} |