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
12namespace 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