coupling/MotionSolver.h

Go to the documentation of this file.
00001 // -*- C++ -*-
00002 
00003 #ifndef MOTION_SOLVER_H
00004 #define MOTION_SOLVER_H
00005 
00012 #include "Solver.h"
00013 
00014 #include "Operations.h"
00015 #include "DH_Link.h"
00016 #include "DH_Chain.h"
00017 #include "Scene.h"
00018 
00019 #include "Curve.h"
00020 #include "SplineCurve.h"
00021 #include "Surface.h"
00022 #include "Loft.h"
00023 #include "Assembly.h"
00024 
00025 #include "MotionControls.h"
00026 
00032 //template <class DataType, int dim> class MotionELCCoupledSolver;
00033 
00034 template <class DataType, int dim>
00035 class MotionSolver : public Solver {
00036   typedef Solver base;
00037 public:
00038   typedef ads::FixedArray<dim,DataType> point_type;
00039   typedef ads::FixedArray<dim,int> multi_index_type;
00040 
00041   typedef Joint<DataType,dim> JointType;
00042   typedef DH_Link<DataType,dim> LinkType;
00043   typedef DH_Chain<DataType,dim> ChainType;
00044   typedef Scene<DataType,dim> SceneType;
00045   typedef Assembly<DataType,dim> AssemblyType;
00046   typedef Part<DataType,dim> PartType;
00047   typedef Curve<DataType,dim> CurveType;
00048   typedef SplineCurve<DataType,dim> SplineCurveType;
00049   typedef Surface<DataType> SurfaceType;
00050   typedef Loft<DataType> LoftType;
00051   typedef Facet<DataType> FacetType;
00052 
00053   MotionSolver() : base(), Tr(0.), scale(1.), dtret(1.), brep_filetype(0), num_vertices(0), num_connections(0), steps(0),
00054        initial_vertices(0), vertices(0), velocities(0), connections(0), nodeids(0), pressures(0), _bolog(0) {
00055     OutputName="-";
00056     CheckpointName="motion";
00057     for (int i=0; i<dim; i++) xs[i]=0.;
00058     pi=4.0*std::atan(1.);
00059     log_every = 10;
00060   }
00061 
00062   ~MotionSolver() {}
00063 
00064   inline std::ostream& log() { return *_bolog; }
00065 
00070   virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) {
00071     char VariableName[16];
00072     char VariableName1[16];
00073 
00074     LocCtrl = Ctrl.getSubDevice(prefix+"MotionSolver");
00075 
00076     RegisterAt(LocCtrl,"MotionRestart",motionRestart);
00077     RegisterAt(LocCtrl,"NumAssemblies",numAssemblies);
00079 
00080     for (register int i=0; i < maxAssemblies; i++) {
00081 
00082       std::sprintf(VariableName,"Assembly(%02d)",i+1);
00083       AssemblyCtrl[i].register_at(LocCtrl,std::string(VariableName));
00084     }
00085 
00086     RegisterAt(LocCtrl,"FileType",brep_filetype);
00087     RegisterAt(LocCtrl,"InputBrep",brep_filename);
00088     RegisterAt(LocCtrl,"VelocityFile",vel_filename);
00089     RegisterAt(LocCtrl,"OutputName",OutputName);
00090     RegisterAt(LocCtrl,"CheckpointName",CheckpointName);
00091 
00092     RegisterAt(LocCtrl,"Scale",scale);
00093     RegisterAt(LocCtrl,"Center(1)",xs[0]);
00094     RegisterAt(LocCtrl,"Center(2)",xs[1]);
00095     RegisterAt(LocCtrl,"Center(3)",xs[2]);
00096     RegisterAt(LocCtrl,"RotationDuration",Tr);
00097     RegisterAt(LocCtrl,"dt",dtret);
00098 
00099     RegisterAt(LocCtrl,"log_every",log_every);
00100 
00102     RegisterAt(LocCtrl,"NumDHJoints",numDHJoints);
00103     for (register int j=0; j < JointLimit; j++) {
00104       std::sprintf(VariableName1,"Joint(%02d)",j+1);
00105       JointCtrls[j].register_at(LocCtrl,std::string(VariableName1));
00106     }
00107 
00108     RegisterAt(LocCtrl,"NumDHLinks",numDHLinks);
00109     for (register int j=0; j < maxLinks; j++) {
00110       std::sprintf(VariableName1,"Link(%02d)",j+1);
00111       LinkCtrls[j].register_at(LocCtrl,std::string(VariableName1));
00112     }
00113 
00114     RegisterAt(LocCtrl,"NumDHChains",numDHChains);
00115     for (register int i=0; i < ChainLimit; i++) {
00116       std::sprintf(VariableName,"DHChain(%02d)",i+1);
00117       DHChainCtrls[i].register_at(LocCtrl,std::string(VariableName));
00118     }
00119 
00120   }
00121   virtual void register_at(ControlDevice& Ctrl) {
00122     register_at(Ctrl,"");
00123   }
00124 
00125   virtual bool specificSetup() { return 0; }
00126 
00127   virtual bool setup() {
00128 #ifdef MOTION_DEBUG_ON
00129     MainCtrl = ControlDevice(GetFileControlDevice("solver.in",""));
00130     register_at(MainCtrl);
00131     MainCtrl.update();
00132     MainCtrl.print_values(log() );
00133 #endif
00134     ( log() << "\n\n*********\t Motion Setup \tBegin\t*********" << std::endl ).flush();
00135     //MSlog = &log();
00136 
00137     std::string nameTmp;
00138     nameTmp = "basicScene";
00139     TheScene.setName(nameTmp);
00140 
00142     // Assemblies & PARTS
00143     specificSetup();
00144 
00145     ( log() << std::endl << TheScene.Name() << " posing " << TheScene.GetNumChains() << " kinematic chains.  ").flush();
00146     //Pose Scene
00147     for (register int i = 0; i < TheScene.GetNumChains(); i++) { // numDHChains; i++ ) {
00148       TheScene.poseChain(i);
00149     }
00150     ( log() << "Done.\n").flush();
00151     TheScene.updateScene(1.0,0.0);
00152 
00153     TheScene.GetNumPC(num_vertices,num_connections);
00154 #ifdef MOTION_DEBUG_ON
00155     TheScene.outputScene(5,0.,"sceneDebug.stl");
00156     ( log() << " debug stl file complete\n").flush();
00157     ( log() << " TheScene.GetNumPC(num_vertices,num_connections) num_vertices = " << num_vertices
00158             << " num_connections = " << num_connections << std::endl).flush();
00159 #endif
00160 
00161     initial_vertices = new point_type[ num_vertices ];
00162     vertices = new point_type[ num_vertices ];
00163     velocities = new point_type[ num_vertices ];
00164     connections = new multi_index_type[ num_connections ];
00165     TheScene.send2CPT_4(vertices,connections,scale);
00166 
00167     for ( register int i = 0; i < num_vertices; i++ ) {
00168       initial_vertices[i](0) = vertices[i](0);  initial_vertices[i](1) = vertices[i](1);   initial_vertices[i](2) = vertices[i](2);
00169     }
00170 
00171     pressures = new DataType[ num_vertices ];
00172     nodeids = new int[ num_vertices ];
00173     for (register int j=0; j<num_vertices; j++)
00174       nodeids[j] = j;
00175 #ifdef MOTION_DEBUG_ON
00176     ( log() << " num_vertices " << num_vertices << " num_connections " << num_connections << std::endl
00177             << " initial_vertices[num_vertices-1] " << initial_vertices[num_vertices-1]
00178             << " connections[num_connections-1] " << connections[num_connections-1] << std::endl
00179             << "\n*********\t Motion Setup END\t*********\n").flush();
00180 #endif
00181     return true;
00182   }
00183 
00184   virtual void finish() {
00185     ( log() << "MotionSolver finish()\n").flush();
00186     //Output();
00187   }
00188 
00189   virtual void SetVertices(double& t) {
00190 #ifdef MOTION_DEBUG_ON
00191     ( log() << " SetVerticies() num_vertices=" << num_vertices << "\n").flush();
00192 #endif
00193     for (int i=0; i<num_vertices; i++) {
00194       vertices[i](0) = initial_vertices[i](0);
00195       vertices[i](1) = initial_vertices[i](1);
00196       vertices[i](2) = initial_vertices[i](2);
00197     }
00198   }
00199 
00200   virtual void UpdateVertices(double& t) {
00201 
00202     TheScene.GetNumPC(num_vertices,num_connections);
00203 #ifdef MOTION_DEBUG_ON
00204     ( log() << "UpdateVertices num_vertices = " << num_vertices << " num_connections = " << num_connections << std::endl).flush();
00205 #endif
00206     vertices = new point_type[ num_vertices ];
00207     connections = new multi_index_type[ num_connections ];
00208     TheScene.send2CPT_4(vertices,connections,scale);
00209   }
00210 
00211   virtual void SetVelocities(double& t) {
00212 #ifdef MOTION_DEBUG_ON
00213     ( log() << " SetVelocities() num_vertices=" << num_vertices << "\n").flush();
00214 #endif
00215     for (int i=0; i<num_vertices; i++) {
00216       velocities[i](0) = 0.;
00217       velocities[i](1) = 0.;
00218       velocities[i](2) = 0.;
00219     }
00220   }
00221 
00222   virtual void UpdateVelocities(double& t) {
00223 #ifdef MOTION_DEBUG_ON
00224     ( log() << " Update Velocity Begun \n"
00225             << " num_vertices = " << num_vertices << " num_connections = " << num_connections << std::endl).flush();
00226 #endif
00227     velocities = new point_type[ num_vertices ];
00228     TheScene.vel2CPT_2(velocities,scale);
00229   }
00230 
00231 
00232   virtual void Initialize(double& t, double& dt) {
00233 #ifdef MOTION_DEBUG_ON
00234     ( log() << "\n\n*********\t Motion Initialize \tbegin\n*********" << std::endl).flush();
00235 #endif
00236     t=0.; time=t; dt=dtret;
00237     SetVertices(t);
00238     SetVelocities(t);
00239 
00240     ( log() << " TheScene.GetNumAssemblies() = " << TheScene.GetNumAssemblies() << std::endl).flush();
00241 
00242     for (int i = 0; i < TheScene.GetNumAssemblies(); i ++) {
00243       ( log() << "\nASSEMBLY " << i << std::endl).flush();
00244       TheScene.GetNthAssembly(i)->print(0);
00245     }
00246 
00247     ( log() << " TheScene.GetNumChains() = " << TheScene.GetNumChains() << std::endl).flush();
00248     for (int i = 0; i < TheScene.GetNumChains(); i ++) {
00249       ( log() << "\nCHAIN " << i << std::endl).flush();
00250       if (TheScene.GetNthChain(i)->getParentChain() <= 0) {
00251         TheScene.GetNthChain(i)->print(_Scene(),1);
00252       }
00253       else {
00254         TheScene.GetNthChain(i)->print(_Scene(),2);
00255       }
00256     }
00257 #ifdef MOTION_DEBUG_ON
00258     ( log() << "\n\n*********\t Motion Initialize \tEND\n*********" << std::endl).flush();
00259 #endif
00260   }
00261 
00262   virtual void movement(double& t, double& dt) {}
00263 
00264   virtual void pressureForce() {
00265     TheScene.partPressureForces(connections,pressures,scale);
00266     // write to log file
00267   }
00268 
00269   virtual void logLoads() {
00270     char fname[256];
00271     sprintf(fname,"%s_Loads.dat",CheckpointName.c_str());
00272     std::ofstream ofs;
00273     if ( ! fexists(fname) ) {
00274       ofs.open(fname, std::ios::out);
00275       ofs << "# 1 NStep\t2 time\t3 Name()\t4 centroid\t7 xAxis\t10yAxis\t13 zAxis\t16 netP\t19momP\t22 projectScalar(netP,xAxis)\t"
00276           << "23 projectScalar(netP,yAxis)\t24 projectScalar(netP,zAxis)\t25 area\t26 volume" << std::endl;
00277     }
00278     else {
00279       ofs.open(fname, std::ios::app);
00280     }
00281     TheScene.logPartLoads(ofs, CurrentTime(), NSteps());
00282     ofs.close();
00283   }
00284 
00285   virtual void Advance(double& t, double& dt) {
00286 #ifdef MOTION_DEBUG_ON
00287     ( log() << "\n\n*********\t aDVANCE bEGIN\n*********" << std::endl).flush();
00288 #endif
00289     t+=dt;
00290     time=t;
00291 #ifdef MOTION_DEBUG_ON
00292     ( log() << " time = " << time << " time*DataType(360.0/6.) = " << time*DataType(360.0/6.)
00293             << " time*DataType(360.0/6.)*d2r = " << time*DataType(360.0/6.)*d2r << std::endl).flush();
00294 #endif
00295 
00296     pressureForce();
00297 #ifdef LOAD_DEBUG_ON
00298     if ( NSteps() % log_every == 0 ) {
00299       logLoads();
00300     }
00301 #endif
00302     movement(t,dt);
00303 
00304     for (register int i = 0; i < TheScene.GetNumChains(); i++) {
00305       TheScene.poseChain(i);
00306     }
00307     TheScene.updateScene(dt,time);
00308     UpdateVertices(t);
00309     UpdateVelocities(t);
00310     TheScene.resetScene();
00311     steps++;
00312     dt=dtret;
00313 
00314 #ifdef MOTION_DEBUG_ON
00315     ( log() << "\n\n*********\t aDVANCE fINISHED\n*********" << std::endl).flush();
00316 #endif
00317   }
00318 
00319   virtual void Output() {
00320     if (OutputName.c_str()[0] == '-')
00321       return;
00322     char ioname[256];
00323 #ifndef MOTION_ONLY
00324     sprintf(ioname,"%s_%d.tec",OutputName.c_str(),NSteps());
00325 #else
00326     sprintf(ioname,"%s%s_%d.tec",OutputName.c_str(),"motion",NSteps());
00327 #endif
00328     ( std::cout << " *** Writing " << OutputName << "_" << NSteps()
00329                 << "  t = " << CurrentTime() << " *** " << std::endl ).flush();
00330     std::ofstream outfile(ioname, std::ios::out);
00331 
00332     std::ostream* out;
00333     outfile.open(ioname, std::ios::out);
00334     out = new std::ostream(outfile.rdbuf());
00335 
00336     *out << "title = mesh" << std::endl;
00337     *out << "variables = x y z Pressures Velocity_u Velocity_v Velocity_w VelocityVec" << std::endl;
00338     *out << "zone t=\"zone 1\", I=" << num_vertices << ", J="
00339          << num_connections << ", F=FEPOINT, ET=TRIANGLE" << std::endl;
00340     for (register int j=0;j<num_vertices; j++)
00341       *out << vertices[j](0) << " " << vertices[j](1) << " " << vertices[j](2) << " "
00342                                                                                << pressures[j] << " " << velocities[j](0) << " " << velocities[j](1) << " " << velocities[j](2) << " "
00343                                                                                                                                                                                 << std::sqrt(velocities[j](0)*velocities[j](0)+velocities[j](1)*velocities[j](1)+velocities[j](2)*velocities[j](2))
00344                                                                                                                                                                                              << std::endl;
00345                                                                                                                                                                                    for (register int j=0;j<num_connections; j++)
00346         *out << connections[j](0)+1 << " " << connections[j](1)+1 << " " << connections[j](2)+1 << std::endl;
00347 
00348     outfile.close();
00349     delete out;
00350 
00351   }
00352 
00353   virtual void Restart(double& t, double& dt) {
00354 #ifdef CHECKRESTART_DEBUG_ON
00355     ( log() << "\nRestarting MOTION SOLVER t= " << t << " dt = " << dt << std::endl).flush();
00356 #endif
00357     char fname[256];
00358     sprintf(fname,"%s.cp",CheckpointName.c_str());
00359     std::ifstream ifs(fname, std::ios::in);
00360     ifs.read((char*)&time,sizeof(double));
00361     ifs.read((char*)&steps,sizeof(int));
00362 #ifdef CHECKRESTART_DEBUG_ON
00363     ( log() << "CheckPointName = " << CheckpointName << std::endl).flush();
00364     if (ifs.is_open()) {
00365       ( log() << " OPEN\n").flush();
00366     }
00367     else {
00368       ( log() << " CLOSED\n").flush();
00369     }
00370     ( log() << "time " << time << " steps " << steps << std::endl).flush();
00371 #endif
00372     t = time;
00373 
00374     ifs.read((char*)&num_vertices,sizeof(int));
00375     ifs.read((char*)&num_connections,sizeof(int));
00376     ifs.read((char*)vertices,num_vertices*sizeof(point_type));
00377     ifs.read((char*)connections,num_connections*sizeof(multi_index_type));
00378     ifs.read((char*)velocities,num_vertices*sizeof(point_type));
00379     ifs.read((char*)pressures,num_vertices*sizeof(DataType));
00380     pos = ifs.tellg();
00381 
00382     TheScene.Restart(ifs,pos,t,dt);
00383     pos = ifs.tellg();
00384     dt=dtret;
00385     ifs.close();
00386 #ifdef CHECKRESTART_DEBUG_ON
00387     ( log() << "\nDone Restarting Motion Solver t = " << t << " dt = " << dt << " pos = " << pos << std::endl).flush();
00388 #endif
00389   }
00390 
00391   virtual void Checkpointing() {
00392 #ifdef CHECKRESTART_DEBUG_ON
00393     ( log() << "\nCHECKPOINTING MOTION SOLVER time " << time << " steps " << steps << std::endl).flush();
00394 #endif
00395     char fname[256], oname[256];
00396     sprintf(fname,"%s.cp",CheckpointName.c_str());
00397     sprintf(oname,".%s.cp",CheckpointName.c_str());
00398     rename(fname,oname);
00399     std::ofstream ofs(fname, std::ios::out);
00400     ofs.write((char*)&time,sizeof(double));
00401     ofs.write((char*)&steps,sizeof(int));
00402 
00403     ofs.write((char*)&num_vertices,sizeof(int));
00404     ofs.write((char*)&num_connections,sizeof(int));
00405     ofs.write((char*)vertices,num_vertices*sizeof(point_type));
00406     ofs.write((char*)connections,num_connections*sizeof(multi_index_type));
00407     ofs.write((char*)velocities,num_vertices*sizeof(point_type));
00408     ofs.write((char*)pressures,num_vertices*sizeof(DataType));
00409 
00410     TheScene.Checkpointing(ofs);
00411 #ifdef CHECKRESTART_DEBUG_ON
00412     ( log() << "\n+++ CHECKPOINTING COMPLETE pos = " << ofs.tellp() <<std::endl).flush();
00413 #endif
00414     ofs.close();
00415   }
00416 
00417   inline double CurrentTime() const { return time; }
00418   virtual  int NSteps() { return steps; }
00419   virtual double CurrentDt() const { return dtret; }
00420   virtual  int LogEvery() { return log_every; }
00421 
00422   inline SceneType& _Scene() { return TheScene; }
00423   inline const SceneType& _Scene() const { return TheScene; }
00424 
00425   inline MotionSolver& _Motion() { return *this; }
00426   inline const MotionSolver& _Motion() const { return *this; }
00427 
00428   int isRestart() const { return motionRestart; }
00429 
00430 protected:
00431   DataType xs[dim], Tr, pi, scale;
00432   double dtret, time;
00433   std::string brep_filename;
00434   std::string vel_filename;
00435   std::string OutputName;
00436   std::string CheckpointName;
00437   int brep_filetype, vel_filetype, num_vertices, num_connections, steps, log_every;
00438   point_type *initial_vertices, *vertices, *velocities;
00439   multi_index_type *connections;
00440   int *nodeids;
00441   DataType *pressures;
00442   int numAssemblies;
00443 
00444   int numDHChains, numDHLinks, numDHJoints;
00445   AssemblyControl AssemblyCtrl[maxAssemblies];
00446   ControlDevice MainCtrl, LocCtrl;
00447 
00448   SceneType TheScene;
00449 
00450   DHChainControl DHChainCtrls[ChainLimit];
00451   LinkControl LinkCtrls[LinkLimit];
00452   JointControl JointCtrls[JointLimit];
00453 
00454   int pos;
00455   int motionRestart;
00456 
00457   std::ostream* _bolog;
00458 };
00459 
00460 
00461 #endif
Generated on Thu Jun 30 02:18:57 2016 for Motion - rigid body dynamics solver by 1.6.3