MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
MLS.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/libraries/libmbmath/MLS.cc,v 1.15 2017/01/12 14:43:53 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  * Implemented by Michele Frumusa, probably also based on work of others
33  */
34 
35 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
36 
37 #ifdef USE_ANN
38 
39 #include "ac/lapack.h"
40 #include "MLS.h"
41 
42 void
43 RBF0::SetWeight(MyVectorHandler& r, SparseMatrixHandler& w_out) {
44  for (int it = 1; it <= r.iGetSize(); it++) {
45  w_out(it,it) = std::pow(1-(r(it)), 2);
46  }
47 }
48 
49 void
50 RBF2::SetWeight(MyVectorHandler& r, SparseMatrixHandler& w_out) {
51  for (int it = 1; it <= r.iGetSize(); it++) {
52  w_out(it,it) = std::pow(1-r(it), 4)*(4*r(it) + 1);
53  }
54 }
55 
56 void
57 RBF4::SetWeight(MyVectorHandler& r, SparseMatrixHandler& w_out) {
58  for (int it = 1; it <= r.iGetSize(); it++) {
59  w_out(it,it) = std::pow(1-r(it), 6) * (35*std::pow(r(it),2) + 18*r(it) + 3);
60  }
61 }
62 
63 void
64 Linear::SetP(double* PosFem, double** PosMb, int* id, unsigned int k, MyVectorHandler& p, FullMatrixHandler& P){
65  // Filling p vector
66  p(1) = 1.;
67  for (unsigned int i=0; i<3; i++){
68  p(i+2) = PosFem[i];
69  }
70  // Filling P matrix
71  for (unsigned int i=0; i< k; i++){
72  P(i+1,1) = 1.;
73  for (unsigned int j=0; j<3; j++){
74  P(i+1,j+2) = PosMb[id[i]][j];
75  }
76  }
77 }
78 
79 void
80 Quadratic::SetP(double* PosFem, double** PosMb, int* id, unsigned int k, MyVectorHandler& p, FullMatrixHandler& P){
81  // Filling p vector
82  p(1) = 1.;
83  for (unsigned int i=0; i<3; i++){
84  p(i+2) = PosFem[i];
85  }
86  for (unsigned int i=0; i<3; i++){
87  p(i+5) = PosFem[i] * PosFem[0];
88  for (unsigned int i=0; i<2; i++){
89  p(i+8)= PosFem[i+1] * PosFem[1];
90  }
91  }
92  p(10) = PosFem[2]*PosFem[2];
93  // Filling P matrix
94  for (unsigned int i=0; i<k; i++){
95  P(i+1,1) = 1;
96  for (unsigned int j=0; j<3; j++){
97  P(i+1,j+2) = PosMb[id[i]][j];
98  }
99  for (unsigned int j=0; j<3; j++){
100  P(i+1,j+5) = PosMb[id[i]][j] * PosMb[id[i]][0];
101  }
102  for (unsigned int j=0; j<2; j++){
103  P(i+1,j+8) = PosMb[id[i]][j+1] * PosMb[id[i]][1];
104  }
105  P(i+1,10) = PosMb[id[i]][2] * PosMb[id[i]][2];
106  }
107 
108 }
109 
110 MLSP::MLSP(unsigned int NodesN, int WgType, bool LinQuad)
111 :N(NodesN), pP(), pW(), pp()
112 {
113  // Weight Matrix
114  switch (WgType) {
115 
116  case 0:
117  pWg = new(RBF0);
118  std::cout << "C0 RBF" << std::endl;
119  break;
120  case 2:
121  pWg = new(RBF2);
122  std::cout << "C2 RBF" << std::endl;
123  break;
124  case 4:
125  pWg = new(RBF4);
126  std::cout << "C4 RBF" << std::endl;
127  break;
128  }
129  // p and P matrix
130  if (LinQuad) {
131  SAFENEWWITHCONSTRUCTOR(pOr , Quadratic , Quadratic() );
132  /*SAFENEWWITHCONSTRUCTOR(pp , MyVectorHandler , MyVectorHandler(10) );
133  SAFENEWWITHCONSTRUCTOR(pP , FullMatrixHandler , FullMatrixHandler(N,10) );
134  SAFENEWWITHCONSTRUCTOR(pW , SpMapMatrixHandler , SpMapMatrixHandler(N,N) );*/
135  pp.ResizeReset(10);
136  pP.ResizeReset(N,10);
137  pW.ResizeReset(N,N);
138  } else {
139  SAFENEWWITHCONSTRUCTOR(pOr , Linear , Linear() );
140  /*SAFENEWWITHCONSTRUCTOR(pp , MyVectorHandler , MyVectorHandler(4) );
141  SAFENEWWITHCONSTRUCTOR(pP , FullMatrixHandler , FullMatrixHandler(N,4) );
142  SAFENEWWITHCONSTRUCTOR(pW , SpMapMatrixHandler , SpMapMatrixHandler(N,N) );*/
143  pp.ResizeReset(4);
144  pP.ResizeReset(N,4);
145  pW.ResizeReset(N,N);
146  }
147 }
148 
149 void
150 MLSP::Interpolate(const GeometryData& fem_data, const GeometryData& mb_data, SpMapMatrixHandler* pH)
151 {
152  // Dimension of space
153  unsigned dim = 3;
154 
155  // Data size
156  unsigned mb_size = pH->iGetNumCols();
157  unsigned fem_size = pH->iGetNumRows();
158 
159  // Data for ANN search
160  ANNpointArray data = annAllocPts(mb_size,dim);
161  ANNpoint query = annAllocPt(dim);
162  ANNidxArray idx = new ANNidx[N];
163  ANNdistArray dist = new ANNdist[N];
164 
165  // Filling ANN structures with data from model
166  for (unsigned i = 0 ; i < mb_size ; i++){
167  for (unsigned j = 0 ; j < dim ; j++){
168  data[i][j] = mb_data.data[i].X(j+1);
169  }
170  }
171 
172  SAFENEWWITHCONSTRUCTOR(pTree , ANNkd_tree , ANNkd_tree(data, mb_size , dim));
173 
174  // Starting search
175  for (unsigned i = 0; i < fem_size ; i++){
176  // Query
177  for (unsigned j = 0 ; j < dim ; j++){
178  query[j] = fem_data.data[i].X(j+1);
179  }
180  // Search
181  pTree->annkSearch(query, N, idx, dist);
182  // Setting p and P
183  pOr->SetP(query, data, idx, N, pp, pP);
184  // Computing distances
185  MyVectorHandler r(N);
186  double dm = sqrt(dist[N-1]);
187  for (unsigned j = 0; j < N ; j++){
188  r(j+1) = sqrt(dist[j])/(1.05*dm);
189  }
190  // Setting Weight
191  pWg->SetWeight(r,pW);
192  // Computing Matrix
193  int d2 = pP.iGetNumCols();
194  FullMatrixHandler A(d2,d2),B(d2,N);;
195  pP.MatTMatMul(B,pW);
196  B.MatMatMul(A,pP);
197  int ord = N;
198  int info;
199  double* S = new double[d2];
200  double rcond = -1.;
201  int rank;
202  int nvl = int(2*log(d2/26))+1;
203  int lwork =12*d2 + 2*d2*25 + 8*d2*nvl + d2*ord + 676;
204  double* work = new double[lwork];
205  int* iwork = new int[(3 * d2 * nvl + 11 * d2)];
206  __FC_DECL__(dgelsd)(&d2, &d2, &ord, A.pdGetMat(),
207  &d2, B.pdGetMat(), &d2, S, &rcond, &rank, work, &lwork, iwork, &info);
208  delete [] S;
209  delete [] work;
210  delete [] iwork;
211  MyVectorHandler h(N);
212  B.MatTVecMul(h,pp);
213  for (unsigned int j = 0 ; j < N ; j++){
214  pH->operator()(i+1, idx[j]+1) = h(j+1);
215  }
216 
217 
218  }
219 }
220 
221 void
222 MLSP::Interpolate_Adj(const GeometryData& fem_data, const GeometryData& mb_data, SpMapMatrixHandler* pH,
223  const std::vector<Vec3>& Adj)
224 {
225  // Dimension of space
226  unsigned dim = 3;
227 
228  // Data size
229  unsigned mb_size = pH->iGetNumCols();
230  unsigned fem_size = pH->iGetNumRows();
231  unsigned Nadj = Adj.size();
232  unsigned mb_base_size = mb_size/(Nadj+1);
233 
234  unsigned k = N/(Nadj+1);
235 
236  // Data for ANN search
237  ANNpointArray data = annAllocPts(mb_size,dim);
238  ANNpointArray data_temp = annAllocPts(mb_base_size,dim);
239  ANNpoint query = annAllocPt(dim);
240  ANNidxArray idx = new ANNidx[N];
241  ANNidxArray idx_temp = new ANNidx[k];
242  ANNdistArray dist = new ANNdist[N];
243  ANNdistArray dist_temp = new ANNdist[k];
244 
245  // Filling ANN structures with data from model
246  for (unsigned i = 0 ; i < mb_base_size ; i++){
247  for (unsigned j = 0 ; j < dim ; j++){
248  data_temp[i][j] = mb_data.data[i].X(j+1);
249  data[i*(Nadj+1)][j] = mb_data.data[i].X(j+1);
250  for (unsigned i_a = 0; i_a < Nadj ; i_a++){
251  for (unsigned d = 0; d < dim ; d++){
252  data[i*(Nadj+1)+i_a+1][d] = mb_data.data[i].X(d+1)+Adj[i_a](d+1);
253  }
254  }
255  }
256  }
257 
258  SAFENEWWITHCONSTRUCTOR(pTree , ANNkd_tree , ANNkd_tree(data_temp, mb_base_size , dim));
259 
260  // Starting search
261  for (unsigned i = 0; i < fem_size ; i++){
262  // Query
263  for (unsigned j = 0 ; j < dim ; j++){
264  query[j] = fem_data.data[i].X(j+1);
265  }
266  // Search
267  pTree->annkSearch(query, k, idx_temp, dist_temp);
268  // Inflating with adjoint nodes
269  for (unsigned i_node = 0; i_node < k ; i_node++) {
270  idx[i_node*(Nadj+1)] = idx_temp[i_node]*(Nadj+1);
271  dist[i_node*(Nadj+1)] = dist_temp[i_node];
272  unsigned count_a = 1;
273  for (std::vector<Vec3>::const_iterator it_adj = Adj.begin();
274  it_adj != Adj.end(); ++it_adj)
275  {
276  ANNpoint adj = annAllocPt(dim);
277  for (unsigned int d = 0 ; d < dim ; d++) {
278  adj[d] = data_temp[idx_temp[i_node]][d]+it_adj->dGet(d+1);
279  }
280  ANNdist dist_adj = annDist(dim,query,adj);
281  idx[i_node*(Nadj+1)+count_a] = idx_temp[i_node]*(Nadj+1)+count_a;
282  dist[i_node*(Nadj+1)+count_a] = dist_adj;
283  }
284  }
285  pOr->SetP(query, data, idx, N, pp, pP);
286  // Computing distances
287  MyVectorHandler r(N);
288  double dm = sqrt(dist[N-1]);
289  for (unsigned j = 0; j < N ; j++){
290  r(j+1) = sqrt(dist[j])/(1.05*dm);
291  }
292  // Setting Weight
293  pWg->SetWeight(r,pW);
294  // Computing Matrix
295  int d2 = pP.iGetNumCols();
296  FullMatrixHandler A(d2,d2),B(d2,N);;
297  pP.MatTMatMul(B,pW);
298  B.MatMatMul(A,pP);
299  int ord = N;
300  int info;
301  double* S = new double[d2];
302  double rcond = -1.;
303  int rank;
304  int nvl = int(2*log(d2/26))+1;
305  int lwork =12*d2 + 2*d2*25 + 8*d2*nvl + d2*ord + 676;
306  double* work = new double[lwork];
307  int* iwork = new int[(3 * d2 * nvl + 11 * d2)];
308  __FC_DECL__(dgelsd)(&d2, &d2, &ord, A.pdGetMat(),
309  &d2, B.pdGetMat(), &d2, S, &rcond, &rank, work, &lwork, iwork, &info);
310  delete [] S;
311  delete [] work;
312  delete [] iwork;
313  MyVectorHandler h(N);
314  B.MatTVecMul(h,pp);
315  for (unsigned int j = 0 ; j < N ; j++){
316  pH->operator()(i+1, idx[j]+1) = h(j+1);
317  }
318  }
319 }
320 
321 #endif // USE_ANN
grad::pow
GradientExpression< BinaryExpr< FuncPow, LhsExpr, RhsExpr > > pow(const GradientExpression< LhsExpr > &u, const GradientExpression< RhsExpr > &v)
Definition: gradient.h:2961
SparseMatrixHandler::iGetNumRows
integer iGetNumRows(void) const
Definition: spmh.h:113
GeometryData::data
std::vector< Geometry > data
Definition: geomdata.h:81
MyVectorHandler::iGetSize
virtual integer iGetSize(void) const
Definition: vh.h:255
SparseMatrixHandler::iGetNumCols
integer iGetNumCols(void) const
Definition: spmh.h:117
grad::log
GradientExpression< UnaryExpr< FuncLog, Expr > > log(const GradientExpression< Expr > &u)
Definition: gradient.h:2976
grad::sqrt
GradientExpression< UnaryExpr< FuncSqrt, Expr > > sqrt(const GradientExpression< Expr > &u)
Definition: gradient.h:2974
SAFENEWWITHCONSTRUCTOR
#define SAFENEWWITHCONSTRUCTOR(pnt, item, constructor)
Definition: mynewmem.h:698