1 /*
2 * Copyright (C) 2011 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 /* $Id: db_utilities_linalg.h,v 1.5 2011/06/17 14:03:31 mbansal Exp $ */
18
19 #ifndef DB_UTILITIES_LINALG
20 #define DB_UTILITIES_LINALG
21
22 #include "db_utilities.h"
23
24
25
26 /*****************************************************************
27 * Lean and mean begins here *
28 *****************************************************************/
29 /*!
30 * \defgroup LMLinAlg (LM) Linear Algebra Utilities (QR factorization, orthogonal basis, etc.)
31 */
32
33 /*!
34 \ingroup LMBasicUtilities
35 */
db_MultiplyScalar6(double A[6],double mult)36 inline void db_MultiplyScalar6(double A[6],double mult)
37 {
38 (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult;
39 (*A++) *= mult;
40 }
41 /*!
42 \ingroup LMBasicUtilities
43 */
db_MultiplyScalar7(double A[7],double mult)44 inline void db_MultiplyScalar7(double A[7],double mult)
45 {
46 (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult;
47 (*A++) *= mult; (*A++) *= mult;
48 }
49 /*!
50 \ingroup LMBasicUtilities
51 */
db_MultiplyScalar9(double A[9],double mult)52 inline void db_MultiplyScalar9(double A[9],double mult)
53 {
54 (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult;
55 (*A++) *= mult; (*A++) *= mult; (*A++) *= mult; (*A++) *= mult;
56 }
57
58 /*!
59 \ingroup LMBasicUtilities
60 */
db_SquareSum6Stride7(const double * x)61 inline double db_SquareSum6Stride7(const double *x)
62 {
63 return(db_sqr(x[0])+db_sqr(x[7])+db_sqr(x[14])+
64 db_sqr(x[21])+db_sqr(x[28])+db_sqr(x[35]));
65 }
66
67 /*!
68 \ingroup LMBasicUtilities
69 */
db_SquareSum8Stride9(const double * x)70 inline double db_SquareSum8Stride9(const double *x)
71 {
72 return(db_sqr(x[0])+db_sqr(x[9])+db_sqr(x[18])+
73 db_sqr(x[27])+db_sqr(x[36])+db_sqr(x[45])+
74 db_sqr(x[54])+db_sqr(x[63]));
75 }
76
77 /*!
78 \ingroup LMLinAlg
79 Cholesky-factorize symmetric positive definite 6 x 6 matrix A. Upper
80 part of A is used from the input. The Cholesky factor is output as
81 subdiagonal part in A and diagonal in d, which is 6-dimensional
82 1.9 microseconds on 450MHz*/
83 DB_API void db_CholeskyDecomp6x6(double A[36],double d[6]);
84
85 /*!
86 \ingroup LMLinAlg
87 Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition
88 of a 6 x 6 matrix and the right hand side b. The vector b is unchanged
89 1.3 microseconds on 450MHz*/
90 DB_API void db_CholeskyBacksub6x6(double x[6],const double A[36],const double d[6],const double b[6]);
91
92 /*!
93 \ingroup LMLinAlg
94 Cholesky-factorize symmetric positive definite n x n matrix A.Part
95 above diagonal of A is used from the input, diagonal of A is assumed to
96 be stored in d. The Cholesky factor is output as
97 subdiagonal part in A and diagonal in d, which is n-dimensional*/
98 DB_API void db_CholeskyDecompSeparateDiagonal(double **A,double *d,int n);
99
100 /*!
101 \ingroup LMLinAlg
102 Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition
103 of an n x n matrix and the right hand side b. The vector b is unchanged*/
104 DB_API void db_CholeskyBacksub(double *x,const double * const *A,const double *d,int n,const double *b);
105
106 /*!
107 \ingroup LMLinAlg
108 Cholesky-factorize symmetric positive definite 3 x 3 matrix A. Part
109 above diagonal of A is used from the input, diagonal of A is assumed to
110 be stored in d. The Cholesky factor is output as subdiagonal part in A
111 and diagonal in d, which is 3-dimensional*/
112 DB_API void db_CholeskyDecomp3x3SeparateDiagonal(double A[9],double d[3]);
113
114 /*!
115 \ingroup LMLinAlg
116 Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition
117 of a 3 x 3 matrix and the right hand side b. The vector b is unchanged*/
118 DB_API void db_CholeskyBacksub3x3(double x[3],const double A[9],const double d[3],const double b[3]);
119
120 /*!
121 \ingroup LMLinAlg
122 perform A-=B*mult*/
db_RowOperation3(double A[3],const double B[3],double mult)123 inline void db_RowOperation3(double A[3],const double B[3],double mult)
124 {
125 *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++);
126 }
127
128 /*!
129 \ingroup LMLinAlg
130 */
db_RowOperation7(double A[7],const double B[7],double mult)131 inline void db_RowOperation7(double A[7],const double B[7],double mult)
132 {
133 *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++);
134 *A++ -= mult*(*B++); *A++ -= mult*(*B++);
135 }
136
137 /*!
138 \ingroup LMLinAlg
139 */
db_RowOperation9(double A[9],const double B[9],double mult)140 inline void db_RowOperation9(double A[9],const double B[9],double mult)
141 {
142 *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++);
143 *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++); *A++ -= mult*(*B++);
144 }
145
146 /*!
147 \ingroup LMBasicUtilities
148 Swap values of A[7] and B[7]
149 */
db_Swap7(double A[7],double B[7])150 inline void db_Swap7(double A[7],double B[7])
151 {
152 double temp;
153 temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp;
154 temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp;
155 temp= *A; *A++ = *B; *B++ =temp;
156 }
157
158 /*!
159 \ingroup LMBasicUtilities
160 Swap values of A[9] and B[9]
161 */
db_Swap9(double A[9],double B[9])162 inline void db_Swap9(double A[9],double B[9])
163 {
164 double temp;
165 temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp;
166 temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp;
167 temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp; temp= *A; *A++ = *B; *B++ =temp;
168 }
169
170
171 /*!
172 \ingroup LMLinAlg
173 */
174 DB_API void db_Orthogonalize6x7(double A[42],int orthonormalize=0);
175
176 /*!
177 \ingroup LMLinAlg
178 */
179 DB_API void db_Orthogonalize8x9(double A[72],int orthonormalize=0);
180
181 /*!
182 \ingroup LMLinAlg
183 */
db_OrthogonalizePair7(double * x,const double * v,double ssv)184 inline double db_OrthogonalizePair7(double *x,const double *v,double ssv)
185 {
186 double m,sp,sp_m;
187
188 m=db_SafeReciprocal(ssv);
189 sp=db_ScalarProduct7(x,v);
190 sp_m=sp*m;
191 db_RowOperation7(x,v,sp_m);
192 return(sp*sp_m);
193 }
194
195 /*!
196 \ingroup LMLinAlg
197 */
db_OrthogonalizePair9(double * x,const double * v,double ssv)198 inline double db_OrthogonalizePair9(double *x,const double *v,double ssv)
199 {
200 double m,sp,sp_m;
201
202 m=db_SafeReciprocal(ssv);
203 sp=db_ScalarProduct9(x,v);
204 sp_m=sp*m;
205 db_RowOperation9(x,v,sp_m);
206 return(sp*sp_m);
207 }
208
209 /*!
210 \ingroup LMLinAlg
211 */
db_OrthogonalizationSwap7(double * A,int i,double * ss)212 inline void db_OrthogonalizationSwap7(double *A,int i,double *ss)
213 {
214 double temp;
215
216 db_Swap7(A,A+7*i);
217 temp=ss[0]; ss[0]=ss[i]; ss[i]=temp;
218 }
219 /*!
220 \ingroup LMLinAlg
221 */
db_OrthogonalizationSwap9(double * A,int i,double * ss)222 inline void db_OrthogonalizationSwap9(double *A,int i,double *ss)
223 {
224 double temp;
225
226 db_Swap9(A,A+9*i);
227 temp=ss[0]; ss[0]=ss[i]; ss[i]=temp;
228 }
229
230 /*!
231 \ingroup LMLinAlg
232 */
233 DB_API void db_NullVectorOrthonormal6x7(double x[7],const double A[42]);
234 /*!
235 \ingroup LMLinAlg
236 */
237 DB_API void db_NullVectorOrthonormal8x9(double x[9],const double A[72]);
238
239 /*!
240 \ingroup LMLinAlg
241 */
db_NullVector6x7Destructive(double x[7],double A[42])242 inline void db_NullVector6x7Destructive(double x[7],double A[42])
243 {
244 db_Orthogonalize6x7(A,1);
245 db_NullVectorOrthonormal6x7(x,A);
246 }
247
248 /*!
249 \ingroup LMLinAlg
250 */
db_NullVector8x9Destructive(double x[9],double A[72])251 inline void db_NullVector8x9Destructive(double x[9],double A[72])
252 {
253 db_Orthogonalize8x9(A,1);
254 db_NullVectorOrthonormal8x9(x,A);
255 }
256
db_ScalarProduct512_s(const short * f,const short * g)257 inline int db_ScalarProduct512_s(const short *f,const short *g)
258 {
259 #ifndef DB_USE_MMX
260 int back;
261 back=0;
262 for(int i=1; i<=512; i++)
263 back+=(*f++)*(*g++);
264
265 return(back);
266 #endif
267 }
268
269
db_ScalarProduct32_s(const short * f,const short * g)270 inline int db_ScalarProduct32_s(const short *f,const short *g)
271 {
272 #ifndef DB_USE_MMX
273 int back;
274 back=0;
275 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
276 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
277 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
278 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
279 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
280 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
281 back+=(*f++)*(*g++); back+=(*f++)*(*g++);
282
283 return(back);
284 #endif
285 }
286
287 /*!
288 \ingroup LMLinAlg
289 Scalar product of 128-vectors (short)
290 Compile-time control: MMX, SSE2 or standard C
291 */
db_ScalarProduct128_s(const short * f,const short * g)292 inline int db_ScalarProduct128_s(const short *f,const short *g)
293 {
294 #ifndef DB_USE_MMX
295 int back;
296 back=0;
297 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
298 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
299 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
300 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
301 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
302 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
303 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
304 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
305 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
306 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
307
308 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
309 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
310 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
311 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
312 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
313 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
314 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
315 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
316 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
317 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
318
319 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
320 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
321 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
322 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
323 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
324 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
325
326 return(back);
327 #else
328 #ifdef DB_USE_SSE2
329 int back;
330
331 _asm
332 {
333 mov eax,f
334 mov ecx,g
335 /*First iteration************************************/
336 movdqa xmm0,[eax]
337 pxor xmm7,xmm7 /*Set xmm7 to zero*/
338 pmaddwd xmm0,[ecx]
339 /*Stall*/
340 /*Standard iteration************************************/
341 movdqa xmm2,[eax+16]
342 paddd xmm7,xmm0
343 pmaddwd xmm2,[ecx+16]
344 /*Stall*/
345 movdqa xmm1,[eax+32]
346 paddd xmm7,xmm2
347 pmaddwd xmm1,[ecx+32]
348 /*Stall*/
349 /*Standard iteration************************************/
350 movdqa xmm0,[eax+48]
351 paddd xmm7,xmm1
352 pmaddwd xmm0,[ecx+48]
353 /*Stall*/
354 /*Standard iteration************************************/
355 movdqa xmm2,[eax+64]
356 paddd xmm7,xmm0
357 pmaddwd xmm2,[ecx+64]
358 /*Stall*/
359 movdqa xmm1,[eax+80]
360 paddd xmm7,xmm2
361 pmaddwd xmm1,[ecx+80]
362 /*Stall*/
363 /*Standard iteration************************************/
364 movdqa xmm0,[eax+96]
365 paddd xmm7,xmm1
366 pmaddwd xmm0,[ecx+96]
367 /*Stall*/
368 /*Standard iteration************************************/
369 movdqa xmm2,[eax+112]
370 paddd xmm7,xmm0
371 pmaddwd xmm2,[ecx+112]
372 /*Stall*/
373 movdqa xmm1,[eax+128]
374 paddd xmm7,xmm2
375 pmaddwd xmm1,[ecx+128]
376 /*Stall*/
377 /*Standard iteration************************************/
378 movdqa xmm0,[eax+144]
379 paddd xmm7,xmm1
380 pmaddwd xmm0,[ecx+144]
381 /*Stall*/
382 /*Standard iteration************************************/
383 movdqa xmm2,[eax+160]
384 paddd xmm7,xmm0
385 pmaddwd xmm2,[ecx+160]
386 /*Stall*/
387 movdqa xmm1,[eax+176]
388 paddd xmm7,xmm2
389 pmaddwd xmm1,[ecx+176]
390 /*Stall*/
391 /*Standard iteration************************************/
392 movdqa xmm0,[eax+192]
393 paddd xmm7,xmm1
394 pmaddwd xmm0,[ecx+192]
395 /*Stall*/
396 /*Standard iteration************************************/
397 movdqa xmm2,[eax+208]
398 paddd xmm7,xmm0
399 pmaddwd xmm2,[ecx+208]
400 /*Stall*/
401 movdqa xmm1,[eax+224]
402 paddd xmm7,xmm2
403 pmaddwd xmm1,[ecx+224]
404 /*Stall*/
405 /*Standard iteration************************************/
406 movdqa xmm0,[eax+240]
407 paddd xmm7,xmm1
408 pmaddwd xmm0,[ecx+240]
409 /*Stall*/
410 /*Rest iteration************************************/
411 paddd xmm7,xmm0
412
413 /* add up the sum squares */
414 movhlps xmm0,xmm7 /* high half to low half */
415 paddd xmm7,xmm0 /* add high to low */
416 pshuflw xmm0,xmm7, 0xE /* reshuffle */
417 paddd xmm7,xmm0 /* add remaining */
418 movd back,xmm7
419
420 emms
421 }
422
423 return(back);
424 #else
425 int back;
426
427 _asm
428 {
429 mov eax,f
430 mov ecx,g
431 /*First iteration************************************/
432 movq mm0,[eax]
433 pxor mm7,mm7 /*Set mm7 to zero*/
434 pmaddwd mm0,[ecx]
435 /*Stall*/
436 movq mm1,[eax+8]
437 /*Stall*/
438 pmaddwd mm1,[ecx+8]
439 /*Stall*/
440 /*Standard iteration************************************/
441 movq mm2,[eax+16]
442 paddd mm7,mm0
443 pmaddwd mm2,[ecx+16]
444 /*Stall*/
445 movq mm0,[eax+24]
446 paddd mm7,mm1
447 pmaddwd mm0,[ecx+24]
448 /*Stall*/
449 movq mm1,[eax+32]
450 paddd mm7,mm2
451 pmaddwd mm1,[ecx+32]
452 /*Stall*/
453 /*Standard iteration************************************/
454 movq mm2,[eax+40]
455 paddd mm7,mm0
456 pmaddwd mm2,[ecx+40]
457 /*Stall*/
458 movq mm0,[eax+48]
459 paddd mm7,mm1
460 pmaddwd mm0,[ecx+48]
461 /*Stall*/
462 movq mm1,[eax+56]
463 paddd mm7,mm2
464 pmaddwd mm1,[ecx+56]
465 /*Stall*/
466 /*Standard iteration************************************/
467 movq mm2,[eax+64]
468 paddd mm7,mm0
469 pmaddwd mm2,[ecx+64]
470 /*Stall*/
471 movq mm0,[eax+72]
472 paddd mm7,mm1
473 pmaddwd mm0,[ecx+72]
474 /*Stall*/
475 movq mm1,[eax+80]
476 paddd mm7,mm2
477 pmaddwd mm1,[ecx+80]
478 /*Stall*/
479 /*Standard iteration************************************/
480 movq mm2,[eax+88]
481 paddd mm7,mm0
482 pmaddwd mm2,[ecx+88]
483 /*Stall*/
484 movq mm0,[eax+96]
485 paddd mm7,mm1
486 pmaddwd mm0,[ecx+96]
487 /*Stall*/
488 movq mm1,[eax+104]
489 paddd mm7,mm2
490 pmaddwd mm1,[ecx+104]
491 /*Stall*/
492 /*Standard iteration************************************/
493 movq mm2,[eax+112]
494 paddd mm7,mm0
495 pmaddwd mm2,[ecx+112]
496 /*Stall*/
497 movq mm0,[eax+120]
498 paddd mm7,mm1
499 pmaddwd mm0,[ecx+120]
500 /*Stall*/
501 movq mm1,[eax+128]
502 paddd mm7,mm2
503 pmaddwd mm1,[ecx+128]
504 /*Stall*/
505 /*Standard iteration************************************/
506 movq mm2,[eax+136]
507 paddd mm7,mm0
508 pmaddwd mm2,[ecx+136]
509 /*Stall*/
510 movq mm0,[eax+144]
511 paddd mm7,mm1
512 pmaddwd mm0,[ecx+144]
513 /*Stall*/
514 movq mm1,[eax+152]
515 paddd mm7,mm2
516 pmaddwd mm1,[ecx+152]
517 /*Stall*/
518 /*Standard iteration************************************/
519 movq mm2,[eax+160]
520 paddd mm7,mm0
521 pmaddwd mm2,[ecx+160]
522 /*Stall*/
523 movq mm0,[eax+168]
524 paddd mm7,mm1
525 pmaddwd mm0,[ecx+168]
526 /*Stall*/
527 movq mm1,[eax+176]
528 paddd mm7,mm2
529 pmaddwd mm1,[ecx+176]
530 /*Stall*/
531 /*Standard iteration************************************/
532 movq mm2,[eax+184]
533 paddd mm7,mm0
534 pmaddwd mm2,[ecx+184]
535 /*Stall*/
536 movq mm0,[eax+192]
537 paddd mm7,mm1
538 pmaddwd mm0,[ecx+192]
539 /*Stall*/
540 movq mm1,[eax+200]
541 paddd mm7,mm2
542 pmaddwd mm1,[ecx+200]
543 /*Stall*/
544 /*Standard iteration************************************/
545 movq mm2,[eax+208]
546 paddd mm7,mm0
547 pmaddwd mm2,[ecx+208]
548 /*Stall*/
549 movq mm0,[eax+216]
550 paddd mm7,mm1
551 pmaddwd mm0,[ecx+216]
552 /*Stall*/
553 movq mm1,[eax+224]
554 paddd mm7,mm2
555 pmaddwd mm1,[ecx+224]
556 /*Stall*/
557 /*Standard iteration************************************/
558 movq mm2,[eax+232]
559 paddd mm7,mm0
560 pmaddwd mm2,[ecx+232]
561 /*Stall*/
562 movq mm0,[eax+240]
563 paddd mm7,mm1
564 pmaddwd mm0,[ecx+240]
565 /*Stall*/
566 movq mm1,[eax+248]
567 paddd mm7,mm2
568 pmaddwd mm1,[ecx+248]
569 /*Stall*/
570 /*Rest iteration************************************/
571 paddd mm7,mm0
572 /*Stall*/
573 /*Stall*/
574 /*Stall*/
575 paddd mm7,mm1
576 /*Stall*/
577 movq mm0,mm7
578 psrlq mm7,32
579 paddd mm0,mm7
580 /*Stall*/
581 /*Stall*/
582 /*Stall*/
583 movd back,mm0
584 emms
585 }
586
587 return(back);
588 #endif
589 #endif /*DB_USE_MMX*/
590 }
591
592 /*!
593 \ingroup LMLinAlg
594 Scalar product of 16 byte aligned 128-vectors (float).
595 Compile-time control: SIMD (SSE) or standard C.
596 */
db_ScalarProduct128Aligned16_f(const float * f,const float * g)597 inline float db_ScalarProduct128Aligned16_f(const float *f,const float *g)
598 {
599 #ifndef DB_USE_SIMD
600 float back;
601 back=0.0;
602 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
603 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
604 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
605 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
606 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
607 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
608 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
609 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
610 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
611 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
612
613 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
614 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
615 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
616 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
617 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
618 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
619 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
620 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
621 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
622 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
623
624 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
625 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
626 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
627 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
628 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
629 back+=(*f++)*(*g++); back+=(*f++)*(*g++); back+=(*f++)*(*g++);
630
631 return(back);
632 #else
633 float back;
634
635 _asm
636 {
637 mov eax,f
638 mov ecx,g
639 /*First iteration************************************/
640 movaps xmm0,[eax]
641 xorps xmm7,xmm7 /*Set mm7 to zero*/
642 mulps xmm0,[ecx]
643 /*Stall*/
644 movaps xmm1,[eax+16]
645 /*Stall*/
646 mulps xmm1,[ecx+16]
647 /*Stall*/
648 /*Standard iteration************************************/
649 movaps xmm2,[eax+32]
650 addps xmm7,xmm0
651 mulps xmm2,[ecx+32]
652 /*Stall*/
653 movaps xmm0,[eax+48]
654 addps xmm7,xmm1
655 mulps xmm0,[ecx+48]
656 /*Stall*/
657 movaps xmm1,[eax+64]
658 addps xmm7,xmm2
659 mulps xmm1,[ecx+64]
660 /*Stall*/
661 /*Standard iteration************************************/
662 movaps xmm2,[eax+80]
663 addps xmm7,xmm0
664 mulps xmm2,[ecx+80]
665 /*Stall*/
666 movaps xmm0,[eax+96]
667 addps xmm7,xmm1
668 mulps xmm0,[ecx+96]
669 /*Stall*/
670 movaps xmm1,[eax+112]
671 addps xmm7,xmm2
672 mulps xmm1,[ecx+112]
673 /*Stall*/
674 /*Standard iteration************************************/
675 movaps xmm2,[eax+128]
676 addps xmm7,xmm0
677 mulps xmm2,[ecx+128]
678 /*Stall*/
679 movaps xmm0,[eax+144]
680 addps xmm7,xmm1
681 mulps xmm0,[ecx+144]
682 /*Stall*/
683 movaps xmm1,[eax+160]
684 addps xmm7,xmm2
685 mulps xmm1,[ecx+160]
686 /*Stall*/
687 /*Standard iteration************************************/
688 movaps xmm2,[eax+176]
689 addps xmm7,xmm0
690 mulps xmm2,[ecx+176]
691 /*Stall*/
692 movaps xmm0,[eax+192]
693 addps xmm7,xmm1
694 mulps xmm0,[ecx+192]
695 /*Stall*/
696 movaps xmm1,[eax+208]
697 addps xmm7,xmm2
698 mulps xmm1,[ecx+208]
699 /*Stall*/
700 /*Standard iteration************************************/
701 movaps xmm2,[eax+224]
702 addps xmm7,xmm0
703 mulps xmm2,[ecx+224]
704 /*Stall*/
705 movaps xmm0,[eax+240]
706 addps xmm7,xmm1
707 mulps xmm0,[ecx+240]
708 /*Stall*/
709 movaps xmm1,[eax+256]
710 addps xmm7,xmm2
711 mulps xmm1,[ecx+256]
712 /*Stall*/
713 /*Standard iteration************************************/
714 movaps xmm2,[eax+272]
715 addps xmm7,xmm0
716 mulps xmm2,[ecx+272]
717 /*Stall*/
718 movaps xmm0,[eax+288]
719 addps xmm7,xmm1
720 mulps xmm0,[ecx+288]
721 /*Stall*/
722 movaps xmm1,[eax+304]
723 addps xmm7,xmm2
724 mulps xmm1,[ecx+304]
725 /*Stall*/
726 /*Standard iteration************************************/
727 movaps xmm2,[eax+320]
728 addps xmm7,xmm0
729 mulps xmm2,[ecx+320]
730 /*Stall*/
731 movaps xmm0,[eax+336]
732 addps xmm7,xmm1
733 mulps xmm0,[ecx+336]
734 /*Stall*/
735 movaps xmm1,[eax+352]
736 addps xmm7,xmm2
737 mulps xmm1,[ecx+352]
738 /*Stall*/
739 /*Standard iteration************************************/
740 movaps xmm2,[eax+368]
741 addps xmm7,xmm0
742 mulps xmm2,[ecx+368]
743 /*Stall*/
744 movaps xmm0,[eax+384]
745 addps xmm7,xmm1
746 mulps xmm0,[ecx+384]
747 /*Stall*/
748 movaps xmm1,[eax+400]
749 addps xmm7,xmm2
750 mulps xmm1,[ecx+400]
751 /*Stall*/
752 /*Standard iteration************************************/
753 movaps xmm2,[eax+416]
754 addps xmm7,xmm0
755 mulps xmm2,[ecx+416]
756 /*Stall*/
757 movaps xmm0,[eax+432]
758 addps xmm7,xmm1
759 mulps xmm0,[ecx+432]
760 /*Stall*/
761 movaps xmm1,[eax+448]
762 addps xmm7,xmm2
763 mulps xmm1,[ecx+448]
764 /*Stall*/
765 /*Standard iteration************************************/
766 movaps xmm2,[eax+464]
767 addps xmm7,xmm0
768 mulps xmm2,[ecx+464]
769 /*Stall*/
770 movaps xmm0,[eax+480]
771 addps xmm7,xmm1
772 mulps xmm0,[ecx+480]
773 /*Stall*/
774 movaps xmm1,[eax+496]
775 addps xmm7,xmm2
776 mulps xmm1,[ecx+496]
777 /*Stall*/
778 /*Rest iteration************************************/
779 addps xmm7,xmm0
780 /*Stall*/
781 addps xmm7,xmm1
782 /*Stall*/
783 movaps xmm6,xmm7
784 /*Stall*/
785 shufps xmm6,xmm6,4Eh
786 /*Stall*/
787 addps xmm7,xmm6
788 /*Stall*/
789 movaps xmm6,xmm7
790 /*Stall*/
791 shufps xmm6,xmm6,11h
792 /*Stall*/
793 addps xmm7,xmm6
794 /*Stall*/
795 movss back,xmm7
796 }
797
798 return(back);
799 #endif /*DB_USE_SIMD*/
800 }
801
802 #endif /* DB_UTILITIES_LINALG */
803