1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Alessandro Tasora, Radu Serban
13 // =============================================================================
14
15 #include "chrono/solver/ChVariablesGeneric.h"
16
17 namespace chrono {
18
19 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChVariablesGeneric)20 CH_FACTORY_REGISTER(ChVariablesGeneric)
21
22 ChVariablesGeneric::ChVariablesGeneric(int m_ndof) : ChVariables(m_ndof), ndof(m_ndof) {
23 Mmass.setIdentity(ndof, ndof);
24 inv_Mmass.setIdentity(ndof, ndof);
25 }
26
operator =(const ChVariablesGeneric & other)27 ChVariablesGeneric& ChVariablesGeneric::operator=(const ChVariablesGeneric& other) {
28 if (&other == this)
29 return *this;
30
31 // copy parent class data
32 ChVariables::operator=(other);
33
34 Mmass = other.Mmass;
35 inv_Mmass = other.inv_Mmass;
36
37 return *this;
38 }
39
40 // Computes the product of the inverse mass matrix by a vector, and add to result: result = [invMb]*vect
Compute_invMb_v(ChVectorRef result,ChVectorConstRef vect) const41 void ChVariablesGeneric::Compute_invMb_v(ChVectorRef result, ChVectorConstRef vect) const {
42 assert(result.size() == ndof);
43 assert(vect.size() == ndof);
44 result = inv_Mmass * vect;
45 }
46
47 // Computes the product of the inverse mass matrix by a vector, and increment result: result += [invMb]*vect
Compute_inc_invMb_v(ChVectorRef result,ChVectorConstRef vect) const48 void ChVariablesGeneric::Compute_inc_invMb_v(ChVectorRef result, ChVectorConstRef vect) const {
49 assert(result.size() == ndof);
50 assert(vect.size() == ndof);
51 result += inv_Mmass * vect;
52 }
53
54 // Computes the product of the mass matrix by a vector, and set in result: result = [Mb]*vect
Compute_inc_Mb_v(ChVectorRef result,ChVectorConstRef vect) const55 void ChVariablesGeneric::Compute_inc_Mb_v(ChVectorRef result, ChVectorConstRef vect) const {
56 assert(result.size() == ndof);
57 assert(vect.size() == ndof);
58 result += Mmass * vect;
59 }
60
61 // Computes the product of the corresponding block in the system matrix (ie. the mass matrix) by 'vect', scale by c_a,
62 // and add to 'result'.
63 // NOTE: the 'vect' and 'result' vectors must already have the size of the total variables&constraints in the system;
64 // the procedure will use the ChVariable offsets (that must be already updated) to know the indexes in result and vect.
MultiplyAndAdd(ChVectorRef result,ChVectorConstRef vect,const double c_a) const65 void ChVariablesGeneric::MultiplyAndAdd(ChVectorRef result, ChVectorConstRef vect, const double c_a) const {
66 result.segment(this->offset, ndof) += c_a * Mmass * vect.segment(this->offset, ndof);
67 }
68
69 // Add the diagonal of the mass matrix scaled by c_a, to 'result'.
70 // NOTE: the 'result' vector must already have the size of system unknowns, ie the size of the total variables &
71 // constraints in the system; the procedure will use the ChVariable offset (that must be already updated) as index.
DiagonalAdd(ChVectorRef result,const double c_a) const72 void ChVariablesGeneric::DiagonalAdd(ChVectorRef result, const double c_a) const {
73 result.segment(this->offset, ndof) += c_a * Mmass.diagonal();
74 }
75
Build_M(ChSparseMatrix & storage,int insrow,int inscol,const double c_a)76 void ChVariablesGeneric::Build_M(ChSparseMatrix& storage, int insrow, int inscol, const double c_a) {
77 for (int row = 0; row < Mmass.rows(); ++row)
78 for (int col = 0; col < Mmass.cols(); ++col)
79 storage.SetElement(insrow + row, inscol + col, c_a * Mmass(row, col));
80 }
81
82 } // end namespace chrono
83