• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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