1// This file is part of Eigen, a lightweight C++ template library 2// for linear algebra. 3// 4// Copyright (C) 2001 Intel Corporation 5// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr> 6// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com> 7// 8// This Source Code Form is subject to the terms of the Mozilla 9// Public License v. 2.0. If a copy of the MPL was not distributed 10// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 11 12// The SSE code for the 4x4 float and double matrix inverse in this file 13// comes from the following Intel's library: 14// http://software.intel.com/en-us/articles/optimized-matrix-library-for-use-with-the-intel-pentiumr-4-processors-sse2-instructions/ 15// 16// Here is the respective copyright and license statement: 17// 18// Copyright (c) 2001 Intel Corporation. 19// 20// Permition is granted to use, copy, distribute and prepare derivative works 21// of this library for any purpose and without fee, provided, that the above 22// copyright notice and this statement appear in all copies. 23// Intel makes no representations about the suitability of this software for 24// any purpose, and specifically disclaims all warranties. 25// See LEGAL.TXT for all the legal information. 26 27#ifndef EIGEN_INVERSE_SSE_H 28#define EIGEN_INVERSE_SSE_H 29 30namespace Eigen { 31 32namespace internal { 33 34template<typename MatrixType, typename ResultType> 35struct compute_inverse_size4<Architecture::SSE, float, MatrixType, ResultType> 36{ 37 enum { 38 MatrixAlignment = traits<MatrixType>::Alignment, 39 ResultAlignment = traits<ResultType>::Alignment, 40 StorageOrdersMatch = (MatrixType::Flags&RowMajorBit) == (ResultType::Flags&RowMajorBit) 41 }; 42 typedef typename conditional<(MatrixType::Flags&LinearAccessBit),MatrixType const &,typename MatrixType::PlainObject>::type ActualMatrixType; 43 44 static void run(const MatrixType& mat, ResultType& result) 45 { 46 ActualMatrixType matrix(mat); 47 EIGEN_ALIGN16 const unsigned int _Sign_PNNP[4] = { 0x00000000, 0x80000000, 0x80000000, 0x00000000 }; 48 49 // Load the full matrix into registers 50 __m128 _L1 = matrix.template packet<MatrixAlignment>( 0); 51 __m128 _L2 = matrix.template packet<MatrixAlignment>( 4); 52 __m128 _L3 = matrix.template packet<MatrixAlignment>( 8); 53 __m128 _L4 = matrix.template packet<MatrixAlignment>(12); 54 55 // The inverse is calculated using "Divide and Conquer" technique. The 56 // original matrix is divide into four 2x2 sub-matrices. Since each 57 // register holds four matrix element, the smaller matrices are 58 // represented as a registers. Hence we get a better locality of the 59 // calculations. 60 61 __m128 A, B, C, D; // the four sub-matrices 62 if(!StorageOrdersMatch) 63 { 64 A = _mm_unpacklo_ps(_L1, _L2); 65 B = _mm_unpacklo_ps(_L3, _L4); 66 C = _mm_unpackhi_ps(_L1, _L2); 67 D = _mm_unpackhi_ps(_L3, _L4); 68 } 69 else 70 { 71 A = _mm_movelh_ps(_L1, _L2); 72 B = _mm_movehl_ps(_L2, _L1); 73 C = _mm_movelh_ps(_L3, _L4); 74 D = _mm_movehl_ps(_L4, _L3); 75 } 76 77 __m128 iA, iB, iC, iD, // partial inverse of the sub-matrices 78 DC, AB; 79 __m128 dA, dB, dC, dD; // determinant of the sub-matrices 80 __m128 det, d, d1, d2; 81 __m128 rd; // reciprocal of the determinant 82 83 // AB = A# * B 84 AB = _mm_mul_ps(_mm_shuffle_ps(A,A,0x0F), B); 85 AB = _mm_sub_ps(AB,_mm_mul_ps(_mm_shuffle_ps(A,A,0xA5), _mm_shuffle_ps(B,B,0x4E))); 86 // DC = D# * C 87 DC = _mm_mul_ps(_mm_shuffle_ps(D,D,0x0F), C); 88 DC = _mm_sub_ps(DC,_mm_mul_ps(_mm_shuffle_ps(D,D,0xA5), _mm_shuffle_ps(C,C,0x4E))); 89 90 // dA = |A| 91 dA = _mm_mul_ps(_mm_shuffle_ps(A, A, 0x5F),A); 92 dA = _mm_sub_ss(dA, _mm_movehl_ps(dA,dA)); 93 // dB = |B| 94 dB = _mm_mul_ps(_mm_shuffle_ps(B, B, 0x5F),B); 95 dB = _mm_sub_ss(dB, _mm_movehl_ps(dB,dB)); 96 97 // dC = |C| 98 dC = _mm_mul_ps(_mm_shuffle_ps(C, C, 0x5F),C); 99 dC = _mm_sub_ss(dC, _mm_movehl_ps(dC,dC)); 100 // dD = |D| 101 dD = _mm_mul_ps(_mm_shuffle_ps(D, D, 0x5F),D); 102 dD = _mm_sub_ss(dD, _mm_movehl_ps(dD,dD)); 103 104 // d = trace(AB*DC) = trace(A#*B*D#*C) 105 d = _mm_mul_ps(_mm_shuffle_ps(DC,DC,0xD8),AB); 106 107 // iD = C*A#*B 108 iD = _mm_mul_ps(_mm_shuffle_ps(C,C,0xA0), _mm_movelh_ps(AB,AB)); 109 iD = _mm_add_ps(iD,_mm_mul_ps(_mm_shuffle_ps(C,C,0xF5), _mm_movehl_ps(AB,AB))); 110 // iA = B*D#*C 111 iA = _mm_mul_ps(_mm_shuffle_ps(B,B,0xA0), _mm_movelh_ps(DC,DC)); 112 iA = _mm_add_ps(iA,_mm_mul_ps(_mm_shuffle_ps(B,B,0xF5), _mm_movehl_ps(DC,DC))); 113 114 // d = trace(AB*DC) = trace(A#*B*D#*C) [continue] 115 d = _mm_add_ps(d, _mm_movehl_ps(d, d)); 116 d = _mm_add_ss(d, _mm_shuffle_ps(d, d, 1)); 117 d1 = _mm_mul_ss(dA,dD); 118 d2 = _mm_mul_ss(dB,dC); 119 120 // iD = D*|A| - C*A#*B 121 iD = _mm_sub_ps(_mm_mul_ps(D,_mm_shuffle_ps(dA,dA,0)), iD); 122 123 // iA = A*|D| - B*D#*C; 124 iA = _mm_sub_ps(_mm_mul_ps(A,_mm_shuffle_ps(dD,dD,0)), iA); 125 126 // det = |A|*|D| + |B|*|C| - trace(A#*B*D#*C) 127 det = _mm_sub_ss(_mm_add_ss(d1,d2),d); 128 rd = _mm_div_ss(_mm_set_ss(1.0f), det); 129 130// #ifdef ZERO_SINGULAR 131// rd = _mm_and_ps(_mm_cmpneq_ss(det,_mm_setzero_ps()), rd); 132// #endif 133 134 // iB = D * (A#B)# = D*B#*A 135 iB = _mm_mul_ps(D, _mm_shuffle_ps(AB,AB,0x33)); 136 iB = _mm_sub_ps(iB, _mm_mul_ps(_mm_shuffle_ps(D,D,0xB1), _mm_shuffle_ps(AB,AB,0x66))); 137 // iC = A * (D#C)# = A*C#*D 138 iC = _mm_mul_ps(A, _mm_shuffle_ps(DC,DC,0x33)); 139 iC = _mm_sub_ps(iC, _mm_mul_ps(_mm_shuffle_ps(A,A,0xB1), _mm_shuffle_ps(DC,DC,0x66))); 140 141 rd = _mm_shuffle_ps(rd,rd,0); 142 rd = _mm_xor_ps(rd, _mm_load_ps((float*)_Sign_PNNP)); 143 144 // iB = C*|B| - D*B#*A 145 iB = _mm_sub_ps(_mm_mul_ps(C,_mm_shuffle_ps(dB,dB,0)), iB); 146 147 // iC = B*|C| - A*C#*D; 148 iC = _mm_sub_ps(_mm_mul_ps(B,_mm_shuffle_ps(dC,dC,0)), iC); 149 150 // iX = iX / det 151 iA = _mm_mul_ps(rd,iA); 152 iB = _mm_mul_ps(rd,iB); 153 iC = _mm_mul_ps(rd,iC); 154 iD = _mm_mul_ps(rd,iD); 155 156 Index res_stride = result.outerStride(); 157 float* res = result.data(); 158 pstoret<float, Packet4f, ResultAlignment>(res+0, _mm_shuffle_ps(iA,iB,0x77)); 159 pstoret<float, Packet4f, ResultAlignment>(res+res_stride, _mm_shuffle_ps(iA,iB,0x22)); 160 pstoret<float, Packet4f, ResultAlignment>(res+2*res_stride, _mm_shuffle_ps(iC,iD,0x77)); 161 pstoret<float, Packet4f, ResultAlignment>(res+3*res_stride, _mm_shuffle_ps(iC,iD,0x22)); 162 } 163 164}; 165 166template<typename MatrixType, typename ResultType> 167struct compute_inverse_size4<Architecture::SSE, double, MatrixType, ResultType> 168{ 169 enum { 170 MatrixAlignment = traits<MatrixType>::Alignment, 171 ResultAlignment = traits<ResultType>::Alignment, 172 StorageOrdersMatch = (MatrixType::Flags&RowMajorBit) == (ResultType::Flags&RowMajorBit) 173 }; 174 typedef typename conditional<(MatrixType::Flags&LinearAccessBit),MatrixType const &,typename MatrixType::PlainObject>::type ActualMatrixType; 175 176 static void run(const MatrixType& mat, ResultType& result) 177 { 178 ActualMatrixType matrix(mat); 179 const __m128d _Sign_NP = _mm_castsi128_pd(_mm_set_epi32(0x0,0x0,0x80000000,0x0)); 180 const __m128d _Sign_PN = _mm_castsi128_pd(_mm_set_epi32(0x80000000,0x0,0x0,0x0)); 181 182 // The inverse is calculated using "Divide and Conquer" technique. The 183 // original matrix is divide into four 2x2 sub-matrices. Since each 184 // register of the matrix holds two elements, the smaller matrices are 185 // consisted of two registers. Hence we get a better locality of the 186 // calculations. 187 188 // the four sub-matrices 189 __m128d A1, A2, B1, B2, C1, C2, D1, D2; 190 191 if(StorageOrdersMatch) 192 { 193 A1 = matrix.template packet<MatrixAlignment>( 0); B1 = matrix.template packet<MatrixAlignment>( 2); 194 A2 = matrix.template packet<MatrixAlignment>( 4); B2 = matrix.template packet<MatrixAlignment>( 6); 195 C1 = matrix.template packet<MatrixAlignment>( 8); D1 = matrix.template packet<MatrixAlignment>(10); 196 C2 = matrix.template packet<MatrixAlignment>(12); D2 = matrix.template packet<MatrixAlignment>(14); 197 } 198 else 199 { 200 __m128d tmp; 201 A1 = matrix.template packet<MatrixAlignment>( 0); C1 = matrix.template packet<MatrixAlignment>( 2); 202 A2 = matrix.template packet<MatrixAlignment>( 4); C2 = matrix.template packet<MatrixAlignment>( 6); 203 tmp = A1; 204 A1 = _mm_unpacklo_pd(A1,A2); 205 A2 = _mm_unpackhi_pd(tmp,A2); 206 tmp = C1; 207 C1 = _mm_unpacklo_pd(C1,C2); 208 C2 = _mm_unpackhi_pd(tmp,C2); 209 210 B1 = matrix.template packet<MatrixAlignment>( 8); D1 = matrix.template packet<MatrixAlignment>(10); 211 B2 = matrix.template packet<MatrixAlignment>(12); D2 = matrix.template packet<MatrixAlignment>(14); 212 tmp = B1; 213 B1 = _mm_unpacklo_pd(B1,B2); 214 B2 = _mm_unpackhi_pd(tmp,B2); 215 tmp = D1; 216 D1 = _mm_unpacklo_pd(D1,D2); 217 D2 = _mm_unpackhi_pd(tmp,D2); 218 } 219 220 __m128d iA1, iA2, iB1, iB2, iC1, iC2, iD1, iD2, // partial invese of the sub-matrices 221 DC1, DC2, AB1, AB2; 222 __m128d dA, dB, dC, dD; // determinant of the sub-matrices 223 __m128d det, d1, d2, rd; 224 225 // dA = |A| 226 dA = _mm_shuffle_pd(A2, A2, 1); 227 dA = _mm_mul_pd(A1, dA); 228 dA = _mm_sub_sd(dA, _mm_shuffle_pd(dA,dA,3)); 229 // dB = |B| 230 dB = _mm_shuffle_pd(B2, B2, 1); 231 dB = _mm_mul_pd(B1, dB); 232 dB = _mm_sub_sd(dB, _mm_shuffle_pd(dB,dB,3)); 233 234 // AB = A# * B 235 AB1 = _mm_mul_pd(B1, _mm_shuffle_pd(A2,A2,3)); 236 AB2 = _mm_mul_pd(B2, _mm_shuffle_pd(A1,A1,0)); 237 AB1 = _mm_sub_pd(AB1, _mm_mul_pd(B2, _mm_shuffle_pd(A1,A1,3))); 238 AB2 = _mm_sub_pd(AB2, _mm_mul_pd(B1, _mm_shuffle_pd(A2,A2,0))); 239 240 // dC = |C| 241 dC = _mm_shuffle_pd(C2, C2, 1); 242 dC = _mm_mul_pd(C1, dC); 243 dC = _mm_sub_sd(dC, _mm_shuffle_pd(dC,dC,3)); 244 // dD = |D| 245 dD = _mm_shuffle_pd(D2, D2, 1); 246 dD = _mm_mul_pd(D1, dD); 247 dD = _mm_sub_sd(dD, _mm_shuffle_pd(dD,dD,3)); 248 249 // DC = D# * C 250 DC1 = _mm_mul_pd(C1, _mm_shuffle_pd(D2,D2,3)); 251 DC2 = _mm_mul_pd(C2, _mm_shuffle_pd(D1,D1,0)); 252 DC1 = _mm_sub_pd(DC1, _mm_mul_pd(C2, _mm_shuffle_pd(D1,D1,3))); 253 DC2 = _mm_sub_pd(DC2, _mm_mul_pd(C1, _mm_shuffle_pd(D2,D2,0))); 254 255 // rd = trace(AB*DC) = trace(A#*B*D#*C) 256 d1 = _mm_mul_pd(AB1, _mm_shuffle_pd(DC1, DC2, 0)); 257 d2 = _mm_mul_pd(AB2, _mm_shuffle_pd(DC1, DC2, 3)); 258 rd = _mm_add_pd(d1, d2); 259 rd = _mm_add_sd(rd, _mm_shuffle_pd(rd, rd,3)); 260 261 // iD = C*A#*B 262 iD1 = _mm_mul_pd(AB1, _mm_shuffle_pd(C1,C1,0)); 263 iD2 = _mm_mul_pd(AB1, _mm_shuffle_pd(C2,C2,0)); 264 iD1 = _mm_add_pd(iD1, _mm_mul_pd(AB2, _mm_shuffle_pd(C1,C1,3))); 265 iD2 = _mm_add_pd(iD2, _mm_mul_pd(AB2, _mm_shuffle_pd(C2,C2,3))); 266 267 // iA = B*D#*C 268 iA1 = _mm_mul_pd(DC1, _mm_shuffle_pd(B1,B1,0)); 269 iA2 = _mm_mul_pd(DC1, _mm_shuffle_pd(B2,B2,0)); 270 iA1 = _mm_add_pd(iA1, _mm_mul_pd(DC2, _mm_shuffle_pd(B1,B1,3))); 271 iA2 = _mm_add_pd(iA2, _mm_mul_pd(DC2, _mm_shuffle_pd(B2,B2,3))); 272 273 // iD = D*|A| - C*A#*B 274 dA = _mm_shuffle_pd(dA,dA,0); 275 iD1 = _mm_sub_pd(_mm_mul_pd(D1, dA), iD1); 276 iD2 = _mm_sub_pd(_mm_mul_pd(D2, dA), iD2); 277 278 // iA = A*|D| - B*D#*C; 279 dD = _mm_shuffle_pd(dD,dD,0); 280 iA1 = _mm_sub_pd(_mm_mul_pd(A1, dD), iA1); 281 iA2 = _mm_sub_pd(_mm_mul_pd(A2, dD), iA2); 282 283 d1 = _mm_mul_sd(dA, dD); 284 d2 = _mm_mul_sd(dB, dC); 285 286 // iB = D * (A#B)# = D*B#*A 287 iB1 = _mm_mul_pd(D1, _mm_shuffle_pd(AB2,AB1,1)); 288 iB2 = _mm_mul_pd(D2, _mm_shuffle_pd(AB2,AB1,1)); 289 iB1 = _mm_sub_pd(iB1, _mm_mul_pd(_mm_shuffle_pd(D1,D1,1), _mm_shuffle_pd(AB2,AB1,2))); 290 iB2 = _mm_sub_pd(iB2, _mm_mul_pd(_mm_shuffle_pd(D2,D2,1), _mm_shuffle_pd(AB2,AB1,2))); 291 292 // det = |A|*|D| + |B|*|C| - trace(A#*B*D#*C) 293 det = _mm_add_sd(d1, d2); 294 det = _mm_sub_sd(det, rd); 295 296 // iC = A * (D#C)# = A*C#*D 297 iC1 = _mm_mul_pd(A1, _mm_shuffle_pd(DC2,DC1,1)); 298 iC2 = _mm_mul_pd(A2, _mm_shuffle_pd(DC2,DC1,1)); 299 iC1 = _mm_sub_pd(iC1, _mm_mul_pd(_mm_shuffle_pd(A1,A1,1), _mm_shuffle_pd(DC2,DC1,2))); 300 iC2 = _mm_sub_pd(iC2, _mm_mul_pd(_mm_shuffle_pd(A2,A2,1), _mm_shuffle_pd(DC2,DC1,2))); 301 302 rd = _mm_div_sd(_mm_set_sd(1.0), det); 303// #ifdef ZERO_SINGULAR 304// rd = _mm_and_pd(_mm_cmpneq_sd(det,_mm_setzero_pd()), rd); 305// #endif 306 rd = _mm_shuffle_pd(rd,rd,0); 307 308 // iB = C*|B| - D*B#*A 309 dB = _mm_shuffle_pd(dB,dB,0); 310 iB1 = _mm_sub_pd(_mm_mul_pd(C1, dB), iB1); 311 iB2 = _mm_sub_pd(_mm_mul_pd(C2, dB), iB2); 312 313 d1 = _mm_xor_pd(rd, _Sign_PN); 314 d2 = _mm_xor_pd(rd, _Sign_NP); 315 316 // iC = B*|C| - A*C#*D; 317 dC = _mm_shuffle_pd(dC,dC,0); 318 iC1 = _mm_sub_pd(_mm_mul_pd(B1, dC), iC1); 319 iC2 = _mm_sub_pd(_mm_mul_pd(B2, dC), iC2); 320 321 Index res_stride = result.outerStride(); 322 double* res = result.data(); 323 pstoret<double, Packet2d, ResultAlignment>(res+0, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 3), d1)); 324 pstoret<double, Packet2d, ResultAlignment>(res+res_stride, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 0), d2)); 325 pstoret<double, Packet2d, ResultAlignment>(res+2, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 3), d1)); 326 pstoret<double, Packet2d, ResultAlignment>(res+res_stride+2, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 0), d2)); 327 pstoret<double, Packet2d, ResultAlignment>(res+2*res_stride, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 3), d1)); 328 pstoret<double, Packet2d, ResultAlignment>(res+3*res_stride, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 0), d2)); 329 pstoret<double, Packet2d, ResultAlignment>(res+2*res_stride+2,_mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 3), d1)); 330 pstoret<double, Packet2d, ResultAlignment>(res+3*res_stride+2,_mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 0), d2)); 331 } 332}; 333 334} // end namespace internal 335 336} // end namespace Eigen 337 338#endif // EIGEN_INVERSE_SSE_H 339