1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
|
#ifndef SIMD_XFORM_H
#define SIMD_XFORM_H
#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h"
#include "Runtime/Math/Simd/float4.h"
#include "Runtime/Math/Simd/quaternion.h"
namespace math
{
struct ATTRIBUTE_ALIGN(ALIGN4F) xform
{
DEFINE_GET_TYPESTRING(xform)
inline xform():t(float4::zero()),q(quatIdentity()),s(float4::one()){}
inline xform( xform const& x)
{ t = x.t; q = x.q; s = x.s; }
inline xform &operator=(xform const& x)
{ t = x.t; q = x.q; s = x.s; return *this; }
inline xform(float4 const& t, float4 const& q, float4 const& s)
{ this->t = t; this->q = q; this->s = s; }
float4 t;
float4 q;
float4 s;
template<class TransferFunction>
inline void Transfer (TransferFunction& transfer)
{
TRANSFER(t);
TRANSFER(q);
TRANSFER(s);
}
};
static inline xform xformIdentity()
{
return xform( float4::zero(), quatIdentity(), float4::one() );
}
static inline float4 xformMulVec(xform const& x, float4 const& v)
{
return x.t + quatMulVec(x.q, v * x.s );
}
static inline float4 xformInvMulVec(xform const& x, float4 const& v)
{
return quatMulVec(quatConj(x.q), v - x.t) / x.s;
}
static inline float4 xformInvMulVecNS(xform const& x, float4 const& v)
{
return quatMulVec(quatConj(x.q), v - x.t);
}
static inline xform xformMul(xform const& a, xform const& b)
{
return xform( xformMulVec(a, b.t), normalize(quatMul(a.q, b.q)), a.s * b.s);
}
static inline xform xformInvMul(xform const& a, xform const& b)
{
return xform(xformInvMulVec(a, b.t), normalize( quatMul( quatConj(a.q), b.q)), b.s / a.s);
}
static inline xform xformInvMulNS(xform const& a, xform const& b)
{
return xform(xformInvMulVecNS(a, b.t), normalize( quatMul( quatConj(a.q), b.q)), float4::one());
}
static inline xform xformMulInv(xform const& a, xform const& b)
{
const float4 qinv = quatConj(b.q);
const float4 sinv = rcp(b.s);
return xform(xformMulVec(a, quatMulVec( qinv, -b.t) * sinv),normalize( quatMul( a.q, qinv)), a.s * sinv);
}
static inline xform xformBlend(xform const &a, xform const &b, float1 const& w)
{
return xform(lerp(a.t, b.t, w),quatLerp(a.q, b.q, w),scaleBlend(a.s, b.s, w));
}
static inline bool operator==(xform const& l, xform const& r)
{
return all(l.t == r.t) && all(l.q == r.q) && all(l.s == r.s);
}
static inline xform xformWeight(xform const& x, float1 const& w)
{
return xform(x.t*w,quatWeight(x.q,w),scaleWeight(x.s,w));
}
static inline xform xformAdd(xform const& a, xform const& b)
{
return xform(a.t+b.t,qtan2Quat(quat2Qtan(a.q)+quat2Qtan(b.q)),a.s*b.s);
}
static inline xform xformSub(xform const& a, xform const& b)
{
return xform(a.t-b.t,qtan2Quat(quat2Qtan(a.q)-quat2Qtan(b.q)),a.s/b.s);
}
static inline xform xformBlend(xform const* apXFormArray, float const* apWeightArray, unsigned int aCount)
{
xform ret;
ret.t = float4::zero();
ret.q = float4::zero();
ret.s = float4::one();
float sumW = 0;
unsigned int i;
for(i = 0; i < aCount ; i++)
{
float w = apWeightArray[i];
math::float1 w1(w);
sumW += w;
ret.t += apXFormArray[i].t*w1;
ret.q += cond(dot(ret.q,apXFormArray[i].q) < float1::zero(),apXFormArray[i].q * -w1,apXFormArray[i].q * w1);
ret.s *= scaleWeight(apXFormArray[i].s,w1);
}
float4 q(0,0,0,saturate(1.0f-sumW));
ret.q = normalize(ret.q+q);
return ret;
}
static inline xform mirror(xform const& x)
{
constant_float4(mirrorQ,1,-1,-1,1);
xform ret = x;
ret.t = mirror(ret.t);
ret.q *= mirrorQ;
return ret;
}
static inline xform cond(bool c, xform const& a, xform const& b)
{
return c?a:b;
}
}
#endif
|
