matop.c 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556
  1. /**************************************************************************
  2. **
  3. ** Copyright (C) 1993 David E. Steward & Zbigniew Leyk, all rights reserved.
  4. **
  5. ** Meschach Library
  6. **
  7. ** This Meschach Library is provided "as is" without any express
  8. ** or implied warranty of any kind with respect to this software.
  9. ** In particular the authors shall not be liable for any direct,
  10. ** indirect, special, incidental or consequential damages arising
  11. ** in any way from use of the software.
  12. **
  13. ** Everyone is granted permission to copy, modify and redistribute this
  14. ** Meschach Library, provided:
  15. ** 1. All copies contain this copyright notice.
  16. ** 2. All modified copies shall carry a notice stating who
  17. ** made the last modification and the date of such modification.
  18. ** 3. No charge is made for this software or works derived from it.
  19. ** This clause shall not be construed as constraining other software
  20. ** distributed on the same medium as this software, nor is a
  21. ** distribution fee considered a charge.
  22. **
  23. ***************************************************************************/
  24. /* matop.c 1.3 11/25/87 */
  25. #include <stdio.h>
  26. #include "matrix.h"
  27. static char rcsid[] = "$Id: matop.c,v 1.4 1995/03/27 15:43:57 des Exp $";
  28. /* m_add -- matrix addition -- may be in-situ */
  29. #ifndef ANSI_C
  30. MAT *m_add(mat1,mat2,out)
  31. MAT *mat1,*mat2,*out;
  32. #else
  33. MAT *m_add(const MAT *mat1, const MAT *mat2, MAT *out)
  34. #endif
  35. {
  36. unsigned int m,n,i;
  37. if ( mat1==(MAT *)NULL || mat2==(MAT *)NULL )
  38. error(E_NULL,"m_add");
  39. if ( mat1->m != mat2->m || mat1->n != mat2->n )
  40. error(E_SIZES,"m_add");
  41. if ( out==(MAT *)NULL || out->m != mat1->m || out->n != mat1->n )
  42. out = m_resize(out,mat1->m,mat1->n);
  43. m = mat1->m; n = mat1->n;
  44. for ( i=0; i<m; i++ )
  45. {
  46. __add__(mat1->me[i],mat2->me[i],out->me[i],(int)n);
  47. /**************************************************
  48. for ( j=0; j<n; j++ )
  49. out->me[i][j] = mat1->me[i][j]+mat2->me[i][j];
  50. **************************************************/
  51. }
  52. return (out);
  53. }
  54. /* m_sub -- matrix subtraction -- may be in-situ */
  55. #ifndef ANSI_C
  56. MAT *m_sub(mat1,mat2,out)
  57. MAT *mat1,*mat2,*out;
  58. #else
  59. MAT *m_sub(const MAT *mat1, const MAT *mat2, MAT *out)
  60. #endif
  61. {
  62. unsigned int m,n,i;
  63. if ( mat1==(MAT *)NULL || mat2==(MAT *)NULL )
  64. error(E_NULL,"m_sub");
  65. if ( mat1->m != mat2->m || mat1->n != mat2->n )
  66. error(E_SIZES,"m_sub");
  67. if ( out==(MAT *)NULL || out->m != mat1->m || out->n != mat1->n )
  68. out = m_resize(out,mat1->m,mat1->n);
  69. m = mat1->m; n = mat1->n;
  70. for ( i=0; i<m; i++ )
  71. {
  72. __sub__(mat1->me[i],mat2->me[i],out->me[i],(int)n);
  73. /**************************************************
  74. for ( j=0; j<n; j++ )
  75. out->me[i][j] = mat1->me[i][j]-mat2->me[i][j];
  76. **************************************************/
  77. }
  78. return (out);
  79. }
  80. /* m_mlt -- matrix-matrix multiplication */
  81. #ifndef ANSI_C
  82. MAT *m_mlt(A,B,OUT)
  83. MAT *A,*B,*OUT;
  84. #else
  85. MAT *m_mlt(const MAT *A, const MAT *B, MAT *OUT)
  86. #endif
  87. {
  88. unsigned int i, /* j, */ k, m, n, p;
  89. Real **A_v, **B_v /*, *B_row, *OUT_row, sum, tmp */;
  90. if ( A==(MAT *)NULL || B==(MAT *)NULL )
  91. error(E_NULL,"m_mlt");
  92. if ( A->n != B->m )
  93. error(E_SIZES,"m_mlt");
  94. if ( A == OUT || B == OUT )
  95. error(E_INSITU,"m_mlt");
  96. m = A->m; n = A->n; p = B->n;
  97. A_v = A->me; B_v = B->me;
  98. if ( OUT==(MAT *)NULL || OUT->m != A->m || OUT->n != B->n )
  99. OUT = m_resize(OUT,A->m,B->n);
  100. /****************************************************************
  101. for ( i=0; i<m; i++ )
  102. for ( j=0; j<p; j++ )
  103. {
  104. sum = 0.0;
  105. for ( k=0; k<n; k++ )
  106. sum += A_v[i][k]*B_v[k][j];
  107. OUT->me[i][j] = sum;
  108. }
  109. ****************************************************************/
  110. m_zero(OUT);
  111. for ( i=0; i<m; i++ )
  112. for ( k=0; k<n; k++ )
  113. {
  114. if ( A_v[i][k] != 0.0 )
  115. __mltadd__(OUT->me[i],B_v[k],A_v[i][k],(int)p);
  116. /**************************************************
  117. B_row = B_v[k]; OUT_row = OUT->me[i];
  118. for ( j=0; j<p; j++ )
  119. (*OUT_row++) += tmp*(*B_row++);
  120. **************************************************/
  121. }
  122. return OUT;
  123. }
  124. /* mmtr_mlt -- matrix-matrix transposed multiplication
  125. -- A.B^T is returned, and stored in OUT */
  126. #ifndef ANSI_C
  127. MAT *mmtr_mlt(A,B,OUT)
  128. MAT *A, *B, *OUT;
  129. #else
  130. MAT *mmtr_mlt(const MAT *A, const MAT *B, MAT *OUT)
  131. #endif
  132. {
  133. int i, j, limit;
  134. /* Real *A_row, *B_row, sum; */
  135. if ( ! A || ! B )
  136. error(E_NULL,"mmtr_mlt");
  137. if ( A == OUT || B == OUT )
  138. error(E_INSITU,"mmtr_mlt");
  139. if ( A->n != B->n )
  140. error(E_SIZES,"mmtr_mlt");
  141. if ( ! OUT || OUT->m != A->m || OUT->n != B->m )
  142. OUT = m_resize(OUT,A->m,B->m);
  143. limit = A->n;
  144. for ( i = 0; i < A->m; i++ )
  145. for ( j = 0; j < B->m; j++ )
  146. {
  147. OUT->me[i][j] = __ip__(A->me[i],B->me[j],(int)limit);
  148. /**************************************************
  149. sum = 0.0;
  150. A_row = A->me[i];
  151. B_row = B->me[j];
  152. for ( k = 0; k < limit; k++ )
  153. sum += (*A_row++)*(*B_row++);
  154. OUT->me[i][j] = sum;
  155. **************************************************/
  156. }
  157. return OUT;
  158. }
  159. /* mtrm_mlt -- matrix transposed-matrix multiplication
  160. -- A^T.B is returned, result stored in OUT */
  161. #ifndef ANSI_C
  162. MAT *mtrm_mlt(A,B,OUT)
  163. MAT *A, *B, *OUT;
  164. #else
  165. MAT *mtrm_mlt(const MAT *A, const MAT *B, MAT *OUT)
  166. #endif
  167. {
  168. int i, k, limit;
  169. /* Real *B_row, *OUT_row, multiplier; */
  170. if ( ! A || ! B )
  171. error(E_NULL,"mmtr_mlt");
  172. if ( A == OUT || B == OUT )
  173. error(E_INSITU,"mtrm_mlt");
  174. if ( A->m != B->m )
  175. error(E_SIZES,"mmtr_mlt");
  176. if ( ! OUT || OUT->m != A->n || OUT->n != B->n )
  177. OUT = m_resize(OUT,A->n,B->n);
  178. limit = B->n;
  179. m_zero(OUT);
  180. for ( k = 0; k < A->m; k++ )
  181. for ( i = 0; i < A->n; i++ )
  182. {
  183. if ( A->me[k][i] != 0.0 )
  184. __mltadd__(OUT->me[i],B->me[k],A->me[k][i],(int)limit);
  185. /**************************************************
  186. multiplier = A->me[k][i];
  187. OUT_row = OUT->me[i];
  188. B_row = B->me[k];
  189. for ( j = 0; j < limit; j++ )
  190. *(OUT_row++) += multiplier*(*B_row++);
  191. **************************************************/
  192. }
  193. return OUT;
  194. }
  195. /* mv_mlt -- matrix-vector multiplication
  196. -- Note: b is treated as a column vector */
  197. #ifndef ANSI_C
  198. VEC *mv_mlt(A,b,out)
  199. MAT *A;
  200. VEC *b,*out;
  201. #else
  202. VEC *mv_mlt(const MAT *A, const VEC *b, VEC *out)
  203. #endif
  204. {
  205. unsigned int i, m, n;
  206. Real **A_v, *b_v /*, *A_row */;
  207. /* register Real sum; */
  208. if ( A==(MAT *)NULL || b==(VEC *)NULL )
  209. error(E_NULL,"mv_mlt");
  210. if ( A->n != b->dim )
  211. error(E_SIZES,"mv_mlt");
  212. if ( b == out )
  213. error(E_INSITU,"mv_mlt");
  214. if ( out == (VEC *)NULL || out->dim != A->m )
  215. out = v_resize(out,A->m);
  216. m = A->m; n = A->n;
  217. A_v = A->me; b_v = b->ve;
  218. for ( i=0; i<m; i++ )
  219. {
  220. /* for ( j=0; j<n; j++ )
  221. sum += A_v[i][j]*b_v[j]; */
  222. out->ve[i] = __ip__(A_v[i],b_v,(int)n);
  223. /**************************************************
  224. A_row = A_v[i]; b_v = b->ve;
  225. for ( j=0; j<n; j++ )
  226. sum += (*A_row++)*(*b_v++);
  227. out->ve[i] = sum;
  228. **************************************************/
  229. }
  230. return out;
  231. }
  232. /* sm_mlt -- scalar-matrix multiply -- may be in-situ */
  233. #ifndef ANSI_C
  234. MAT *sm_mlt(scalar,matrix,out)
  235. double scalar;
  236. MAT *matrix,*out;
  237. #else
  238. MAT *sm_mlt(double scalar, const MAT *matrix, MAT *out)
  239. #endif
  240. {
  241. unsigned int m,n,i;
  242. if ( matrix==(MAT *)NULL )
  243. error(E_NULL,"sm_mlt");
  244. if ( out==(MAT *)NULL || out->m != matrix->m || out->n != matrix->n )
  245. out = m_resize(out,matrix->m,matrix->n);
  246. m = matrix->m; n = matrix->n;
  247. for ( i=0; i<m; i++ )
  248. __smlt__(matrix->me[i],(double)scalar,out->me[i],(int)n);
  249. /**************************************************
  250. for ( j=0; j<n; j++ )
  251. out->me[i][j] = scalar*matrix->me[i][j];
  252. **************************************************/
  253. return (out);
  254. }
  255. /* vm_mlt -- vector-matrix multiplication
  256. -- Note: b is treated as a row vector */
  257. #ifndef ANSI_C
  258. VEC *vm_mlt(A,b,out)
  259. MAT *A;
  260. VEC *b,*out;
  261. #else
  262. VEC *vm_mlt(const MAT *A, const VEC *b, VEC *out)
  263. #endif
  264. {
  265. unsigned int j,m,n;
  266. /* Real sum,**A_v,*b_v; */
  267. if ( A==(MAT *)NULL || b==(VEC *)NULL )
  268. error(E_NULL,"vm_mlt");
  269. if ( A->m != b->dim )
  270. error(E_SIZES,"vm_mlt");
  271. if ( b == out )
  272. error(E_INSITU,"vm_mlt");
  273. if ( out == (VEC *)NULL || out->dim != A->n )
  274. out = v_resize(out,A->n);
  275. m = A->m; n = A->n;
  276. v_zero(out);
  277. for ( j = 0; j < m; j++ )
  278. if ( b->ve[j] != 0.0 )
  279. __mltadd__(out->ve,A->me[j],b->ve[j],(int)n);
  280. /**************************************************
  281. A_v = A->me; b_v = b->ve;
  282. for ( j=0; j<n; j++ )
  283. {
  284. sum = 0.0;
  285. for ( i=0; i<m; i++ )
  286. sum += b_v[i]*A_v[i][j];
  287. out->ve[j] = sum;
  288. }
  289. **************************************************/
  290. return out;
  291. }
  292. /* m_transp -- transpose matrix */
  293. #ifndef ANSI_C
  294. MAT *m_transp(in,out)
  295. MAT *in, *out;
  296. #else
  297. MAT *m_transp(const MAT *in, MAT *out)
  298. #endif
  299. {
  300. int i, j;
  301. int in_situ;
  302. Real tmp;
  303. if ( in == (MAT *)NULL )
  304. error(E_NULL,"m_transp");
  305. if ( in == out && in->n != in->m )
  306. error(E_INSITU2,"m_transp");
  307. in_situ = ( in == out );
  308. if ( out == (MAT *)NULL || out->m != in->n || out->n != in->m )
  309. out = m_resize(out,in->n,in->m);
  310. if ( ! in_situ )
  311. for ( i = 0; i < in->m; i++ )
  312. for ( j = 0; j < in->n; j++ )
  313. out->me[j][i] = in->me[i][j];
  314. else
  315. for ( i = 1; i < in->m; i++ )
  316. for ( j = 0; j < i; j++ )
  317. { tmp = in->me[i][j];
  318. in->me[i][j] = in->me[j][i];
  319. in->me[j][i] = tmp;
  320. }
  321. return out;
  322. }
  323. /* swap_rows -- swaps rows i and j of matrix A for cols lo through hi */
  324. #ifndef ANSI_C
  325. MAT *swap_rows(A,i,j,lo,hi)
  326. MAT *A;
  327. int i, j, lo, hi;
  328. #else
  329. MAT *swap_rows(MAT *A, int i, int j, int lo, int hi)
  330. #endif
  331. {
  332. int k;
  333. Real **A_me, tmp;
  334. if ( ! A )
  335. error(E_NULL,"swap_rows");
  336. if ( i < 0 || j < 0 || i >= A->m || j >= A->m )
  337. error(E_SIZES,"swap_rows");
  338. lo = max(0,lo);
  339. hi = min(hi,A->n-1);
  340. A_me = A->me;
  341. for ( k = lo; k <= hi; k++ )
  342. {
  343. tmp = A_me[k][i];
  344. A_me[k][i] = A_me[k][j];
  345. A_me[k][j] = tmp;
  346. }
  347. return A;
  348. }
  349. /* swap_cols -- swap columns i and j of matrix A for cols lo through hi */
  350. #ifndef ANSI_C
  351. MAT *swap_cols(A,i,j,lo,hi)
  352. MAT *A;
  353. int i, j, lo, hi;
  354. #else
  355. MAT *swap_cols(MAT *A, int i, int j, int lo, int hi)
  356. #endif
  357. {
  358. int k;
  359. Real **A_me, tmp;
  360. if ( ! A )
  361. error(E_NULL,"swap_cols");
  362. if ( i < 0 || j < 0 || i >= A->n || j >= A->n )
  363. error(E_SIZES,"swap_cols");
  364. lo = max(0,lo);
  365. hi = min(hi,A->m-1);
  366. A_me = A->me;
  367. for ( k = lo; k <= hi; k++ )
  368. {
  369. tmp = A_me[i][k];
  370. A_me[i][k] = A_me[j][k];
  371. A_me[j][k] = tmp;
  372. }
  373. return A;
  374. }
  375. /* ms_mltadd -- matrix-scalar multiply and add
  376. -- may be in situ
  377. -- returns out == A1 + s*A2 */
  378. #ifndef ANSI_C
  379. MAT *ms_mltadd(A1,A2,s,out)
  380. MAT *A1, *A2, *out;
  381. double s;
  382. #else
  383. MAT *ms_mltadd(const MAT *A1, const MAT *A2, double s, MAT *out)
  384. #endif
  385. {
  386. /* register Real *A1_e, *A2_e, *out_e; */
  387. /* register int j; */
  388. int i, m, n;
  389. if ( ! A1 || ! A2 )
  390. error(E_NULL,"ms_mltadd");
  391. if ( A1->m != A2->m || A1->n != A2->n )
  392. error(E_SIZES,"ms_mltadd");
  393. if ( out != A1 && out != A2 )
  394. out = m_resize(out,A1->m,A1->n);
  395. if ( s == 0.0 )
  396. return m_copy(A1,out);
  397. if ( s == 1.0 )
  398. return m_add(A1,A2,out);
  399. tracecatch(out = m_copy(A1,out),"ms_mltadd");
  400. m = A1->m; n = A1->n;
  401. for ( i = 0; i < m; i++ )
  402. {
  403. __mltadd__(out->me[i],A2->me[i],s,(int)n);
  404. /**************************************************
  405. A1_e = A1->me[i];
  406. A2_e = A2->me[i];
  407. out_e = out->me[i];
  408. for ( j = 0; j < n; j++ )
  409. out_e[j] = A1_e[j] + s*A2_e[j];
  410. **************************************************/
  411. }
  412. return out;
  413. }
  414. /* mv_mltadd -- matrix-vector multiply and add
  415. -- may not be in situ
  416. -- returns out == v1 + alpha*A*v2 */
  417. #ifndef ANSI_C
  418. VEC *mv_mltadd(v1,v2,A,alpha,out)
  419. VEC *v1, *v2, *out;
  420. MAT *A;
  421. double alpha;
  422. #else
  423. VEC *mv_mltadd(const VEC *v1, const VEC *v2, const MAT *A,
  424. double alpha, VEC *out)
  425. #endif
  426. {
  427. /* register int j; */
  428. int i, m, n;
  429. Real *v2_ve, *out_ve;
  430. if ( ! v1 || ! v2 || ! A )
  431. error(E_NULL,"mv_mltadd");
  432. if ( out == v2 )
  433. error(E_INSITU,"mv_mltadd");
  434. if ( v1->dim != A->m || v2->dim != A->n )
  435. error(E_SIZES,"mv_mltadd");
  436. tracecatch(out = v_copy(v1,out),"mv_mltadd");
  437. v2_ve = v2->ve; out_ve = out->ve;
  438. m = A->m; n = A->n;
  439. if ( alpha == 0.0 )
  440. return out;
  441. for ( i = 0; i < m; i++ )
  442. {
  443. out_ve[i] += alpha*__ip__(A->me[i],v2_ve,(int)n);
  444. /**************************************************
  445. A_e = A->me[i];
  446. sum = 0.0;
  447. for ( j = 0; j < n; j++ )
  448. sum += A_e[j]*v2_ve[j];
  449. out_ve[i] = v1->ve[i] + alpha*sum;
  450. **************************************************/
  451. }
  452. return out;
  453. }
  454. /* vm_mltadd -- vector-matrix multiply and add
  455. -- may not be in situ
  456. -- returns out' == v1' + v2'*A */
  457. #ifndef ANSI_C
  458. VEC *vm_mltadd(v1,v2,A,alpha,out)
  459. VEC *v1, *v2, *out;
  460. MAT *A;
  461. double alpha;
  462. #else
  463. VEC *vm_mltadd(const VEC *v1, const VEC *v2, const MAT *A,
  464. double alpha, VEC *out)
  465. #endif
  466. {
  467. int /* i, */ j, m, n;
  468. Real tmp, /* *A_e, */ *out_ve;
  469. if ( ! v1 || ! v2 || ! A )
  470. error(E_NULL,"vm_mltadd");
  471. if ( v2 == out )
  472. error(E_INSITU,"vm_mltadd");
  473. if ( v1->dim != A->n || A->m != v2->dim )
  474. error(E_SIZES,"vm_mltadd");
  475. tracecatch(out = v_copy(v1,out),"vm_mltadd");
  476. out_ve = out->ve; m = A->m; n = A->n;
  477. for ( j = 0; j < m; j++ )
  478. {
  479. tmp = v2->ve[j]*alpha;
  480. if ( tmp != 0.0 )
  481. __mltadd__(out_ve,A->me[j],tmp,(int)n);
  482. /**************************************************
  483. A_e = A->me[j];
  484. for ( i = 0; i < n; i++ )
  485. out_ve[i] += A_e[i]*tmp;
  486. **************************************************/
  487. }
  488. return out;
  489. }