1 | // Copyright 2009-2021 Intel Corporation |
2 | // SPDX-License-Identifier: Apache-2.0 |
3 | |
4 | #pragma once |
5 | |
6 | #include "linearspace2.h" |
7 | #include "linearspace3.h" |
8 | #include "quaternion.h" |
9 | #include "bbox.h" |
10 | #include "vec4.h" |
11 | |
12 | namespace embree |
13 | { |
14 | #define VectorT typename L::Vector |
15 | #define ScalarT typename L::Vector::Scalar |
16 | |
17 | //////////////////////////////////////////////////////////////////////////////// |
18 | // Affine Space |
19 | //////////////////////////////////////////////////////////////////////////////// |
20 | |
21 | template<typename L> |
22 | struct AffineSpaceT |
23 | { |
24 | L l; /*< linear part of affine space */ |
25 | VectorT p; /*< affine part of affine space */ |
26 | |
27 | //////////////////////////////////////////////////////////////////////////////// |
28 | // Constructors, Assignment, Cast, Copy Operations |
29 | //////////////////////////////////////////////////////////////////////////////// |
30 | |
31 | __forceinline AffineSpaceT ( ) { } |
32 | __forceinline AffineSpaceT ( const AffineSpaceT& other ) { l = other.l; p = other.p; } |
33 | __forceinline AffineSpaceT ( const L & other ) { l = other ; p = VectorT(zero); } |
34 | __forceinline AffineSpaceT& operator=( const AffineSpaceT& other ) { l = other.l; p = other.p; return *this; } |
35 | |
36 | __forceinline AffineSpaceT( const VectorT& vx, const VectorT& vy, const VectorT& vz, const VectorT& p ) : l(vx,vy,vz), p(p) {} |
37 | __forceinline AffineSpaceT( const L& l, const VectorT& p ) : l(l), p(p) {} |
38 | |
39 | template<typename L1> __forceinline AffineSpaceT( const AffineSpaceT<L1>& s ) : l(s.l), p(s.p) {} |
40 | |
41 | //////////////////////////////////////////////////////////////////////////////// |
42 | // Constants |
43 | //////////////////////////////////////////////////////////////////////////////// |
44 | |
45 | __forceinline AffineSpaceT( ZeroTy ) : l(zero), p(zero) {} |
46 | __forceinline AffineSpaceT( OneTy ) : l(one), p(zero) {} |
47 | |
48 | /*! return matrix for scaling */ |
49 | static __forceinline AffineSpaceT scale(const VectorT& s) { return L::scale(s); } |
50 | |
51 | /*! return matrix for translation */ |
52 | static __forceinline AffineSpaceT translate(const VectorT& p) { return AffineSpaceT(one,p); } |
53 | |
54 | /*! return matrix for rotation, only in 2D */ |
55 | static __forceinline AffineSpaceT rotate(const ScalarT& r) { return L::rotate(r); } |
56 | |
57 | /*! return matrix for rotation around arbitrary point (2D) or axis (3D) */ |
58 | static __forceinline AffineSpaceT rotate(const VectorT& u, const ScalarT& r) { return L::rotate(u,r); } |
59 | |
60 | /*! return matrix for rotation around arbitrary axis and point, only in 3D */ |
61 | static __forceinline AffineSpaceT rotate(const VectorT& p, const VectorT& u, const ScalarT& r) { return translate(+p) * rotate(u,r) * translate(-p); } |
62 | |
63 | /*! return matrix for looking at given point, only in 3D */ |
64 | static __forceinline AffineSpaceT lookat(const VectorT& eye, const VectorT& point, const VectorT& up) { |
65 | VectorT Z = normalize(point-eye); |
66 | VectorT U = normalize(cross(up,Z)); |
67 | VectorT V = normalize(cross(Z,U)); |
68 | return AffineSpaceT(L(U,V,Z),eye); |
69 | } |
70 | |
71 | }; |
72 | |
73 | // template specialization to get correct identity matrix for type AffineSpace3fa |
74 | template<> |
75 | __forceinline AffineSpaceT<LinearSpace3ff>::AffineSpaceT( OneTy ) : l(one), p(0.f, 0.f, 0.f, 1.f) {} |
76 | |
77 | //////////////////////////////////////////////////////////////////////////////// |
78 | // Unary Operators |
79 | //////////////////////////////////////////////////////////////////////////////// |
80 | |
81 | template<typename L> __forceinline AffineSpaceT<L> operator -( const AffineSpaceT<L>& a ) { return AffineSpaceT<L>(-a.l,-a.p); } |
82 | template<typename L> __forceinline AffineSpaceT<L> operator +( const AffineSpaceT<L>& a ) { return AffineSpaceT<L>(+a.l,+a.p); } |
83 | template<typename L> __forceinline AffineSpaceT<L> rcp( const AffineSpaceT<L>& a ) { L il = rcp(a.l); return AffineSpaceT<L>(il,-(il*a.p)); } |
84 | |
85 | //////////////////////////////////////////////////////////////////////////////// |
86 | // Binary Operators |
87 | //////////////////////////////////////////////////////////////////////////////// |
88 | |
89 | template<typename L> __forceinline const AffineSpaceT<L> operator +( const AffineSpaceT<L>& a, const AffineSpaceT<L>& b ) { return AffineSpaceT<L>(a.l+b.l,a.p+b.p); } |
90 | template<typename L> __forceinline const AffineSpaceT<L> operator -( const AffineSpaceT<L>& a, const AffineSpaceT<L>& b ) { return AffineSpaceT<L>(a.l-b.l,a.p-b.p); } |
91 | |
92 | template<typename L> __forceinline const AffineSpaceT<L> operator *( const ScalarT & a, const AffineSpaceT<L>& b ) { return AffineSpaceT<L>(a*b.l,a*b.p); } |
93 | template<typename L> __forceinline const AffineSpaceT<L> operator *( const AffineSpaceT<L>& a, const AffineSpaceT<L>& b ) { return AffineSpaceT<L>(a.l*b.l,a.l*b.p+a.p); } |
94 | template<typename L> __forceinline const AffineSpaceT<L> operator /( const AffineSpaceT<L>& a, const AffineSpaceT<L>& b ) { return a * rcp(b); } |
95 | template<typename L> __forceinline const AffineSpaceT<L> operator /( const AffineSpaceT<L>& a, const ScalarT & b ) { return a * rcp(b); } |
96 | |
97 | template<typename L> __forceinline AffineSpaceT<L>& operator *=( AffineSpaceT<L>& a, const AffineSpaceT<L>& b ) { return a = a * b; } |
98 | template<typename L> __forceinline AffineSpaceT<L>& operator *=( AffineSpaceT<L>& a, const ScalarT & b ) { return a = a * b; } |
99 | template<typename L> __forceinline AffineSpaceT<L>& operator /=( AffineSpaceT<L>& a, const AffineSpaceT<L>& b ) { return a = a / b; } |
100 | template<typename L> __forceinline AffineSpaceT<L>& operator /=( AffineSpaceT<L>& a, const ScalarT & b ) { return a = a / b; } |
101 | |
102 | template<typename L> __forceinline VectorT xfmPoint (const AffineSpaceT<L>& m, const VectorT& p) { return madd(VectorT(p.x),m.l.vx,madd(VectorT(p.y),m.l.vy,madd(VectorT(p.z),m.l.vz,m.p))); } |
103 | template<typename L> __forceinline VectorT xfmVector(const AffineSpaceT<L>& m, const VectorT& v) { return xfmVector(m.l,v); } |
104 | template<typename L> __forceinline VectorT xfmNormal(const AffineSpaceT<L>& m, const VectorT& n) { return xfmNormal(m.l,n); } |
105 | |
106 | __forceinline const BBox<Vec3fa> xfmBounds(const AffineSpaceT<LinearSpace3<Vec3fa> >& m, const BBox<Vec3fa>& b) |
107 | { |
108 | BBox3fa dst = empty; |
109 | const Vec3fa p0(b.lower.x,b.lower.y,b.lower.z); dst.extend(xfmPoint(m,p0)); |
110 | const Vec3fa p1(b.lower.x,b.lower.y,b.upper.z); dst.extend(xfmPoint(m,p1)); |
111 | const Vec3fa p2(b.lower.x,b.upper.y,b.lower.z); dst.extend(xfmPoint(m,p2)); |
112 | const Vec3fa p3(b.lower.x,b.upper.y,b.upper.z); dst.extend(xfmPoint(m,p3)); |
113 | const Vec3fa p4(b.upper.x,b.lower.y,b.lower.z); dst.extend(xfmPoint(m,p4)); |
114 | const Vec3fa p5(b.upper.x,b.lower.y,b.upper.z); dst.extend(xfmPoint(m,p5)); |
115 | const Vec3fa p6(b.upper.x,b.upper.y,b.lower.z); dst.extend(xfmPoint(m,p6)); |
116 | const Vec3fa p7(b.upper.x,b.upper.y,b.upper.z); dst.extend(xfmPoint(m,p7)); |
117 | return dst; |
118 | } |
119 | |
120 | //////////////////////////////////////////////////////////////////////////////// |
121 | /// Comparison Operators |
122 | //////////////////////////////////////////////////////////////////////////////// |
123 | |
124 | template<typename L> __forceinline bool operator ==( const AffineSpaceT<L>& a, const AffineSpaceT<L>& b ) { return a.l == b.l && a.p == b.p; } |
125 | template<typename L> __forceinline bool operator !=( const AffineSpaceT<L>& a, const AffineSpaceT<L>& b ) { return a.l != b.l || a.p != b.p; } |
126 | |
127 | //////////////////////////////////////////////////////////////////////////////// |
128 | /// Select |
129 | //////////////////////////////////////////////////////////////////////////////// |
130 | |
131 | template<typename L> __forceinline AffineSpaceT<L> select ( const typename L::Vector::Scalar::Bool& s, const AffineSpaceT<L>& t, const AffineSpaceT<L>& f ) { |
132 | return AffineSpaceT<L>(select(s,t.l,f.l),select(s,t.p,f.p)); |
133 | } |
134 | |
135 | //////////////////////////////////////////////////////////////////////////////// |
136 | // Output Operators |
137 | //////////////////////////////////////////////////////////////////////////////// |
138 | |
139 | template<typename L> static embree_ostream operator<<(embree_ostream cout, const AffineSpaceT<L>& m) { |
140 | return cout << "{ l = " << m.l << ", p = " << m.p << " }" ; |
141 | } |
142 | |
143 | //////////////////////////////////////////////////////////////////////////////// |
144 | // Template Instantiations |
145 | //////////////////////////////////////////////////////////////////////////////// |
146 | |
147 | typedef AffineSpaceT<LinearSpace2f> AffineSpace2f; |
148 | typedef AffineSpaceT<LinearSpace3f> AffineSpace3f; |
149 | typedef AffineSpaceT<LinearSpace3fa> AffineSpace3fa; |
150 | typedef AffineSpaceT<LinearSpace3fx> AffineSpace3fx; |
151 | typedef AffineSpaceT<LinearSpace3ff> AffineSpace3ff; |
152 | typedef AffineSpaceT<Quaternion3f > OrthonormalSpace3f; |
153 | |
154 | template<int N> using AffineSpace3vf = AffineSpaceT<LinearSpace3<Vec3<vfloat<N>>>>; |
155 | typedef AffineSpaceT<LinearSpace3<Vec3<vfloat<4>>>> AffineSpace3vf4; |
156 | typedef AffineSpaceT<LinearSpace3<Vec3<vfloat<8>>>> AffineSpace3vf8; |
157 | typedef AffineSpaceT<LinearSpace3<Vec3<vfloat<16>>>> AffineSpace3vf16; |
158 | |
159 | template<int N> using AffineSpace3vff = AffineSpaceT<LinearSpace3<Vec4<vfloat<N>>>>; |
160 | typedef AffineSpaceT<LinearSpace3<Vec4<vfloat<4>>>> AffineSpace3vfa4; |
161 | typedef AffineSpaceT<LinearSpace3<Vec4<vfloat<8>>>> AffineSpace3vfa8; |
162 | typedef AffineSpaceT<LinearSpace3<Vec4<vfloat<16>>>> AffineSpace3vfa16; |
163 | |
164 | ////////////////////////////////////////////////////////////////////////////// |
165 | /// Interpolation |
166 | ////////////////////////////////////////////////////////////////////////////// |
167 | template<typename T, typename R> |
168 | __forceinline AffineSpaceT<T> lerp(const AffineSpaceT<T>& M0, |
169 | const AffineSpaceT<T>& M1, |
170 | const R& t) |
171 | { |
172 | return AffineSpaceT<T>(lerp(M0.l,M1.l,t),lerp(M0.p,M1.p,t)); |
173 | } |
174 | |
175 | // slerp interprets the 16 floats of the matrix M = D * R * S as components of |
176 | // three matrizes (D, R, S) that are interpolated individually. |
177 | template<typename T> __forceinline AffineSpaceT<LinearSpace3<Vec3<T>>> |
178 | slerp(const AffineSpaceT<LinearSpace3<Vec4<T>>>& M0, |
179 | const AffineSpaceT<LinearSpace3<Vec4<T>>>& M1, |
180 | const T& t) |
181 | { |
182 | QuaternionT<T> q0(M0.p.w, M0.l.vx.w, M0.l.vy.w, M0.l.vz.w); |
183 | QuaternionT<T> q1(M1.p.w, M1.l.vx.w, M1.l.vy.w, M1.l.vz.w); |
184 | QuaternionT<T> q = slerp(q0, q1, t); |
185 | |
186 | AffineSpaceT<LinearSpace3<Vec3<T>>> S = lerp(M0, M1, t); |
187 | AffineSpaceT<LinearSpace3<Vec3<T>>> D(one); |
188 | D.p.x = S.l.vx.y; |
189 | D.p.y = S.l.vx.z; |
190 | D.p.z = S.l.vy.z; |
191 | S.l.vx.y = 0; |
192 | S.l.vx.z = 0; |
193 | S.l.vy.z = 0; |
194 | |
195 | AffineSpaceT<LinearSpace3<Vec3<T>>> R = LinearSpace3<Vec3<T>>(q); |
196 | return D * R * S; |
197 | } |
198 | |
199 | // this is a specialized version for Vec3fa because that does |
200 | // not play along nicely with the other templated Vec3/Vec4 types |
201 | __forceinline AffineSpace3fa slerp(const AffineSpace3ff& M0, |
202 | const AffineSpace3ff& M1, |
203 | const float& t) |
204 | { |
205 | Quaternion3f q0(M0.p.w, M0.l.vx.w, M0.l.vy.w, M0.l.vz.w); |
206 | Quaternion3f q1(M1.p.w, M1.l.vx.w, M1.l.vy.w, M1.l.vz.w); |
207 | Quaternion3f q = slerp(q0, q1, t); |
208 | |
209 | AffineSpace3fa S = lerp(M0, M1, t); |
210 | AffineSpace3fa D(one); |
211 | D.p.x = S.l.vx.y; |
212 | D.p.y = S.l.vx.z; |
213 | D.p.z = S.l.vy.z; |
214 | S.l.vx.y = 0; |
215 | S.l.vx.z = 0; |
216 | S.l.vy.z = 0; |
217 | |
218 | AffineSpace3fa R = LinearSpace3fa(q); |
219 | return D * R * S; |
220 | } |
221 | |
222 | __forceinline AffineSpace3fa quaternionDecompositionToAffineSpace(const AffineSpace3ff& qd) |
223 | { |
224 | // compute affine transform from quaternion decomposition |
225 | Quaternion3f q(qd.p.w, qd.l.vx.w, qd.l.vy.w, qd.l.vz.w); |
226 | AffineSpace3fa M = qd; |
227 | AffineSpace3fa D(one); |
228 | D.p.x = M.l.vx.y; |
229 | D.p.y = M.l.vx.z; |
230 | D.p.z = M.l.vy.z; |
231 | M.l.vx.y = 0; |
232 | M.l.vx.z = 0; |
233 | M.l.vy.z = 0; |
234 | AffineSpace3fa R = LinearSpace3fa(q); |
235 | return D * R * M; |
236 | } |
237 | |
238 | __forceinline void quaternionDecomposition(const AffineSpace3ff& qd, Vec3fa& T, Quaternion3f& q, AffineSpace3fa& S) |
239 | { |
240 | q = Quaternion3f(qd.p.w, qd.l.vx.w, qd.l.vy.w, qd.l.vz.w); |
241 | S = qd; |
242 | T.x = qd.l.vx.y; |
243 | T.y = qd.l.vx.z; |
244 | T.z = qd.l.vy.z; |
245 | S.l.vx.y = 0; |
246 | S.l.vx.z = 0; |
247 | S.l.vy.z = 0; |
248 | } |
249 | |
250 | __forceinline AffineSpace3fx quaternionDecomposition(Vec3fa const& T, Quaternion3f const& q, AffineSpace3fa const& S) |
251 | { |
252 | AffineSpace3ff M = S; |
253 | M.l.vx.w = q.i; |
254 | M.l.vy.w = q.j; |
255 | M.l.vz.w = q.k; |
256 | M.p.w = q.r; |
257 | M.l.vx.y = T.x; |
258 | M.l.vx.z = T.y; |
259 | M.l.vy.z = T.z; |
260 | return M; |
261 | } |
262 | |
263 | struct __aligned(16) QuaternionDecomposition |
264 | { |
265 | float scale_x = 1.f; |
266 | float scale_y = 1.f; |
267 | float scale_z = 1.f; |
268 | float skew_xy = 0.f; |
269 | float skew_xz = 0.f; |
270 | float skew_yz = 0.f; |
271 | float shift_x = 0.f; |
272 | float shift_y = 0.f; |
273 | float shift_z = 0.f; |
274 | float quaternion_r = 1.f; |
275 | float quaternion_i = 0.f; |
276 | float quaternion_j = 0.f; |
277 | float quaternion_k = 0.f; |
278 | float translation_x = 0.f; |
279 | float translation_y = 0.f; |
280 | float translation_z = 0.f; |
281 | }; |
282 | |
283 | __forceinline QuaternionDecomposition quaternionDecomposition(AffineSpace3ff const& M) |
284 | { |
285 | QuaternionDecomposition qd; |
286 | qd.scale_x = M.l.vx.x; |
287 | qd.scale_y = M.l.vy.y; |
288 | qd.scale_z = M.l.vz.z; |
289 | qd.shift_x = M.p.x; |
290 | qd.shift_y = M.p.y; |
291 | qd.shift_z = M.p.z; |
292 | qd.translation_x = M.l.vx.y; |
293 | qd.translation_y = M.l.vx.z; |
294 | qd.translation_z = M.l.vy.z; |
295 | qd.skew_xy = M.l.vy.x; |
296 | qd.skew_xz = M.l.vz.x; |
297 | qd.skew_yz = M.l.vz.y; |
298 | qd.quaternion_r = M.p.w; |
299 | qd.quaternion_i = M.l.vx.w; |
300 | qd.quaternion_j = M.l.vy.w; |
301 | qd.quaternion_k = M.l.vz.w; |
302 | return qd; |
303 | } |
304 | |
305 | //////////////////////////////////////////////////////////////////////////////// |
306 | /* |
307 | * ! Template Specialization for 2D: return matrix for rotation around point |
308 | * (rotation around arbitrarty vector is not meaningful in 2D) |
309 | */ |
310 | template<> __forceinline |
311 | AffineSpace2f AffineSpace2f::rotate(const Vec2f& p, const float& r) { |
312 | return translate(+p)*AffineSpace2f(LinearSpace2f::rotate(r))*translate(-p); |
313 | } |
314 | |
315 | //////////////////////////////////////////////////////////////////////////////// |
316 | // Similarity Transform |
317 | // |
318 | // checks, if M is a similarity transformation, i.e if there exists a factor D |
319 | // such that for all x,y: distance(Mx, My) = D * distance(x, y) |
320 | //////////////////////////////////////////////////////////////////////////////// |
321 | __forceinline bool similarityTransform(const AffineSpace3fa& M, float* D) |
322 | { |
323 | if (D) *D = 0.f; |
324 | if (abs(dot(M.l.vx, M.l.vy)) > 1e-5f) return false; |
325 | if (abs(dot(M.l.vx, M.l.vz)) > 1e-5f) return false; |
326 | if (abs(dot(M.l.vy, M.l.vz)) > 1e-5f) return false; |
327 | |
328 | const float D_x = dot(M.l.vx, M.l.vx); |
329 | const float D_y = dot(M.l.vy, M.l.vy); |
330 | const float D_z = dot(M.l.vz, M.l.vz); |
331 | |
332 | if (abs(D_x - D_y) > 1e-5f || |
333 | abs(D_x - D_z) > 1e-5f || |
334 | abs(D_y - D_z) > 1e-5f) |
335 | return false; |
336 | |
337 | if (D) *D = sqrtf(D_x); |
338 | return true; |
339 | } |
340 | |
341 | __forceinline void AffineSpace3fa_store_unaligned(const AffineSpace3fa &source, AffineSpace3fa* ptr) |
342 | { |
343 | Vec3fa::storeu(&ptr->l.vx, source.l.vx); |
344 | Vec3fa::storeu(&ptr->l.vy, source.l.vy); |
345 | Vec3fa::storeu(&ptr->l.vz, source.l.vz); |
346 | Vec3fa::storeu(&ptr->p, source.p); |
347 | } |
348 | |
349 | __forceinline AffineSpace3fa AffineSpace3fa_load_unaligned(AffineSpace3fa* ptr) |
350 | { |
351 | AffineSpace3fa space; |
352 | space.l.vx = Vec3fa::loadu(&ptr->l.vx); |
353 | space.l.vy = Vec3fa::loadu(&ptr->l.vy); |
354 | space.l.vz = Vec3fa::loadu(&ptr->l.vz); |
355 | space.p = Vec3fa::loadu(&ptr->p); |
356 | return space; |
357 | } |
358 | |
359 | #undef VectorT |
360 | #undef ScalarT |
361 | } |
362 | |