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 AffineSpaceTAffineSpaceT31 __forceinline AffineSpaceT ( ) { } AffineSpaceTAffineSpaceT32 __forceinline AffineSpaceT ( const AffineSpaceT& other ) { l = other.l; p = other.p; } AffineSpaceTAffineSpaceT33 __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 AffineSpaceTAffineSpaceT36 __forceinline AffineSpaceT( const VectorT& vx, const VectorT& vy, const VectorT& vz, const VectorT& p ) : l(vx,vy,vz), p(p) {} AffineSpaceTAffineSpaceT37 __forceinline AffineSpaceT( const L& l, const VectorT& p ) : l(l), p(p) {} 38 AffineSpaceTAffineSpaceT39 template<typename L1> __forceinline AffineSpaceT( const AffineSpaceT<L1>& s ) : l(s.l), p(s.p) {} 40 41 //////////////////////////////////////////////////////////////////////////////// 42 // Constants 43 //////////////////////////////////////////////////////////////////////////////// 44 AffineSpaceTAffineSpaceT45 __forceinline AffineSpaceT( ZeroTy ) : l(zero), p(zero) {} AffineSpaceTAffineSpaceT46 __forceinline AffineSpaceT( OneTy ) : l(one), p(zero) {} 47 48 /*! return matrix for scaling */ scaleAffineSpaceT49 static __forceinline AffineSpaceT scale(const VectorT& s) { return L::scale(s); } 50 51 /*! return matrix for translation */ translateAffineSpaceT52 static __forceinline AffineSpaceT translate(const VectorT& p) { return AffineSpaceT(one,p); } 53 54 /*! return matrix for rotation, only in 2D */ rotateAffineSpaceT55 static __forceinline AffineSpaceT rotate(const ScalarT& r) { return L::rotate(r); } 56 57 /*! return matrix for rotation around arbitrary point (2D) or axis (3D) */ rotateAffineSpaceT58 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 */ rotateAffineSpaceT61 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 */ lookatAffineSpaceT64 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<> AffineSpaceT(OneTy)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); } rcp(const AffineSpaceT<L> & a)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 xfmPoint(const AffineSpaceT<L> & m,const VectorT & p)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))); } xfmVector(const AffineSpaceT<L> & m,const VectorT & v)103 template<typename L> __forceinline VectorT xfmVector(const AffineSpaceT<L>& m, const VectorT& v) { return xfmVector(m.l,v); } xfmNormal(const AffineSpaceT<L> & m,const VectorT & n)104 template<typename L> __forceinline VectorT xfmNormal(const AffineSpaceT<L>& m, const VectorT& n) { return xfmNormal(m.l,n); } 105 xfmBounds(const AffineSpaceT<LinearSpace3<Vec3fa>> & m,const BBox<Vec3fa> & b)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 select(const typename L::Vector::Scalar::Bool & s,const AffineSpaceT<L> & t,const AffineSpaceT<L> & f)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> lerp(const AffineSpaceT<T> & M0,const AffineSpaceT<T> & M1,const R & t)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>>> slerp(const AffineSpaceT<LinearSpace3<Vec4<T>>> & M0,const AffineSpaceT<LinearSpace3<Vec4<T>>> & M1,const T & 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 slerp(const AffineSpace3ff & M0,const AffineSpace3ff & M1,const float & t)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 quaternionDecompositionToAffineSpace(const AffineSpace3ff & qd)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 quaternionDecomposition(const AffineSpace3ff & qd,Vec3fa & T,Quaternion3f & q,AffineSpace3fa & S)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 quaternionDecomposition(Vec3fa const & T,Quaternion3f const & q,AffineSpace3fa const & S)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 quaternionDecomposition(AffineSpace3ff const & M)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 rotate(const Vec2f & p,const float & r)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 //////////////////////////////////////////////////////////////////////////////// similarityTransform(const AffineSpace3fa & M,float * D)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 AffineSpace3fa_store_unaligned(const AffineSpace3fa & source,AffineSpace3fa * ptr)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 AffineSpace3fa_load_unaligned(AffineSpace3fa * ptr)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