MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
matvec3n.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/libraries/libmbmath/matvec3n.cc,v 1.38 2017/01/12 14:43:54 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
33 
34 #include <matvec3n.h>
35 
36 /* VecN - begin */
37 
38 #ifdef DEBUG
39 void VecN::IsValid(void) const
40 {
41  ASSERT(iMaxRows > 0);
42  ASSERT(iNumRows > 0);
43  ASSERT(pdVec != NULL);
44 }
45 #endif /* DEBUG */
46 
48 {
49  ASSERT(ns > 0);
50  if (pdVec != NULL) {
51  Destroy_();
52  }
54 }
55 
56 
57 void VecN::Destroy_(void)
58 {
59  if (pdVec != NULL) {
61  }
62 }
63 
64 
66 : iMaxRows(0), iNumRows(0), pdVec(NULL)
67 {
68  NO_OP;
69 }
70 
71 
73 : iMaxRows(ns), iNumRows(ns), pdVec(NULL)
74 {
75  Create_(ns);
76 #ifdef DEBUG
77  IsValid();
78 #endif /* DEBUG */
79 }
80 
81 
83 : iMaxRows(ns), iNumRows(ns), pdVec(NULL)
84 {
85  Create_(ns);
86  Reset(d);
87 }
88 
89 VecN::VecN(const VecN& v)
90 : iMaxRows(v.iNumRows), iNumRows(v.iNumRows), pdVec(NULL)
91 {
92  Create_(v.iNumRows);
93  for (integer iCnt = 0; iCnt < iNumRows; iCnt++) {
94  pdVec[iCnt] = v.pdVec[iCnt];
95  }
96 }
97 
98 /*
99  Costruttore da VectorHandler. Prende i valori da iFirstIndex
100  a iFirstIndex+ns. Nota: gli indici del VectorHandler partono da 1,
101  in stile FORTRAN.
102 */
103 
104 VecN::VecN(const VectorHandler& vh, integer ns, integer iFirstIndex)
105 :iMaxRows(ns), iNumRows(ns), pdVec(NULL)
106 {
107  ASSERT(iFirstIndex > 0 && iFirstIndex + ns - 1 <= vh.iGetSize());
108  Create_(ns);
109  for(integer iCnt = 0; iCnt < ns; iCnt++) {
110  pdVec[iCnt] = vh(iFirstIndex+iCnt);
111  }
112 }
113 
114 
115 const VecN&
116 VecN::Copy(const VectorHandler& vh, integer iFirstIndex)
117 {
118  ASSERT(iFirstIndex > 0 && iFirstIndex + iNumRows - 1 <= vh.iGetSize());
119  for (integer iCnt = 0; iCnt < iNumRows; iCnt++) {
120  pdVec[iCnt] = vh(iFirstIndex+iCnt);
121  }
122 
123  return *this;
124 }
125 
126 
128 {
129  Destroy_();
130 }
131 
132 
134 {
135  ASSERT(ns > 0);
136  if (pdVec != NULL) {
137  Destroy_();
138  }
139  Create_(ns);
140  iMaxRows = iNumRows = ns;
141 #ifdef DEBUG
142  IsValid();
143 #endif /* DEBUG */
144 }
145 
146 
147 void VecN::Reset(const doublereal d)
148 {
149 #ifdef DEBUG
150  IsValid();
151 #endif /* DEBUG */
152  for (integer i = iNumRows; i-- > 0; ) {
153  pdVec[i] = d;
154  }
155 }
156 
157 void VecN::RightMult(const MatNx3& n, const Vec3& v)
158 {
159 #ifdef DEBUG
160  IsValid();
161 #endif /* DEBUG */
162  for (integer i = iNumRows; i-- > 0; ) {
163  pdVec[i] = n.pdCols[0][i]*v.pdVec[V1]
164  + n.pdCols[1][i]*v.pdVec[V2]
165  + n.pdCols[2][i]*v.pdVec[V3];
166  }
167 }
168 
169 const VecN& VecN::operator += (const VecN& m)
170 {
171 #ifdef DEBUG
172  IsValid();
173 #endif /* DEBUG */
174  for (integer i = m.iNumRows; i-- > 0; ) {
175  pdVec[i] += m.pdVec[i];
176  }
177  return *this;
178 }
179 
180 const VecN&
182 {
183 #ifdef DEBUG
184  IsValid();
185 #endif /* DEBUG */
186  for (integer i = iNumRows; i-- > 0; ) {
187  pdVec[i] *= d;
188  }
189  return *this;
190 }
191 
192 const VecN& VecN::Mult(const MatNxN& m, const VecN& n)
193 {
194 #ifdef DEBUG
195  IsValid();
196 #endif /* DEBUG */
197  ASSERT(iNumRows == m.iNumRows);
198  ASSERT(m.iNumRows == n.iNumRows);
199 
200  for (integer i = 0; i < iNumRows; i++ ) {
201  pdVec[i] = 0.;
202  for (integer j = 0; j < iNumRows; j++ ) {
203  pdVec[i] += m.pdMat[j][i]*n.pdVec[j];
204  }
205  }
206 
207  return *this;
208 }
209 
210 const VecN&
211 VecN::Mult(const MatNxN& m, const ArrayView& vm,
212  const VecN& n, const ArrayView& vn)
213 {
214 #ifdef DEBUG
215  IsValid();
216 #endif /* DEBUG */
217  ASSERT(iNumRows == m.iNumRows);
218  ASSERT(m.iNumRows >= vm.Last());
219  ASSERT(n.iNumRows >= vn.Last());
220  ASSERT(vm.Number() == vm.Number());
221 
222  for(integer i = 0; i < iNumRows; i++) {
223  integer jm = vm.Start() - 1;
224  integer jn = vn.Start() - 1;
225 
226  pdVec[i] = 0.;
227  for(integer j = 0; j < vm.Number(); j++) {
228  pdVec[i] += m.pdMat[jm][i]*n.pdVec[jn];
229 
230  jm += vm.Offset();
231  jn += vn.Offset();
232  }
233  }
234 
235  return *this;
236 }
237 
238 /* VecN - end */
239 
240 
241 /* Mat3xN - begin */
242 #ifdef DEBUG
243 void Mat3xN::IsValid(void) const
244 {
245  ASSERT(iNumCols > 0);
246  ASSERT(iMaxCols > 0);
247  ASSERT(pdRows[0] != NULL);
248 }
249 #endif /* DEBUG */
250 
251 
253 {
254  ASSERT(ns > 0);
255  if (pdRows[0] != NULL) {
256  Destroy_();
257  }
258  SAFENEWARR(pdRows[0], doublereal, 3*ns);
259  pdRows[1] = pdRows[0]+ns;
260  pdRows[2] = pdRows[1]+ns;
261 }
262 
263 
265 {
266  if (pdRows[0] != NULL) {
267  SAFEDELETEARR(pdRows[0]);
268  }
269 }
270 
271 
273 : iMaxCols(0), iNumCols(0)
274 {
275  pdRows[0] = NULL;
276  pdRows[1] = NULL;
277  pdRows[2] = NULL;
278 }
279 
280 
282 : iMaxCols(nc), iNumCols(nc)
283 {
284  ASSERT(iNumCols > 0);
285  pdRows[0] = NULL;
286  pdRows[1] = NULL;
287  pdRows[2] = NULL;
288 
289  Create_(iNumCols);
290 #ifdef DEBUG
291  IsValid();
292 #endif /* DEBUG */
293 }
294 
295 
297 : iMaxCols(nc), iNumCols(nc)
298 {
299  ASSERT(iNumCols > 0);
300  pdRows[0] = NULL;
301  pdRows[1] = NULL;
302  pdRows[2] = NULL;
303 
304  Create_(iNumCols);
305 #ifdef DEBUG
306  IsValid();
307 #endif /* DEBUG */
308  Reset(d);
309 }
310 
311 
313 {
314  Destroy_();
315 }
316 
317 
319 {
320  ASSERT(ns > 0);
321  if (ns <= iMaxCols) {
322  iNumCols = ns;
323  } else {
324  Destroy_();
325  Create_(ns);
326  iMaxCols = iNumCols = ns;
327  }
328 #ifdef DEBUG
329  IsValid();
330 #endif /* DEBUG */
331 }
332 
333 
334 void Mat3xN::Reset(const doublereal& d)
335 {
336 #ifdef DEBUG
337  IsValid();
338 #endif /* DEBUG */
339 
340  for (integer j = iNumCols; j-- > 0; ) {
341  pdRows[0][j] = d;
342  pdRows[1][j] = d;
343  pdRows[2][j] = d;
344  }
345 }
346 
347 
348 const Mat3xN& Mat3xN::LeftMult(const Mat3x3& m)
349 {
350 #ifdef DEBUG
351  IsValid();
352 #endif /* DEBUG */
353  for (integer i = iNumCols; i-- > 0; ) {
354  doublereal d[3] = { 0., 0., 0. };
355 
356  d[0] =
357  m.pdMat[M11] * pdRows[0][i]
358  + m.pdMat[M12] * pdRows[1][i]
359  + m.pdMat[M13] * pdRows[2][i];
360  d[1] =
361  m.pdMat[M21] * pdRows[0][i]
362  + m.pdMat[M22] * pdRows[1][i]
363  + m.pdMat[M23] * pdRows[2][i];
364  d[2] =
365  m.pdMat[M31] * pdRows[0][i]
366  + m.pdMat[M32] * pdRows[1][i]
367  + m.pdMat[M33] * pdRows[2][i];
368 
369  pdRows[0][i] = d[0];
370  pdRows[1][i] = d[1];
371  pdRows[2][i] = d[2];
372  }
373  return *this;
374 }
375 
376 
377 const Mat3xN& Mat3xN::LeftMult(const Mat3x3& m, const Mat3xN& n)
378 {
379 #ifdef DEBUG
380  IsValid();
381  n.IsValid();
382 #endif /* DEBUG */
383 
384  if (iNumCols != n.iNumCols) {
385  Resize(n.iNumCols);
386  /* FIXME: sicuri di voler fare resize? non si azzera? */
387  }
388 
389  for (integer i = iNumCols; i-- > 0; ) {
390  pdRows[0][i] =
391  m.pdMat[M11] * n.pdRows[0][i]
392  + m.pdMat[M12] * n.pdRows[1][i]
393  + m.pdMat[M13] * n.pdRows[2][i];
394  pdRows[1][i] =
395  m.pdMat[M21] * n.pdRows[0][i]
396  + m.pdMat[M22] * n.pdRows[1][i]
397  + m.pdMat[M23] * n.pdRows[2][i];
398  pdRows[2][i] =
399  m.pdMat[M31] * n.pdRows[0][i]
400  + m.pdMat[M32] * n.pdRows[1][i]
401  + m.pdMat[M33] * n.pdRows[2][i];
402  }
403  return *this;
404 }
405 
406 
407 const Mat3xN& Mat3xN::Copy(const Mat3xN& m)
408 {
409 #ifdef DEBUG
410  m.IsValid();
411 #endif /* DEBUG */
412  Resize(m.iNumCols);
413  for (integer j = m.iNumCols; j-- > 0; ) {
414  pdRows[0][j] = m.pdRows[0][j];
415  pdRows[1][j] = m.pdRows[1][j];
416  pdRows[2][j] = m.pdRows[2][j];
417  }
418  return *this;
419 }
420 
421 
423 {
424 #ifdef DEBUG
425  IsValid();
426 #endif /* DEBUG */
427 
428  if (d != 1.) {
429  for (integer j = iNumCols; j-- > 0; ) {
430  pdRows[0][j] *= d;
431  pdRows[1][j] *= d;
432  pdRows[2][j] *= d;
433  }
434  }
435  return *this;
436 }
437 
438 
440 {
441 #ifdef DEBUG
442  IsValid();
443 #endif /* DEBUG */
444  if (d == 0.) {
445  silent_cerr("division by zero" << std::endl);
447  }
448  if (d != 1.) {
449  for (integer j = iNumCols; j-- > 0; ) {
450  pdRows[0][j] /= d;
451  pdRows[1][j] /= d;
452  pdRows[2][j] /= d;
453  }
454  }
455  return *this;
456 }
457 
458 
460 {
461 #ifdef DEBUG
462  IsValid();
463 #endif /* DEBUG */
464  for (integer j = m.iNumCols; j-- > 0; ) {
465  pdRows[0][j] += m.pdRows[0][j];
466  pdRows[1][j] += m.pdRows[1][j];
467  pdRows[2][j] += m.pdRows[2][j];
468  }
469  return *this;
470 }
471 
472 
474 {
475 #ifdef DEBUG
476  IsValid();
477 #endif /* DEBUG */
478  for (integer j = m.iNumCols; j-- > 0; ) {
479  pdRows[0][j] -= m.pdRows[0][j];
480  pdRows[1][j] -= m.pdRows[1][j];
481  pdRows[2][j] -= m.pdRows[2][j];
482  }
483  return *this;
484 }
485 
486 
487 Vec3
488 Mat3xN::operator * (const VecN& v) const
489 {
490 #ifdef DEBUG
491  IsValid();
492 #endif /* DEBUG */
493  ASSERT(iNumCols == v.iNumRows);
494 
495  doublereal d[3] = { 0., 0., 0. };
496  for (integer j = iNumCols; j-- > 0; ) {
497  d[0] += pdRows[0][j]*v.pdVec[j];
498  d[1] += pdRows[1][j]*v.pdVec[j];
499  d[2] += pdRows[2][j]*v.pdVec[j];
500  }
501 
502  return Vec3(d);
503 }
504 
505 Vec3
506 Mat3xN::Mult(const ArrayView& vm, const VecN& v) const
507 {
508 #ifdef DEBUG
509  IsValid();
510 #endif /* DEBUG */
511  ASSERT(iNumCols >= vm.Last());
512  ASSERT(vm.Number() == v.iNumRows);
513 
514  doublereal d[3] = { 0., 0., 0. };
515  integer jm = vm.Start() - 1;
516  for (integer j = 0; j < vm.Number(); j++) {
517  d[0] += pdRows[0][jm]*v.pdVec[j];
518  d[1] += pdRows[1][jm]*v.pdVec[j];
519  d[2] += pdRows[2][jm]*v.pdVec[j];
520 
521  jm += vm.Offset();
522  }
523 
524  return Vec3(d);
525 }
526 
527 Vec3
528 Mat3xN::Mult(const ArrayView& vm, const VecN& v, const ArrayView& vv) const
529 {
530 #ifdef DEBUG
531  IsValid();
532 #endif /* DEBUG */
533  ASSERT(iNumCols >= vm.Last());
534  ASSERT(v.iNumRows >= vv.Last());
535  ASSERT(vm.Number() == vv.Number());
536 
537  doublereal d[3] = { 0., 0., 0. };
538  integer jm = vm.Start() - 1;
539  integer jv = vv.Start() - 1;
540  for (integer j = 0; j < vm.Number(); j++) {
541  d[0] += pdRows[0][jm]*v.pdVec[jv];
542  d[1] += pdRows[1][jm]*v.pdVec[jv];
543  d[2] += pdRows[2][jm]*v.pdVec[jv];
544 
545  jm += vm.Offset();
546  jv += vv.Offset();
547  }
548 
549  return Vec3(d);
550 }
551 
552 Vec3
554 {
555 #ifdef DEBUG
556  IsValid();
557 #endif /* DEBUG */
558  ASSERT(iCol > 0 && iCol <= iNumCols);
559 
560  --iCol;
561  return Vec3(pdRows[0][iCol], pdRows[1][iCol], pdRows[2][iCol]);
562 }
563 
564 void
565 Mat3xN::PutVec(integer iCol, const Vec3& v)
566 {
567 #ifdef DEBUG
568  IsValid();
569 #endif /* DEBUG */
570  ASSERT(iCol > 0 && iCol <= iNumCols);
571 
572  --iCol;
573  pdRows[0][iCol] = v.pdVec[0];
574  pdRows[1][iCol] = v.pdVec[1];
575  pdRows[2][iCol] = v.pdVec[2];
576 }
577 
578 void
579 Mat3xN::AddVec(integer iCol, const Vec3& v)
580 {
581 #ifdef DEBUG
582  IsValid();
583 #endif /* DEBUG */
584  ASSERT(iCol > 0 && iCol <= iNumCols);
585 
586  --iCol;
587  pdRows[0][iCol] += v.pdVec[0];
588  pdRows[1][iCol] += v.pdVec[1];
589  pdRows[2][iCol] += v.pdVec[2];
590 }
591 
592 void
593 Mat3xN::SubVec(integer iCol, const Vec3& v)
594 {
595 #ifdef DEBUG
596  IsValid();
597 #endif /* DEBUG */
598  ASSERT(iCol > 0 && iCol <= iNumCols);
599 
600  --iCol;
601  pdRows[0][iCol] -= v.pdVec[0];
602  pdRows[1][iCol] -= v.pdVec[1];
603  pdRows[2][iCol] -= v.pdVec[2];
604 }
605 
606 Mat3x3
607 Mat3xN::GetMat3x3(integer iFirstCol) const
608 {
609 #ifdef DEBUG
610  IsValid();
611 #endif /* DEBUG */
612  ASSERT(iFirstCol >= 1 && iFirstCol <= iNumCols-2);
613 
614  --iFirstCol;
615  return Mat3x3(
616  pdRows[0][iFirstCol],
617  pdRows[1][iFirstCol],
618  pdRows[2][iFirstCol],
619  pdRows[0][iFirstCol+1],
620  pdRows[1][iFirstCol+1],
621  pdRows[2][iFirstCol+1],
622  pdRows[0][iFirstCol+2],
623  pdRows[1][iFirstCol+2],
624  pdRows[2][iFirstCol+2]);
625 }
626 
627 void
628 Mat3xN::PutMat3x3(integer iFirstCol, const Mat3x3& m)
629 {
630 #ifdef DEBUG
631  IsValid();
632 #endif /* DEBUG */
633  ASSERT(iFirstCol >= 1 && iFirstCol <= iNumCols-2);
634 
635  --iFirstCol;
636  pdRows[V1][iFirstCol] = m.pdMat[M11];
637  pdRows[V2][iFirstCol] = m.pdMat[M21];
638  pdRows[V3][iFirstCol] = m.pdMat[M31];
639 
640  iFirstCol++;
641  pdRows[V1][iFirstCol] = m.pdMat[M12];
642  pdRows[V2][iFirstCol] = m.pdMat[M22];
643  pdRows[V3][iFirstCol] = m.pdMat[M32];
644 
645  iFirstCol++;
646  pdRows[V1][iFirstCol] = m.pdMat[M13];
647  pdRows[V2][iFirstCol] = m.pdMat[M23];
648  pdRows[V3][iFirstCol] = m.pdMat[M33];
649 }
650 
651 void
652 Mat3xN::AddMat3x3(integer iFirstCol, const Mat3x3& m)
653 {
654 #ifdef DEBUG
655  IsValid();
656 #endif /* DEBUG */
657  ASSERT(iFirstCol >= 1 && iFirstCol <= iNumCols-2);
658 
659  --iFirstCol;
660  pdRows[V1][iFirstCol] += m.pdMat[M11];
661  pdRows[V2][iFirstCol] += m.pdMat[M21];
662  pdRows[V3][iFirstCol] += m.pdMat[M31];
663 
664  iFirstCol++;
665  pdRows[V1][iFirstCol] += m.pdMat[M12];
666  pdRows[V2][iFirstCol] += m.pdMat[M22];
667  pdRows[V3][iFirstCol] += m.pdMat[M32];
668 
669  iFirstCol++;
670  pdRows[V1][iFirstCol] += m.pdMat[M13];
671  pdRows[V2][iFirstCol] += m.pdMat[M23];
672  pdRows[V3][iFirstCol] += m.pdMat[M33];
673 }
674 
675 void
676 Mat3xN::SubMat3x3(integer iFirstCol, const Mat3x3& m)
677 {
678 #ifdef DEBUG
679  IsValid();
680 #endif /* DEBUG */
681  ASSERT(iFirstCol >= 1 && iFirstCol <= iNumCols-2);
682 
683  --iFirstCol;
684  pdRows[V1][iFirstCol] -= m.pdMat[M11];
685  pdRows[V2][iFirstCol] -= m.pdMat[M21];
686  pdRows[V3][iFirstCol] -= m.pdMat[M31];
687 
688  iFirstCol++;
689  pdRows[V1][iFirstCol] -= m.pdMat[M12];
690  pdRows[V2][iFirstCol] -= m.pdMat[M22];
691  pdRows[V3][iFirstCol] -= m.pdMat[M32];
692 
693  iFirstCol++;
694  pdRows[V1][iFirstCol] -= m.pdMat[M13];
695  pdRows[V2][iFirstCol] -= m.pdMat[M23];
696  pdRows[V3][iFirstCol] -= m.pdMat[M33];
697 }
698 
699 Mat3x3
701 {
702 #ifdef DEBUG
703  IsValid();
704 #endif /* DEBUG */
705  ASSERT(iFirstCol >= 1 && iFirstCol <= iNumCols-2);
706 
707  --iFirstCol;
708  return Mat3x3(
709  pdRows[0][iFirstCol]*d,
710  pdRows[1][iFirstCol]*d,
711  pdRows[2][iFirstCol]*d,
712  pdRows[0][iFirstCol+1]*d,
713  pdRows[1][iFirstCol+1]*d,
714  pdRows[2][iFirstCol+1]*d,
715  pdRows[0][iFirstCol+2]*d,
716  pdRows[1][iFirstCol+2]*d,
717  pdRows[2][iFirstCol+2]*d);
718 
719 }
720 
721 /* Mat3xN - end */
722 
723 
724 /* MatNx3 - begin */
725 #ifdef DEBUG
726 void MatNx3::IsValid(void) const
727 {
728  ASSERT(iMaxRows > 0);
729  ASSERT(iNumRows > 0);
730  ASSERT(pdCols[0] != NULL);
731  ASSERT(pdCols[1] != NULL);
732  ASSERT(pdCols[2] != NULL);
733 }
734 #endif /* DEBUG */
735 
736 
738 {
739  ASSERT(ns > 0);
740  if (pdCols[0] != NULL) {
741  Destroy_();
742  }
743  SAFENEWARR(pdCols[0], doublereal, 3*ns);
744  pdCols[1] = pdCols[0] + ns;
745  pdCols[2] = pdCols[1] + ns;
746 }
747 
748 
750 {
751  if (pdCols[0] != NULL) {
752  SAFEDELETEARR(pdCols[0]);
753  }
754 }
755 
756 
758 : iMaxRows(0), iNumRows(0)
759 {
760  pdCols[0] = NULL;
761 }
762 
763 
765 : iMaxRows(ns), iNumRows(ns)
766 {
767  pdCols[0] = NULL;
768  Create_(ns);
769 #ifdef DEBUG
770  IsValid();
771 #endif /* DEBUG */
772 }
773 
774 
776 : iMaxRows(ns), iNumRows(ns)
777 {
778  pdCols[0] = NULL;
779  Create_(ns);
780  Reset(d);
781 }
782 
783 
785 {
786  Destroy_();
787 }
788 
790 {
791  ASSERT(ns > 0);
792  if (pdCols[0] != NULL) {
793  Destroy_();
794  }
795  Create_(ns);
796  iMaxRows = iNumRows = ns;
797 #ifdef DEBUG
798  IsValid();
799 #endif /* DEBUG */
800 }
801 
803 {
804 #ifdef DEBUG
805  IsValid();
806 #endif /* DEBUG */
807  for (integer i = iNumRows; i-- > 0; ) {
808  pdCols[0][i] = d;
809  pdCols[1][i] = d;
810  pdCols[2][i] = d;
811  }
812 }
813 
814 const MatNx3& MatNx3::RightMult(const MatNx3& n, const Mat3x3& m)
815 {
816 #ifdef DEBUG
817  IsValid();
818  n.IsValid();
819 #endif /* DEBUG */
820 
821  if (iNumRows != n.iNumRows) {
822  Resize(n.iNumRows);
823  }
824 
825  for (integer i = iNumRows; i-- > 0; ) {
826  pdCols[0][i] =
827  n.pdCols[0][i]*m.pdMat[M11]
828  +n.pdCols[1][i]*m.pdMat[M21]
829  +n.pdCols[2][i]*m.pdMat[M31];
830  pdCols[1][i] =
831  n.pdCols[0][i]*m.pdMat[M12]
832  +n.pdCols[1][i]*m.pdMat[M22]
833  +n.pdCols[2][i]*m.pdMat[M32];
834  pdCols[2][i] =
835  n.pdCols[0][i]*m.pdMat[M13]
836  +n.pdCols[1][i]*m.pdMat[M23]
837  +n.pdCols[2][i]*m.pdMat[M33];
838  }
839 
840  return *this;
841 }
842 
843 Vec3
845 {
846 #ifdef DEBUG
847  IsValid();
848 #endif /* DEBUG */
849  ASSERT(iRow > 0 && iRow <= iNumRows);
850 
851  --iRow;
852  return Vec3(pdCols[0][iRow], pdCols[1][iRow], pdCols[2][iRow]);
853 }
854 
855 void
856 MatNx3::PutVec(integer iRow, const Vec3& v)
857 {
858 #ifdef DEBUG
859  IsValid();
860 #endif /* DEBUG */
861  ASSERT(iRow > 0 && iRow <= iNumRows);
862 
863  --iRow;
864  pdCols[0][iRow] = v.pdVec[0];
865  pdCols[1][iRow] = v.pdVec[1];
866  pdCols[2][iRow] = v.pdVec[2];
867 }
868 
869 void
870 MatNx3::AddVec(integer iRow, const Vec3& v)
871 {
872 #ifdef DEBUG
873  IsValid();
874 #endif /* DEBUG */
875  ASSERT(iRow > 0 && iRow <= iNumRows);
876 
877  --iRow;
878  pdCols[0][iRow] += v.pdVec[0];
879  pdCols[1][iRow] += v.pdVec[1];
880  pdCols[2][iRow] += v.pdVec[2];
881 }
882 
883 void
884 MatNx3::SubVec(integer iRow, const Vec3& v)
885 {
886 #ifdef DEBUG
887  IsValid();
888 #endif /* DEBUG */
889  ASSERT(iRow > 0 && iRow <= iNumRows);
890 
891  --iRow;
892  pdCols[0][iRow] -= v.pdVec[0];
893  pdCols[1][iRow] -= v.pdVec[1];
894  pdCols[2][iRow] -= v.pdVec[2];
895 }
896 
897 
899 {
900 #ifdef DEBUG
901  IsValid();
902  n.IsValid();
903 #endif /* DEBUG */
904 
905  for (integer i = iNumRows; i-- > 0; ) {
906  pdCols[0][i] = n.pdRows[0][i];
907  pdCols[1][i] = n.pdRows[1][i];
908  pdCols[2][i] = n.pdRows[2][i];
909  }
910  return *this;
911 }
912 /* MatNx3 - end */
913 
914 
915 /* MatNxN - begin */
916 
917 #ifdef DEBUG
918 void MatNxN::IsValid(void) const
919 {
920  ASSERT(iMaxRows > 0);
921  ASSERT(iNumRows > 0);
922  ASSERT(pdVec != NULL);
923  ASSERT(pdMat != NULL);
924 }
925 #endif /* DEBUG */
926 
927 
929 {
930  ASSERT(ns > 0);
931  if (pdVec != NULL) {
932  Destroy_();
933  }
934  SAFENEWARR(pdVec, doublereal, ns*ns);
935  SAFENEWARR(pdMat, doublereal*, ns);
936 
937  pdMat[0] = pdVec;
938  for (integer i = 1; i < ns; i++) {
939  pdMat[i] = pdMat[i-1]+ns;
940  }
941 }
942 
943 
945 {
946  if (pdVec != NULL) {
948  }
949 
950  if (pdMat != NULL) {
952  }
953 }
954 
955 
957 : iMaxRows(0), iNumRows(0), pdVec(NULL), pdMat(NULL)
958 {
959  NO_OP;
960 }
961 
962 
964 : iMaxRows(ns), iNumRows(ns), pdVec(NULL), pdMat(NULL)
965 {
966  Create_(ns);
967 #ifdef DEBUG
968  IsValid();
969 #endif /* DEBUG */
970 }
971 
972 
974 : iMaxRows(ns), iNumRows(ns), pdVec(NULL), pdMat(NULL)
975 {
976  Create_(ns);
977  Reset(d);
978 }
979 
980 
982 {
983  Destroy_();
984 }
985 
987 {
988 #ifdef DEBUG
989  IsValid();
990 #endif /* DEBUG */
991  for (integer i = iNumRows*iNumRows; i-- > 0; ) {
992  pdVec[i] = d;
993  }
994 }
995 
996 const MatNxN&
998 {
999 #ifdef DEBUG
1000  m.IsValid();
1001 #endif /* DEBUG */
1002  ASSERT(iMaxRows >= m.iNumRows);
1003  iNumRows = m.iNumRows;
1004 
1005  for (integer c = 0; c < iNumRows; c++) {
1006 #ifdef HAVE_MEMCPY
1007  memcpy(pdMat[c], m.pdMat[c], sizeof(doublereal)*iNumRows);
1008 #else // ! HAVE_MEMCPY
1009  for (integer r = 0; r < iNumRows; r++) {
1010  pdMat[c][r] = m.pdMat[c][r];
1011  }
1012 #endif // ! HAVE_MEMCPY
1013  }
1014  return *this;
1015 }
1016 
1017 const MatNxN& MatNxN::Mult(const MatNx3& m, const Mat3xN& n)
1018 {
1019 #ifdef DEBUG
1020  IsValid();
1021  n.IsValid();
1022  m.IsValid();
1023 #endif /* DEBUG */
1024 
1025  ASSERT(m.iNumRows == iNumRows);
1026  ASSERT(m.iNumRows == n.iNumCols);
1027 
1028  for (integer i = 0; i < iNumRows; i++) {
1029  for (integer j = 0; j < iNumRows; j++) {
1030  pdMat[j][i] =
1031  m.pdCols[0][i]*n.pdRows[0][j]
1032  + m.pdCols[1][i]*n.pdRows[1][j]
1033  + m.pdCols[2][i]*n.pdRows[2][j];
1034  }
1035  }
1036 
1037  return *this;
1038 }
1039 
1040 std::ostream&
1041 operator << (std::ostream& out, const MatNxN& m)
1042 {
1043  for (integer r = 0; r < m.iNumRows; r++) {
1044  for (integer c = 0; c < m.iNumRows; c++) {
1045  out << " " << m.pdMat[c][r];
1046  }
1047  out << std::endl;
1048  }
1049 
1050  return out;
1051 }
1052 
1053 /* MatNxN - end */
1054 
void Resize(integer ns)
Definition: matvec3n.cc:133
integer iMaxRows
Definition: matvec3n.h:527
const Mat3xN & LeftMult(const Mat3x3 &m)
Definition: matvec3n.cc:348
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Definition: matvec3.h:98
const VecN & Copy(const VectorHandler &vh, integer iFirstIndex=1)
Definition: matvec3n.cc:116
std::ostream & operator<<(std::ostream &out, const MatNxN &m)
Definition: matvec3n.cc:1041
~MatNx3(void)
Definition: matvec3n.cc:784
void PutVec(integer iRow, const Vec3 &v)
Definition: matvec3n.cc:856
void Create_(integer ns)
Definition: matvec3n.cc:252
Definition: matvec3.h:59
~Mat3xN(void)
Definition: matvec3n.cc:312
void Create_(integer ns)
Definition: matvec3n.cc:737
integer iMaxRows
Definition: matvec3n.h:387
#define SAFEDELETEARR(pnt)
Definition: mynewmem.h:713
~VecN(void)
Definition: matvec3n.cc:127
Mat3x3 GetMat3x3(integer iFirstCol) const
Definition: matvec3n.cc:607
void Resize(integer ns)
Definition: matvec3n.cc:789
const Mat3xN & operator+=(const Mat3xN &m)
Definition: matvec3n.cc:459
void PutMat3x3(integer iCol, const Mat3x3 &m)
Definition: matvec3n.cc:628
#define NO_OP
Definition: myassert.h:74
MatNx3(void)
Definition: matvec3n.cc:757
Definition: matvec3.h:50
integer iNumRows
Definition: matvec3n.h:528
const MatNxN & Mult(const MatNx3 &m, const Mat3xN &n)
Definition: matvec3n.cc:1017
doublereal pdVec[3]
Definition: matvec3.h:109
integer iNumCols
Definition: matvec3n.h:235
virtual integer iGetSize(void) const =0
doublereal * pdVec
Definition: matvec3n.h:89
void Destroy_(void)
Definition: matvec3n.cc:264
doublereal ** pdMat
Definition: matvec3n.h:530
Vec3 GetVec(integer iRow) const
Definition: matvec3n.cc:844
~MatNxN(void)
Definition: matvec3n.cc:981
Definition: matvec3.h:58
integer Number(void) const
Definition: matvec3n.h:67
const Mat3xN & operator-=(const Mat3xN &m)
Definition: matvec3n.cc:473
integer iNumRows
Definition: matvec3n.h:88
void SubMat3x3(integer iCol, const Mat3x3 &m)
Definition: matvec3n.cc:676
Definition: matvec3.h:51
Definition: matvec3.h:55
const MatNx3 & Transpose(const Mat3xN &n)
Definition: matvec3n.cc:898
integer iMaxCols
Definition: matvec3n.h:234
Vec3 GetVec(integer iCol) const
Definition: matvec3n.cc:553
const MatNx3 & RightMult(const MatNx3 &n, const Mat3x3 &m)
Definition: matvec3n.cc:814
void Create_(integer ns)
Definition: matvec3n.cc:928
void SubVec(integer iRow, const Vec3 &v)
Definition: matvec3n.cc:884
Definition: matvec3.h:56
integer Offset(void) const
Definition: matvec3n.h:63
void Destroy_(void)
Definition: matvec3n.cc:944
Definition: matvec3.h:63
Definition: matvec3.h:62
Definition: matvec3.h:61
doublereal * pdRows[3]
Definition: matvec3n.h:236
void Reset(const doublereal d=0.)
Definition: matvec3n.cc:986
integer iMaxRows
Definition: matvec3n.h:87
Definition: matvec3.h:57
void Reset(const doublereal &d=0.)
Definition: matvec3n.cc:334
void AddVec(integer iRow, const Vec3 &v)
Definition: matvec3n.cc:870
void SubVec(integer iCol, const Vec3 &v)
Definition: matvec3n.cc:593
void RightMult(const MatNx3 &n, const Vec3 &v)
Definition: matvec3n.cc:157
Mat3x3 GetMat3x3ScalarMult(integer iFirstCol, const doublereal &d) const
Definition: matvec3n.cc:700
#define ASSERT(expression)
Definition: colamd.c:977
const VecN & operator*=(const doublereal &d)
Definition: matvec3n.cc:181
Vec3 operator*(const VecN &v) const
Definition: matvec3n.cc:488
void Resize(integer ns)
Definition: matvec3n.cc:318
integer Last(void) const
Definition: matvec3n.h:71
const VecN & Mult(const MatNxN &m, const VecN &n)
Definition: matvec3n.cc:192
void AddVec(integer iCol, const Vec3 &v)
Definition: matvec3n.cc:579
void Create_(integer ns)
Definition: matvec3n.cc:47
MatNxN(void)
Definition: matvec3n.cc:956
static std::stack< cleanup * > c
Definition: cleanup.cc:59
const Mat3xN & Mult(const Mat3xN &m, const MatNxN &n)
Definition: matvec3.h:49
doublereal pdMat[9]
Definition: matvec3.h:559
Definition: matvec3.h:60
void PutVec(integer iCol, const Vec3 &v)
Definition: matvec3n.cc:565
Definition: matvec3n.h:76
#define SAFENEWARR(pnt, item, sz)
Definition: mynewmem.h:701
void Destroy_(void)
Definition: matvec3n.cc:57
Mat3xN(void)
Definition: matvec3n.cc:272
doublereal * pdCols[3]
Definition: matvec3n.h:389
const Mat3xN & Copy(const Mat3xN &m)
Definition: matvec3n.cc:407
void Reset(const doublereal d=0.)
Definition: matvec3n.cc:147
void Reset(const doublereal d=0.)
Definition: matvec3n.cc:802
void AddMat3x3(integer iCol, const Mat3x3 &m)
Definition: matvec3n.cc:652
double doublereal
Definition: colamd.c:52
doublereal * pdVec
Definition: matvec3n.h:529
const VecN & operator+=(const VecN &m)
Definition: matvec3n.cc:169
integer Start(void) const
Definition: matvec3n.h:59
long int integer
Definition: colamd.c:51
VecN(void)
Definition: matvec3n.cc:65
const Mat3xN & operator*=(const doublereal &d)
Definition: matvec3n.cc:422
integer iNumRows
Definition: matvec3n.h:388
const Mat3xN & operator/=(const doublereal &d)
Definition: matvec3n.cc:439
void Destroy_(void)
Definition: matvec3n.cc:749
const MatNxN & Copy(const MatNxN &m)
Definition: matvec3n.cc:997