1 /*
2 *_________________________________________________________________________*
3 * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
4 * DESCRIPTION: SEE READ-ME *
5 * FILE NAME: workspace.cpp *
6 * AUTHORS: See Author List *
7 * GRANTS: See Grants List *
8 * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
9 * LICENSE: Please see License Agreement *
10 * DOWNLOAD: Free at www.rpi.edu/~anderk5 *
11 * ADMINISTRATOR: Prof. Kurt Anderson *
12 * Computational Dynamics Lab *
13 * Rensselaer Polytechnic Institute *
14 * 110 8th St. Troy NY 12180 *
15 * CONTACT: anderk5@rpi.edu *
16 *_________________________________________________________________________*/
17
18
19 #include "workspace.h"
20 #include "system.h"
21 #include "solver.h"
22 #include "SystemProcessor.h"
23 #include <iostream>
24 #include <fstream>
25 #include <iomanip>
26 #include <cmath>
27
28
29 using namespace std;
30
allocateNewSystem()31 void Workspace::allocateNewSystem() {
32 currentIndex++;
33 if(currentIndex < maxAlloc)
34 {
35 system[currentIndex].system = new System;
36 }
37 else
38 {
39 maxAlloc = (maxAlloc + 1) * 2;
40
41 SysData * tempPtrSys = new SysData[maxAlloc];
42 for(int i = 0; i < currentIndex; i++)
43 {
44 tempPtrSys[i] = system[i];
45 }
46 delete [] system;
47 system = tempPtrSys;
48 system[currentIndex].system = new System;
49 }
50 }
51
Workspace()52 Workspace::Workspace(){
53 currentIndex = -1;
54 maxAlloc = 0;
55 system = nullptr;
56 }
57
~Workspace()58 Workspace::~Workspace(){
59 for(int i = 0; i <= currentIndex; i++){
60 delete system[i].system;
61 }
62 delete [] system;
63 }
64
65
LoadFile(char * filename)66 bool Workspace::LoadFile(char* filename){
67 bool ans;
68 ifstream file;
69 file.open(filename, ifstream::in);
70 if( !file.is_open() ){
71 cerr << "File '" << filename << "' not found." << endl;
72 return false;
73 }
74 allocateNewSystem();
75 ans = system[currentIndex].system->ReadIn(file);
76 file.close();
77
78 return ans;
79 }
80
SetLammpsValues(double dtv,double dthalf,double tempcon)81 void Workspace::SetLammpsValues(double dtv, double dthalf, double tempcon){
82 Thalf = dthalf;
83 Tfull = dtv;
84 ConFac = tempcon;
85 }
86
87
MakeSystem(int & nbody,double * & masstotal,double ** & inertia,double ** & xcm,double ** & vcm,double ** & omega,double ** & ex_space,double ** & ey_space,double ** & ez_space,int & njoint,int ** & jointbody,double ** & xjoint,int & nfree,int * freelist,double dthalf,double dtv,double tempcon,double KE)88 bool Workspace::MakeSystem(int& nbody, double *&masstotal, double **&inertia, double **&xcm, double **&vcm, double **&omega, double **&ex_space, double **&ey_space, double **&ez_space, int &njoint, int **&jointbody, double **&xjoint, int& nfree, int*freelist, double dthalf, double dtv, double tempcon, double KE){
89
90 SetLammpsValues(dtv, dthalf, tempcon);
91
92 if(njoint){
93 SystemProcessor sys;
94 sys.processArray(jointbody, njoint);
95 List<POEMSChain> * results = sys.getSystemData();
96
97 int numsyschains = results->GetNumElements();
98 int headvalue = 0;
99 List<POEMSChain> * newresults = results;
100 ListElement<POEMSChain> * tempNode = results->GetHeadElement();
101 int stop = 1;
102 int counter = 1;
103 for(int n = 1; n<=numsyschains; n++){
104 while(stop){
105 if ( (*(tempNode->value->listOfNodes.GetHeadElement()->value) == (headvalue+1) ) || (*(tempNode->value->listOfNodes.GetTailElement()->value) == (headvalue+1) ) ) {
106 newresults->Append(tempNode->value);
107 headvalue = headvalue + tempNode->value->listOfNodes.GetNumElements();
108 tempNode = results->GetHeadElement();
109 stop = 0;
110 counter ++;
111 }
112 else{
113 tempNode = tempNode->next;
114 }
115 }
116 stop=1;
117 }
118
119 ListElement<POEMSChain> * TNode = newresults->GetHeadElement();
120 ListElement<POEMSChain> * TTNode = newresults->GetHeadElement();
121 for(int kk=1; kk<=numsyschains; kk++){
122 if(kk!=numsyschains)
123 TTNode = TNode->next;
124 newresults->Remove(TNode);
125 if(kk!=numsyschains)
126 TNode = TTNode;
127 }
128 ListElement<POEMSChain> * NodeValue = newresults->GetHeadElement();
129 int count = 0;
130 int * array;
131 int ** arrayFromChain;
132 int numElementsInSystem;
133 int ttk = 0;
134
135
136 while(NodeValue != nullptr) {
137 array = new int[NodeValue->value->listOfNodes.GetNumElements()];
138 arrayFromChain = NodeValue->value->listOfNodes.CreateArray();
139 numElementsInSystem = NodeValue->value->listOfNodes.GetNumElements();
140 for(counter = 0; counter < numElementsInSystem; counter++){
141 array[counter] = *arrayFromChain[counter];
142 }
143
144 SetKE(1,KE);
145 allocateNewSystem();
146 system[currentIndex].system->Create_System_LAMMPS(nbody,masstotal,inertia,xcm,xjoint,vcm,omega,ex_space,ey_space,ez_space, numElementsInSystem, array, count);
147
148 system[currentIndex].solver = ONSOLVER;
149 ttk = ttk + 1;
150 count = ttk;
151 delete [] array;
152 delete [] arrayFromChain;
153 NodeValue= NodeValue->next;
154 }
155 }
156 if(nfree){
157 MakeDegenerateSystem(nfree,freelist,masstotal,inertia,xcm,vcm,omega,ex_space,ey_space,ez_space);
158 }
159 return true;
160 }
161
162
MakeDegenerateSystem(int & nfree,int * freelist,double * & masstotal,double ** & inertia,double ** & xcm,double ** & vcm,double ** & omega,double ** & ex_space,double ** & ey_space,double ** & ez_space)163 bool Workspace::MakeDegenerateSystem(int& nfree, int*freelist, double *&masstotal, double **&inertia, double **&xcm, double **&vcm, double **&omega, double **&ex_space, double **&ey_space, double **&ez_space){
164 allocateNewSystem();
165 system[currentIndex].system->Create_DegenerateSystem(nfree,freelist,masstotal,inertia,xcm,vcm,omega,ex_space,ey_space,ez_space);
166 system[currentIndex].solver = ONSOLVER;
167 return true;
168 }
169
SaveFile(char * filename,int index)170 bool Workspace::SaveFile(char* filename, int index){
171 if(index < 0){
172 index = currentIndex;
173 }
174 ofstream file;
175
176 file.open(filename, ofstream::out);
177
178 if( !file.is_open() ){
179 cerr << "File '" << filename << "' could not be opened." << endl;
180 return false;
181 }
182 if(index >= 0 && index <= currentIndex){
183 system[index].system->WriteOut(file);
184 }
185 else {
186 cerr << "Error, requested system index " << index << ", minimum index 0 and maximum index " << currentIndex << endl;
187 }
188 file.close();
189 return true;
190 }
191
192
GetSystem(int index)193 System* Workspace::GetSystem(int index){
194 if(index <= currentIndex){
195 if(index >= 0){
196 return system[index].system;
197 }
198 else{
199 return system[currentIndex].system;
200 }
201 }
202 else{
203 return nullptr;
204 }
205 }
206
AddSolver(Solver * s,int index)207 void Workspace::AddSolver(Solver* s, int index){
208 if(currentIndex >= index){
209 if(index >= 0){
210 system[index].solver = (int)s->GetSolverType();
211 }
212 else{
213 system[currentIndex].solver = (int)s->GetSolverType();
214 }
215 }
216 else{
217 cout << "Error adding solver to index " << index << endl;
218 }
219 }
220
getNumberOfSystems()221 int Workspace::getNumberOfSystems(){
222 return currentIndex + 1;
223 }
224
225
226
SetKE(int temp,double SysKE)227 void Workspace::SetKE(int temp, double SysKE){
228 KE_val = SysKE;
229 FirstTime =temp;
230 }
231
232
LobattoOne(double ** & xcm,double ** & vcm,double ** & omega,double ** & torque,double ** & fcm,double ** & ex_space,double ** & ey_space,double ** & ez_space)233 void Workspace::LobattoOne(double **&xcm, double **&vcm,double **&omega,double **&torque, double **&fcm, double **&ex_space, double **&ey_space, double **&ez_space){
234
235 int numsys = currentIndex;
236 int numbodies;
237 double time = 0.0;
238 int * mappings;
239 double SysKE=0.0;
240
241 for (int i = 0; i <= numsys; i++){
242 mappings = system[i].system->GetMappings();
243 numbodies = system[i].system->GetNumBodies() - 1;
244 Matrix FF(6,numbodies);
245 FF.Zeros();
246 for (int j=1; j<=numbodies; j++){
247 FF(1,j) = torque[mappings[j - 1]-1][0]*ConFac;
248 FF(2,j) = torque[mappings[j - 1]-1][1]*ConFac;
249 FF(3,j) = torque[mappings[j - 1]-1][2]*ConFac;
250
251 FF(4,j) = fcm[mappings[j - 1]-1][0]*ConFac;
252 FF(5,j) = fcm[mappings[j - 1]-1][1]*ConFac;
253 FF(6,j) = fcm[mappings[j - 1]-1][2]*ConFac;
254 }
255
256 //------------------------------------//
257 // Get a solver and solve that system.
258 Solver * theSolver = Solver::GetSolver((SolverType)system[i].solver);
259 theSolver->SetSystem(system[i].system);
260 theSolver->Solve(time, FF);
261
262
263 theSolver->Solve(time, FF);
264 ColMatrix tempx = *((*theSolver).GetState());
265 ColMatrix tempv = *((*theSolver).GetStateDerivative());
266 ColMatrix tempa = *((*theSolver).GetStateDerivativeDerivative());
267
268
269 for(int numint =0 ; numint<3; numint++){
270 theSolver->Solve(time, FF);
271 tempa = *((*theSolver).GetStateDerivativeDerivative());
272 *((*theSolver).GetStateDerivative())= tempv + Thalf*tempa;
273 }
274
275 ColMatrix TempV= *((*theSolver).GetStateDerivative());
276 *((*theSolver).GetState())= tempx + Tfull*TempV;
277
278 int numjoints = system[i].system->joints.GetNumElements();
279 for(int k = 0; k < numjoints; k++){
280 system[i].system->joints(k)->ForwardKinematics();
281 }
282
283 for(int k = 0; k < numbodies; k++){
284 Vect3 temp1 =system[i].system->bodies(k+1)->r;
285 Vect3 temp2 =system[i].system->bodies(k+1)->v;
286 Vect3 temp3 =system[i].system->bodies(k+1)->omega;
287 Mat3x3 temp4 =system[i].system->bodies(k+1)->n_C_k;
288 for(int m = 0; m < 3; m++){
289 xcm[mappings[k]-1][m] = temp1(m+1);
290 vcm[mappings[k]-1][m] = temp2(m+1);
291 omega[mappings[k]-1][m] = temp3(m+1);
292 ex_space[mappings[k]-1][m] = temp4(m+1,1);
293 ey_space[mappings[k]-1][m] = temp4(m+1,2);
294 ez_space[mappings[k]-1][m] = temp4(m+1,3);
295 }
296 SysKE = SysKE + system[i].system->bodies(k+1)->KE;
297 }
298 delete theSolver;
299 }
300 }
301
LobattoTwo(double ** & vcm,double ** & omega,double ** & torque,double ** & fcm)302 void Workspace::LobattoTwo(double **&vcm,double **&omega,double **&torque, double **&fcm){
303 int numsys = currentIndex;
304 int numbodies;
305 double time = 0.0;
306 int * mappings;
307 double SysKE =0.0;
308 for (int i = 0; i <= numsys; i++){
309 mappings = system[i].system->GetMappings();
310 numbodies = system[i].system->GetNumBodies() - 1;
311 Matrix FF(6,numbodies);
312
313 for (int j=1; j<=numbodies; j++){
314 FF(1,j) = torque[mappings[j - 1]-1][0]*ConFac;
315 FF(2,j) = torque[mappings[j - 1]-1][1]*ConFac;
316 FF(3,j) = torque[mappings[j - 1]-1][2]*ConFac;
317 FF(4,j) = fcm[mappings[j - 1]-1][0]*ConFac;
318 FF(5,j) = fcm[mappings[j - 1]-1][1]*ConFac;
319 FF(6,j) = fcm[mappings[j - 1]-1][2]*ConFac;
320 }
321
322 //------------------------------------//
323 // Get a solver and solve that system.
324 Solver * theSolver = Solver::GetSolver((SolverType)system[i].solver);
325 theSolver->SetSystem(system[i].system);
326 theSolver->Solve(time, FF);
327 ColMatrix tempv = *((*theSolver).GetStateDerivative());
328 ColMatrix tempa = *((*theSolver).GetStateDerivativeDerivative());
329 *((*theSolver).GetStateDerivative()) = tempv + Thalf*tempa;
330
331
332 int numjoints = system[i].system->joints.GetNumElements();
333 for(int k = 0; k < numjoints; k++){
334 system[i].system->joints(k)->ForwardKinematics();
335 }
336
337 for(int k = 0; k < numbodies; k++){
338 Vect3 temp1 =system[i].system->bodies(k+1)->r;
339 Vect3 temp2 =system[i].system->bodies(k+1)->v;
340 Vect3 temp3 =system[i].system->bodies(k+1)->omega;
341 SysKE = SysKE + system[i].system->bodies(k+1)->KE;
342 for(int m = 0; m < 3; m++){
343 vcm[mappings[k]-1][m] = temp2(m+1);
344 omega[mappings[k]-1][m] = temp3(m+1);
345 }
346 }
347 delete theSolver;
348 }
349 }
350
351
352
RKStep(double ** & xcm,double ** & vcm,double ** & omega,double ** & torque,double ** & fcm,double ** & ex_space,double ** & ey_space,double ** & ez_space)353 void Workspace::RKStep(double **&xcm, double **&vcm,double **&omega,double **&torque, double **&fcm, double **&ex_space, double **&ey_space, double **&ez_space){
354
355 double a[6];
356 double b[6][6];
357 double c[6];
358 //double e[6];
359
360 a[1] = 0.2;
361 a[2] = 0.3;
362 a[3] = 0.6;
363 a[4] = 1.0;
364 a[5] = 0.875;
365
366 b[1][0] = 0.2;
367 b[2][0] = 0.075; b[2][1] = 0.225;
368 b[3][0] = 0.3; b[3][1] = -0.9; b[3][2] = 1.2;
369 b[4][0] = -11.0/54.0; b[4][1] = 2.5; b[4][2] = -70.0/27.0; b[4][3] = 35.0/27.0;
370 b[5][0] = 1631.0/55296.0; b[5][1] = 175.0/512.0; b[5][2] = 575.0/13824.0; b[5][3] = 44275.0/110592.0; b[5][4] = 253.0/4096.0;
371
372 c[0] = 37.0/378.0;
373 c[1] = 0.0;
374 c[2] = 250.0/621.0;
375 c[3] = 125.0/594.0;
376 c[4] = 0.0;
377 c[5] = 512.0/1771.0;
378
379 int numsys = currentIndex;
380 int numbodies;
381 double time = 0.0;
382 int * mappings;
383 double SysKE =0.0;
384
385
386 for (int i = 0; i <= numsys; i++){
387 mappings = system[i].system->GetMappings();
388
389 numbodies = system[i].system->GetNumBodies() - 1;
390 Matrix FF(6,numbodies);
391 for (int j=1; j<=numbodies; j++){
392 FF(1,j) = ConFac*torque[mappings[j - 1]-1][0];
393 FF(2,j) = ConFac*torque[mappings[j - 1]-1][1];
394 FF(3,j) = ConFac*torque[mappings[j - 1]-1][2];
395
396 FF(4,j) = ConFac*fcm[mappings[j - 1]-1][0];
397 FF(5,j) = ConFac*fcm[mappings[j - 1]-1][1];
398 FF(6,j) = ConFac*fcm[mappings[j - 1]-1][2];
399 }
400
401
402 //------------------------------------//
403 // Get a solver and solve that system.
404 Solver * theSolver = Solver::GetSolver((SolverType)system[i].solver);
405 theSolver->SetSystem(system[i].system);
406 theSolver->Solve(time, FF);
407
408 ColMatrix initial_x;
409 ColMatrix initial_xdot;
410 ColMatMap* x;
411 ColMatMap* xdot;
412 ColMatMap* xdotdot;
413
414 x = theSolver->GetState();
415 xdot = theSolver->GetStateDerivative();
416 xdotdot=theSolver->GetStateDerivativeDerivative();
417
418 initial_x = *x;
419 initial_xdot = *xdot;
420 ColMatrix f[6];
421 ColMatrix ff[6];
422
423 ff[0] = initial_xdot;
424 f[0] = *xdotdot;
425
426 for(int ii=1;ii<6;ii++){
427 time = a[ii] * Tfull;
428 (*x) = initial_x;
429 (*xdot) = initial_xdot;
430 for(int j=0;j<ii;j++){
431 (*x) = (*x) + (b[ii][j]*Tfull)*ff[j];
432 (*xdot) = (*xdot) + (b[ii][j]*Tfull)*f[j];
433 }
434 theSolver->Solve(time,FF);
435 f[ii] = (*xdotdot);
436 ff[ii] = (*xdot);
437 }
438
439
440 (*x) = initial_x + (c[0]*Tfull)*ff[0] + (c[2]*Tfull)*ff[2] + (c[3]*Tfull)*ff[3] + (c[5]*Tfull)*ff[5];
441
442 (*xdot) = initial_xdot + (c[0]*Tfull)*f[0] + (c[2]*Tfull)*f[2] + (c[3]*Tfull)*f[3] + (c[5]*Tfull)*f[5];
443
444
445 int numjoints = system[i].system->joints.GetNumElements();
446 for(int k = 0; k < numjoints; k++){
447 system[i].system->joints(k)->ForwardKinematics();
448 }
449
450
451 for(int k = 0; k < numbodies; k++){
452 Vect3 temp1 =system[i].system->bodies(k+1)->r;
453 Vect3 temp2 =system[i].system->bodies(k+1)->v;
454 Vect3 temp3 =system[i].system->bodies(k+1)->omega;
455 Mat3x3 temp4 =system[i].system->bodies(k+1)->n_C_k;
456 SysKE = SysKE + system[i].system->bodies(k+1)->KE;
457 for(int m = 0; m < 3; m++){
458 xcm[mappings[k]-1][m] = temp1(m+1);
459 vcm[mappings[k]-1][m] = temp2(m+1);
460 omega[mappings[k]-1][m] = temp3(m+1);
461
462 ex_space[mappings[k]-1][m] = temp4(m+1,1);
463 ey_space[mappings[k]-1][m] = temp4(m+1,2);
464 ez_space[mappings[k]-1][m] = temp4(m+1,3);
465 }
466 }
467 delete theSolver;
468 }
469 }
470
471
WriteFile(char * filename)472 void Workspace::WriteFile(char * filename){
473 int numsys = currentIndex;
474 int numbodies;
475
476
477 for (int i = 0; i <= numsys; i++){
478 numbodies = system[i].system->GetNumBodies() - 1;
479 ofstream outfile;
480 outfile.open(filename,ofstream::out | ios::app);
481 outfile << numbodies<<endl;
482 outfile << "Atoms "<<endl;
483 for(int k = 0; k < numbodies; k++){
484 Vect3 temp1 =system[i].system->bodies(k+1)->r;
485 outfile<<1<<"\t"<<temp1(1)<<"\t"<<temp1(2)<<"\t"<<temp1(3)<<endl;
486 }
487 outfile.close();
488 }
489 }
490