00001
00007 template<class S>
00008 void scalar_product(const vector<S> &_x, const vector<S> &_y, S &_s)
00009 {
00010 S s = 0.0;
00011 const S* x = &_x[0], *y = &_y[0];
00012 int m = (int)_x.size();
00013 for(int i = 0; i < m; i++)
00014 {
00015 s += *x++ * *y++;
00016 }
00017 _s = s;
00018 }
00025 template<class S>
00026 void scalar_product(const vector<S> &_x, const vector<S> &_y, vector<S> &_s)
00027 {
00028 S s0, s1, s2, s3;
00029 const S* x = &_x[0], *y = &_y[0];
00030 S* s = &_s[0];
00031 int m = (int)_x.size(), n = (int)_s.size();
00032 switch(n)
00033 {
00034 case 0:
00035 break;
00036 case 1:
00037 s0 = 0.0;
00038 for(int i = 0; i < m; i++)
00039 {
00040 s0 += x[i] * y[i];
00041 }
00042 s[0] = s0;
00043 break;
00044 case 2:
00045 s0 = 0.0, s1 = 0.0;
00046 for(int i = 0; i < m; i += 2)
00047 {
00048 s0 += x[i + 0] * y[i + 0];
00049 s1 += x[i + 1] * y[i + 1];
00050 }
00051 s[0] = s0, s[1] = s1;
00052 break;
00053 case 3:
00054 s0 = 0.0, s1 = 0.0, s2 = 0.0;
00055 for(int i = 0; i < m; i += 3)
00056 {
00057 s0 += x[i + 0] * y[i + 0];
00058 s1 += x[i + 1] * y[i + 1];
00059 s2 += x[i + 2] * y[i + 2];
00060 }
00061 s[0] = s0, s[1] = s1, s[2] = s2;
00062 break;
00063 case 4:
00064 s0 = 0.0, s1 = 0.0, s2 = 0.0, s3 = 0.0;
00065 for(int i = 0; i < m; i += 4)
00066 {
00067 s0 += x[i + 0] * y[i + 0];
00068 s1 += x[i + 1] * y[i + 1];
00069 s2 += x[i + 2] * y[i + 2];
00070 s3 += x[i + 3] * y[i + 3];
00071 }
00072 s[0] = s0, s[1] = s1, s[2] = s2, s[3] = s3;
00073 break;
00074 default:
00075 memset(&_s[0], 0, sizeof(S) * _s.size());
00076 for(int i = 0; i < m; i += n)
00077 {
00078 for(int j = 0; j < n; j++)
00079 {
00080 s[j] += x[i + j] * y[i + j];
00081 }
00082 }
00083 }
00084 }
00091 template<class S>
00092 void add_scale(vector<S> &_x, const vector<S> &_y, const S _s)
00093 {
00094 S *x = &_x[0];
00095 const S *y = &_y[0];
00096 const S s = _s;
00097 for(int i = 0; i < (int)_x.size(); i++)
00098 {
00099 *x++ += *y++ * s;
00100 }
00101 }
00108 template<class S>
00109 void add_scale(vector<S> &_x, const vector<S> &_y, const vector<S> &_s)
00110 {
00111 S s0, s1, s2, s3;
00112 S *x = &_x[0];
00113 const S *y = &_y[0], *s = &_s[0];
00114 int m = (int)_x.size(), n = (int)_s.size();
00115 switch(n)
00116 {
00117 case 0:
00118 break;
00119 case 1:
00120 s0 = s[0];
00121 for(int i = 0; i < m; i++)
00122 {
00123 x[i] += y[i] * s0;
00124 }
00125 break;
00126 case 2:
00127 s0 = s[0], s1 = s[1];
00128 for(int i = 0; i < m; i += 2)
00129 {
00130 x[i + 0] += y[i + 0] * s0;
00131 x[i + 1] += y[i + 1] * s1;
00132 }
00133 break;
00134 case 3:
00135 s0 = s[0], s1 = s[1], s2 = s[2];
00136 for(int i = 0; i < m; i += 3)
00137 {
00138 x[i + 0] += y[i + 0] * s0;
00139 x[i + 1] += y[i + 1] * s1;
00140 x[i + 2] += y[i + 2] * s2;
00141 }
00142 break;
00143 case 4:
00144 s0 = s[0], s1 = s[1], s2 = s[2], s3 = s[3];
00145 for(int i = 0; i < m; i += 4)
00146 {
00147 x[i + 0] += y[i + 0] * s0;
00148 x[i + 1] += y[i + 1] * s1;
00149 x[i + 2] += y[i + 2] * s2;
00150 x[i + 3] += y[i + 3] * s3;
00151 }
00152 break;
00153 default:
00154 for(int i = 0; i < m; i += n)
00155 {
00156 for(int j = 0; j < n; j++)
00157 {
00158 x[i + j] += y[i + j] * s[j];
00159 }
00160 }
00161 }
00162 }
00169 template<class S>
00170 void sub_scale(vector<S> &_x, const vector<S> &_y, const S _s)
00171 {
00172 S *x = &_x[0];
00173 const S *y = &_y[0];
00174 const S s = _s;
00175 for(int i = 0; i < (int)_x.size(); i++)
00176 {
00177 *x++ -= *y++ * s;
00178 }
00179 }
00186 template<class S>
00187 void sub_scale(vector<S> &_x, const vector<S> &_y, const vector<S> &_s)
00188 {
00189 S s0, s1, s2, s3;
00190 S *x = &_x[0];
00191 const S *y = &_y[0], *s = &_s[0];
00192 int m = (int)_x.size(), n = (int)_s.size();
00193 switch(n)
00194 {
00195 case 0:
00196 break;
00197 case 1:
00198 s0 = s[0];
00199 for(int i = 0; i < m; i++)
00200 {
00201 x[i] -= y[i] * s0;
00202 }
00203 break;
00204 case 2:
00205 s0 = s[0], s1 = s[1];
00206 for(int i = 0; i < m; i += 2)
00207 {
00208 x[i + 0] -= y[i + 0] * s0;
00209 x[i + 1] -= y[i + 1] * s1;
00210 }
00211 break;
00212 case 3:
00213 s0 = s[0], s1 = s[1], s2 = s[2];
00214 for(int i = 0; i < m; i += 3)
00215 {
00216 x[i + 0] -= y[i + 0] * s0;
00217 x[i + 1] -= y[i + 1] * s1;
00218 x[i + 2] -= y[i + 2] * s2;
00219 }
00220 break;
00221 case 4:
00222 s0 = s[0], s1 = s[1], s2 = s[2], s3 = s[3];
00223 for(int i = 0; i < m; i += 4)
00224 {
00225 x[i + 0] -= y[i + 0] * s0;
00226 x[i + 1] -= y[i + 1] * s1;
00227 x[i + 2] -= y[i + 2] * s2;
00228 x[i + 3] -= y[i + 3] * s3;
00229 }
00230 break;
00231 default:
00232 for(int i = 0; i < m; i += n)
00233 {
00234 for(int j = 0; j < n; j++)
00235 {
00236 x[i + j] -= y[i + j] * s[j];
00237 }
00238 }
00239 }
00240 }
00247 template<class S>
00248 void scale_add(vector<S> &_x, const vector<S> &_y, const S _s)
00249 {
00250 S *x = &_x[0];
00251 const S *y = &_y[0];
00252 const S s = _s;
00253 for(int i = 0; i < (int)_x.size(); i++)
00254 {
00255 *x = *x * s + *y;
00256 x++; y++;
00257 }
00258 }
00265 template<class S>
00266 void scale_add(vector<S> &_x, const vector<S> &_y, const vector<S> &_s)
00267 {
00268 S s0, s1, s2, s3;
00269 S *x = &_x[0];
00270 const S *y = &_y[0], *s = &_s[0];
00271 int m = (int)_x.size(), n = (int)_s.size();
00272 switch(n)
00273 {
00274 case 0:
00275 break;
00276 case 1:
00277 s0 = s[0];
00278 for(int i = 0; i < m; i++)
00279 {
00280 x[i + 0] = x[i + 0] * s0 + y[i + 0];
00281 }
00282 break;
00283 case 2:
00284 s0 = s[0], s1 = s[1];
00285 for(int i = 0; i < m; i += 2)
00286 {
00287 x[i + 0] = x[i + 0] * s0 + y[i + 0];
00288 x[i + 1] = x[i + 1] * s1 + y[i + 1];
00289 }
00290 break;
00291 case 3:
00292 s0 = s[0], s1 = s[1], s2 = s[2];
00293 for(int i = 0; i < m; i += 3)
00294 {
00295 x[i + 0] = x[i + 0] * s0 + y[i + 0];
00296 x[i + 1] = x[i + 1] * s1 + y[i + 1];
00297 x[i + 2] = x[i + 2] * s2 + y[i + 2];
00298 }
00299 break;
00300 case 4:
00301 s0 = s[0], s1 = s[1], s2 = s[2], s3 = s[3];
00302 for(int i = 0; i < m; i += 4)
00303 {
00304 x[i + 0] = x[i + 0] * s0 + y[i + 0];
00305 x[i + 1] = x[i + 1] * s1 + y[i + 1];
00306 x[i + 2] = x[i + 2] * s2 + y[i + 2];
00307 x[i + 3] = x[i + 3] * s3 + y[i + 3];
00308 }
00309 break;
00310 default:
00311 for(int i = 0; i < m; i += n)
00312 {
00313 for(int j = 0; j < n; j++)
00314 {
00315 x[i + j] = x[i + j] * s[j] + y[i + j];
00316 }
00317 }
00318 }
00319 }
00320
00329 template<class T, class S>
00330 void element_accumulation(const vector<T> &_hdr, vector<T> &_con, vector<T> &_row, vector<T> &_col, vector<S> &_ele)
00331 {
00332 T elemsize = _hdr[3];
00333 T gnumnode = _hdr[4];
00334
00335 _row.resize(_ele.size());
00336 _col.resize(_ele.size());
00337
00338 for(int i = 0; i < (int)_con.size(); i += elemsize)
00339 {
00340 for(int j = 0; j < elemsize; j++)
00341 {
00342 for(int k = 0; k < elemsize; k++)
00343 {
00344 _row[i * elemsize + j * elemsize + k] = _con[i + j];
00345 _col[i * elemsize + j * elemsize + k] = _con[i + k];
00346 }
00347 }
00348 }
00349
00350 binary_sort_sort_copy(_row, _col, _ele);
00351 unique_accumulate(_row, _col, _ele);
00352
00353 binary_sort(_con);
00354 unique(_con);
00355
00356 vector<T> glo(gnumnode, -1);
00357 for(int i = 0; i < (int)_con.size(); i++) glo[_con[i]] = i;
00358 for(int i = 0; i < (int)_row.size(); i++) _row[i] = glo[_row[i]];
00359 for(int i = 0; i < (int)_row.size(); i++) _col[i] = glo[_col[i]];
00360 }
00361
00370 template<class T, class S>
00371 void diagonal(const vector<T> &_con, const vector<T> &_row, const vector<T> &_col, const vector<S> &_ele, vector<S> &_dia)
00372 {
00373 _dia.resize(_con.size());
00374 for(int i = 0, j = 0; i < (int)_ele.size(); i++)
00375 {
00376 if((_row[i] == j) && (_col[i] == j))
00377 {
00378 _dia[j] = _ele[i];
00379 j++;
00380 }
00381 }
00382 }
00383
