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 */
36inline 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 */
44inline 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 */
52inline 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 */
61inline 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 */
70inline 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
80part of A is used from the input. The Cholesky factor is output as
81subdiagonal part in A and diagonal in d, which is 6-dimensional
821.9 microseconds on 450MHz*/
83DB_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
88of a 6 x 6 matrix and the right hand side b. The vector b is unchanged
891.3 microseconds on 450MHz*/
90DB_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
95above diagonal of A is used from the input, diagonal of A is assumed to
96be stored in d. The Cholesky factor is output as
97subdiagonal part in A and diagonal in d, which is n-dimensional*/
98DB_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
103of an n x n matrix and the right hand side b. The vector b is unchanged*/
104DB_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
109above diagonal of A is used from the input, diagonal of A is assumed to
110be stored in d. The Cholesky factor is output as subdiagonal part in A
111and diagonal in d, which is 3-dimensional*/
112DB_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
117of a 3 x 3 matrix and the right hand side b. The vector b is unchanged*/
118DB_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*/
123inline 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 */
131inline 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 */
140inline 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 */
150inline 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 */
162inline 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 */
174DB_API void db_Orthogonalize6x7(double A[42],int orthonormalize=0);
175
176/*!
177 \ingroup LMLinAlg
178 */
179DB_API void db_Orthogonalize8x9(double A[72],int orthonormalize=0);
180
181/*!
182 \ingroup LMLinAlg
183 */
184inline 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 */
198inline 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 */
212inline 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 */
222inline 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 */
233DB_API void db_NullVectorOrthonormal6x7(double x[7],const double A[42]);
234/*!
235 \ingroup LMLinAlg
236 */
237DB_API void db_NullVectorOrthonormal8x9(double x[9],const double A[72]);
238
239/*!
240 \ingroup LMLinAlg
241 */
242inline 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 */
251inline void db_NullVector8x9Destructive(double x[9],double A[72])
252{
253    db_Orthogonalize8x9(A,1);
254    db_NullVectorOrthonormal8x9(x,A);
255}
256
257inline 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
270inline 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 */
292inline 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 */
597inline 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