kernel/Assembly.h

Go to the documentation of this file.

// -*- C++ -*-

#ifndef ASSEMBLY_H
#define ASSEMBLY_H

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <set>
#include <map>
#include <stdlib.h>
#include <stdio.h>

#include "Part.h"

#define averbose 0

//extern std::ostream* MSlog;

template <class DataType, int dim>
class Assembly {
  typedef Assembly<DataType,dim> base;
public:
  typedef Part<DataType,dim> PartType;
  typedef ads::FixedArray<dim,DataType> point_type;
  typedef ads::FixedArray<dim,int> multi_index_type;

  // Constructors
  Assembly()
  {
    idTag.clear();
    edges.resize(0);
    component = -1;
    backedge = -1;
    gdistance = -1;

    parts.clear();
    subAssemblies.clear();

    AssemblyTmatrix.resize(dim+1,dim+1);
    AssemblyTmatrix.assign(imatrix(dim+1));
    MotionTmatrix.resize(dim+1,dim+1);
    MotionTmatrix.assign(imatrix(dim+1));
    DHMat.resize(dim+1,dim+1);
    DHMat.assign(imatrix(dim+1));
    DHMat_old.resize(dim+1,dim+1);
    DHMat_old.assign(imatrix(dim+1));
  }

  Assembly(const base& v)
  {
    base();
    idTag = v.idTag;

    component = v.component;
    backedge = v.backedge;
    gdistance = v.gdistance;
    edges = v.edges;
    parent = v.parent;

    AssemblyTmatrix = v.AssemblyTmatrix;
    MotionTmatrix = v.MotionTmatrix;

    parts = v.parts;
    subAssemblies = v.subAssemblies;

    name =  v.name;
    mobile = v.mobile;

    // For posing in a Denavit-Hartenberg kinematic chain.
    DHMat = v.DHMat;
    DHMat_old = v.DHMat_old;

  }

  // Destructor
  ~Assembly() {}

  virtual void setDim() {
    if (dim == 2) {

    }
  }

  virtual std::vector<int>& IdTag() { return idTag; }
  virtual void setIdTag(std::vector<int> val) {
    idTag = val;
  }

  virtual std::string Name() { return name; }
  virtual void setName(std::string val) {
    name = val;
  }

  virtual base * GetWorld() { return parent; }

  virtual void setDHMat( MType val ) { DHMat = val; }
  virtual MType GetDHMat() { return DHMat; }


  virtual void AddPart( PartType * v) {
    std::vector<int> tmpId;
    tmpId = idTag;
    ( mklog() << " Adding Part to assembly " << Name() << " that has idTag " << stringID(tmpId) ).flush();
    tmpId.push_back( -(parts.size()+1) );
    v->setIdTag(tmpId);
    printID(v->IdTag());
    ( mklog() << " Part added to " << Name() << ". Part has " << v->Name() << " and idTag " << stringID(tmpId) ).flush();
    base::parts.push_back(v);
  }
  virtual void AddPart( PartType& v) { AddPart(&v); }
  virtual int GetNumParts() {
    return ((int) parts.size());
  }
  virtual PartType * GetNthPart(int n) {
    return (parts[n]);
  }

  virtual void AddSubAssembly( base * v) {
    ( mklog() << base::Name() << " adding subassembly "<< v->Name() <<std::endl
              << Name() << " " << stringID(idTag) << std::endl ).flush();
    std::vector<int> tmpId;
    tmpId = idTag;
    tmpId.push_back( subAssemblies.size() );
    v->setIdTag(tmpId);
    printID(v->IdTag());
    base::subAssemblies.push_back(*v);
  }
  virtual void AddSubAssembly( base& v) { AddSubAssembly(&v); }

  virtual int GetNumSubAssemblies() {
    return ((int) subAssemblies.size());
  }
  virtual base * GetNthSubAssembly(int n) {
    return (&subAssemblies[n]);
  }

  virtual void setAssemblyTmatrix() {
    AssemblyTmatrix.assign(imatrix(dim));
  }

  virtual void setAssemblyTmatrix( const MType ttemp) {
    AssemblyTmatrix = ttemp;
  }

  virtual MType * getAssemblyTmatrix() {
    return &AssemblyTmatrix;
  }

  void setAssemblyMotion(MType &Tmatrix) {
    MotionTmatrix = Tmatrix;
  }

  void updateAssembly(DataType dt, DataType time, MType transform, int level) {
    level++;
#ifdef ASSEMBLY_DEBUG_ON
    ( mklog() <<"Updating Assembly "<<Name()<<" :::: NumParts "<<GetNumParts()<<std::endl ).flush();
#endif
    MType tTmp;
    PType otemp(0.), otemp2(0.), atemp(0.);

    for (register int p = 0; p < GetNumParts(); p++) {
#ifdef ASSEMBLY_DEBUG_ON
      ( mklog() <<"     _Part "<<p<<" "<<GetNthPart(p)->Name()
               <<" numConnections "<<GetNthPart(p)->GetNumConnections()<<std::endl ).flush();
#endif
      GetNthPart(p)->updatePart(dt,time);
    }

    for (register int sa = 0; sa < GetNumSubAssemblies(); sa++) {
#ifdef ASSEMBLY_DEBUG_ON
      ( mklog() <<"Sub-Assembly "<<sa<<" "<<GetNthSubAssembly(sa)->Name()
               <<" NumParts "<<GetNthSubAssembly(sa)->GetNumParts()<<std::endl ).flush();
#endif
      GetNthSubAssembly(sa)->updateAssembly(dt,time,tTmp,level);
    }
  }

  void resetAssembly() {
#ifdef ASSEMBLY_DEBUG_ON
    ( mklog() <<"Reseting Assembly "<<Name()<<" :::: NumParts "<<GetNumParts()<<std::endl).flush();
#endif
    for (register int p = 0; p < GetNumParts(); p++) {
      GetNthPart(p)->resetPart();
    }
    for (register int sa = 0; sa < GetNumSubAssemblies(); sa++) {
      GetNthSubAssembly(sa)->resetAssembly();
    }
  }

  void partPressureForces( multi_index_type* cons, DataType* press, DataType scale ) {
#ifdef ASSEMBLY_DEBUG_ON
    ( mklog() <<"Calculating part pressure forces in Assembly "<<Name()
             <<" :::: NumParts "<<GetNumParts()<<std::endl).flush();
#endif
    for (register int p = 0; p < GetNumParts(); p++) {
      GetNthPart(p)->pressureForce( cons, press, scale );
    }
    for (register int sa = 0; sa < GetNumSubAssemblies(); sa++) {
      GetNthSubAssembly(sa)->partPressureForces( cons, press, scale );
    }
  }

  virtual void logPartLoads( std::ofstream& ofs, DataType time_, int steps ) {
#ifdef SCENE_DEBUG_ON
    ( mklog() <<"SCENE: logPartLoads()\n").flush();
#endif
    for (register int p = 0; p < GetNumParts(); p++) {
      GetNthPart(p)->logLoad( ofs, time_, steps );
    }
    for (register int a = 0; a < GetNumSubAssemblies(); a++) {
      GetNthSubAssembly(a)->logPartLoads( ofs, time_, steps );
    }
  }

  void send2CPT(int& nump, int& numc, point_type* vertices, multi_index_type* connections, DataType scale) {
    point_type * ptmp;
    Facet<DataType> * ctmp;
    PartType * partTmp;
    int it = 0, it2 = nump, it1 = numc, it0 = numc;
    for (register int p = 0; p < GetNumParts(); p++) {
      partTmp = GetNthPart(p);
      partTmp->setCGS(numc);

      for ( register int ii=0; ii < partTmp->GetNumPoints(); ii++ ) {
        ptmp = partTmp->GetNthPointAddress(ii);
        vertices[it2](0) = (*ptmp)(0)*scale;
        vertices[it2](1) = (*ptmp)(1)*scale;
        vertices[it2](2) = (*ptmp)(2)*scale;
        it2++;
      }
      it1 += partTmp->GetNumConnections();//connections.size();

      for (register int c=0; c < partTmp->GetNumConnections(); c++) {
        ctmp = (Facet<DataType> *) partTmp->GetNthConnection(c);
        connections[it0+c](0) = ctmp->getNthCon(0);
        connections[it0+c](1) = ctmp->getNthCon(1);
        connections[it0+c](2) = ctmp->getNthCon(2);
      }
      for (it = it0; it < it1; it++) {
        connections[it](0) += nump;
        connections[it](1) += nump;
        connections[it](2) += nump;
      }
      it0 += partTmp->GetNumConnections();
      numc += partTmp->GetNumConnections();
      nump += partTmp->GetNumPoints();
    }
    for (register int sa = 0; sa < GetNumSubAssemblies(); sa++) {
      GetNthSubAssembly(sa)->send2CPT(nump,numc,vertices,connections,scale);
    }
  }

  void vel2CPT(int& totPoints, point_type* velocities, DataType scale) { //, int numvertices, int numConnections ) {
    point_type * ptmp;
    PartType * partTmp;
    int it2 = totPoints;
    for (register int p = 0; p < GetNumParts(); p++) {
      partTmp = GetNthPart(p);

      for ( register int ii=0; ii < partTmp->GetNumPoints(); ii++ ) {
        ptmp = partTmp->GetNthVelocityAddress(ii);
        velocities[it2](0) = (*ptmp)(0)*scale;
        velocities[it2](1) = (*ptmp)(1)*scale;
        velocities[it2](2) = (*ptmp)(2)*scale;
        it2++;
      }
      totPoints = it2;
    }
    for (register int sa = 0.; sa < GetNumSubAssemblies(); sa++) {
      GetNthSubAssembly(sa)->vel2CPT(totPoints, velocities, scale);
    }
  }

  int GetNumPoints() {
    int acount = 0;

    for (register int p = 0; p < GetNumParts(); p++) {
      acount += GetNthPart(p)->GetNumPoints();
    }
    for (register int sa = 0; sa < GetNumSubAssemblies(); sa++) {
      acount += GetNthSubAssembly(sa)->GetNumPoints();
    }
    return acount;
  }

  void GetNumPC(int& pcount, int& ccount) {
    for (register int p = 0; p < GetNumParts(); p++) {
      pcount += GetNthPart(p)->GetNumPoints();
      ccount += GetNthPart(p)->GetNumConnections();
    }
    for (register int sa = 0; sa < GetNumSubAssemblies(); sa++) {
      GetNthSubAssembly(sa)->GetNumPC(pcount,ccount);
    }
  }

  void print(int level) {
    ( mklog() << std::endl << std::string(level, '\t') << "Assembly " <<  Name() << " :::: NumParts " << GetNumParts() << "\n"
            << std::string(level, '\t') << "AssemblyTmatrix = " << AssemblyTmatrix << std::endl ).flush();
    level++;
    for (register int p = 0; p < GetNumParts(); p++) {
      ( mklog() << std::string(level, '\t') << "Part " << GetNthPart(p)->Name()
                << " idTag " << stringID(GetNthPart(p)->IdTag())
                << " numConnections " << GetNthPart(p)->GetNumConnections() << std::endl
                << std::string(level, '\t') << "  DHMat " << GetNthPart(p)->GetDHMat() <<std::endl ).flush();
      if ( GetNthPart(p)->GetMobility() == 1 ) {
        ( mklog() << std::string(level, '\t') << "  mobile ").flush();
      }
      else {
        ( mklog() << std::string(level, '\t') << " stationary ").flush();
      }
      if ( GetNthPart(p)->GetDeformation() == 1  ) {
        ( mklog() << " deformable\n").flush();
      }
      else {
        ( mklog() << " rigid\n").flush();
      }
    }

    if ( GetNumSubAssemblies() > 0)
      ( mklog() << std::endl<< std::string(level, '\t') << "GetNumSubAssemblies() " << GetNumSubAssemblies() << std::endl ).flush();
    for (register int sa = 0; sa < GetNumSubAssemblies(); sa++) {
      GetNthSubAssembly(sa)->print(level);
    }

  }

  void printSummary(int level) {
    ( mklog() << std::endl << std::string(level, '\t') << "Assembly " <<  Name()
              << " :::: NumParts " << GetNumParts() << std::endl ).flush();
    std::cout << std::endl << std::string(level, '\t') << "Assembly " <<  Name()
              << " :::: NumParts " << GetNumParts() << std::endl;
    level++;
    for (register int p = 0; p < GetNumParts(); p++) {
      ( mklog() << std::string(level, '\t') << "Part " << GetNthPart(p)->Name()
                << " idTag " << stringID(GetNthPart(p)->IdTag()) <<std::endl ).flush();
      std::cout << std::string(level, '\t') << "Part " << GetNthPart(p)->Name()
                << " idTag " << stringID(GetNthPart(p)->IdTag()) <<std::endl;
      if ( GetNthPart(p)->GetMobility() == 1 ) {
        ( mklog() << std::string(level, '\t') << "  mobile ").flush();
        std::cout << std::string(level, '\t') << "  mobile ";
      }
      else {
        ( mklog() << std::string(level, '\t') << " stationary ").flush();
        std::cout << std::string(level, '\t') << " stationary ";
      }
      if ( GetNthPart(p)->GetDeformation() == 1  ) {
        ( mklog() << " deformable\n").flush();
        std::cout << " deformable\n";
      }
      else {
        ( mklog() << " rigid\n").flush();
        std::cout << " rigid\n";
      }
    }
    if ( GetNumSubAssemblies() > 0) {
      ( mklog() << std::endl<< std::string(level, '\t') << "GetNumSubAssemblies() " << GetNumSubAssemblies() << std::endl ).flush();
      std::cout << std::endl<< std::string(level, '\t') << "GetNumSubAssemblies() " << GetNumSubAssemblies() << std::endl;
    }
    for (register int sa = 0; sa < GetNumSubAssemblies(); sa++) {
      GetNthSubAssembly(sa)->printSummary(level);
    }
  }

  virtual void outputAssembly_Brep3D(DataType time, int pc, int pointCount[], int& pp, FILE * fout) {
    // enum IOType { pointList, VTK_curve, Brep2D, VTK_PL, Brep3D, STL };
    const Facet<DataType> * facetTmp;
    const PartType * partTmp;

    if (pc == 0) {
      for (register int p = 0; p < GetNumParts(); p++) {
        ( mklog() <<"Part "<<p<<" "<<GetNthPart(p)->Name() <<" numPoints "<< GetNthPart(p)->GetNumPoints()<<std::endl ).flush();
        partTmp = GetNthPart(p);
        for (register int j=0; j < partTmp->GetNumPoints(); j ++) {
          fprintf(fout,"%10d %8.5e %8.5e %8.5e\n",j,
                  partTmp->GetNthPoint(j)(0),partTmp->GetNthPoint(j)(1),partTmp->GetNthPoint(j)(2) );
        }
      }
      for (register int sa = 0; sa < GetNumSubAssemblies(); sa++) {
        GetNthSubAssembly(sa)->outputAssembly_Brep3D(time,1,pointCount,pp,fout);
      }
    }

    if (pc == 1) {
      for (register int p = 0; p < GetNumParts(); p++) {
        partTmp = GetNthPart(p);
        for (register int j = 0; j < partTmp->GetNumConnections(); j++) {
          facetTmp = (partTmp->GetNthFacet(j));
          fprintf(fout,"%d\t%d\t%d\n",
                  facetTmp->getNthCon(0)+pointCount[pp],facetTmp->getNthCon(1)+pointCount[pp],
                  facetTmp->getNthCon(2)+pointCount[pp] );
        }
        pp++;
      }
      for (register int sa = 0; sa < GetNumSubAssemblies(); sa++) {
        GetNthSubAssembly(sa)->outputAssembly_Brep3D(time,1,pointCount,pp,fout);
      }
    }

  }

  virtual void outputAssembly_STL(DataType time, FILE * fout) {
    // enum IOType { pointList, VTK_curve, Brep2D, VTK_PL, Brep3D, STL };
    for (register int p = 0; p < GetNumParts(); p++) {
      ( mklog() <<"Asssembly "<<Name()<<" contains Part "<<GetNthPart(p)->Name()
               <<" with "<<  GetNthPart(p)->GetNumConnections()<<" numConnections "<< std::endl ).flush();
      GetNthPart(p)->output(PartType::STL,time,fout);
    }
    ( mklog() <<"Assembly "<<Name()<<" has "<< GetNumSubAssemblies() <<" SubAssemblies\n").flush();
    for (register int sa = 0; sa < GetNumSubAssemblies(); sa++) {
      ( mklog() <<"Calling output on Subassembly "<<sa<<" : "<<GetNthSubAssembly(sa)->Name()<<std::endl).flush();
      GetNthSubAssembly(sa)->outputAssembly_STL(time,fout);
    }

  }

  virtual void Restart(std::ifstream& ifs, int& pos, double& t, double& dt) {
#ifdef CHECKRESTART_DEBUG_ON
    (mklog() << "\nRestarting Assembly " << Name() << " t = " << t << " dt = " << dt << " pos = " << pos << std::endl).flush();
#endif
    ifs.read((char*)&num_parts,sizeof(int));
    ifs.read((char*)&num_subAssemblies,sizeof(int));
    pos = ifs.tellg();

    for (register int i=0; i<num_parts; i++) {
      ( mklog() << stringID(GetNthPart(i)->IdTag()) << " " << parts[i]->Name() << std::endl ).flush();
      parts[i]->Restart(ifs,pos,t,dt);
      pos = ifs.tellg();
    }

    for (register int i=0; i<num_subAssemblies; i++) {
      subAssemblies[i].Restart(ifs,pos,t,dt);
      pos = ifs.tellg();
    }
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << "\nDone Restarting Assembly " << Name() << " t = " << t << " dt = " << dt << " pos = " << pos << std::endl ).flush();
#endif
  }

  virtual void Checkpointing(std::ofstream& ofs) {
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << "\nCHECKPOINTING Assembly " << Name() << std::endl ).flush();
#endif
    ( mklog() << "\nCHECKPOINTING Assembly " << Name() << std::endl ).flush();
    num_parts = parts.size();
    ( mklog() << "num_parts " << num_parts << std::endl ).flush();
    num_subAssemblies = subAssemblies.size();
    ( mklog() << "num_subAssemblies " << num_subAssemblies << std::endl ).flush();
    ofs.write((char*)&num_parts,sizeof(int));
    ofs.write((char*)&num_subAssemblies,sizeof(int));

    for (register int i=0; i<num_parts; i++) {
      ( mklog() << stringID(GetNthPart(i)->IdTag())  << " " << parts[i]->Name() << std::endl ).flush();
      parts[i]->Checkpointing(ofs);
    }

    for (register int i=0; i<num_subAssemblies; i++) {
      subAssemblies[i].Checkpointing(ofs);
    }

  }

  virtual PType * GetVerticies() { return verticies; }
  virtual multi_index_type * GetConnections() { return connections; }
  virtual void makeVerticies(int n) { verticies = new PType[n]; }
  virtual void makeConnections(int n) { connections = new multi_index_type[n]; }

  virtual PType surfacePoint(PType I_A, DataType zmin, int flag) {
    PType I_B(I_A);
    I_B(2) = zmin;
    int nump = 0, numc = 0;
    GetNumPC(nump,numc);

    ( mklog() << " I_A " << I_A << " I_B " << I_B << std::endl
              << "SurfacePoint nump = " << nump << " numc = " << numc << std::endl ).flush();
    makeVerticies(nump);
    makeConnections(numc);

    DataType scale = 1.;
    nump = 0;
    numc = 0;
    send2CPT(nump, numc, GetVerticies(), GetConnections(), scale);
    I_B = intersection_line2facet(I_A, I_B, nump, numc, GetVerticies(), GetConnections(), flag );
    return I_B;
  }

  virtual PType surfacePoint(PType I_A, DataType zmin, int flag, DataType off ) {
    PType I_B(I_A), normal;
    I_B(2) = zmin;
    int nump = 0, numc = 0; // flag = 0;
    GetNumPC(nump,numc);
    normal(0) = 0; normal(1) = 0; normal(2) = 1;

    ( mklog() << Name() << " I_A " << I_A << " I_B " << I_B << std::endl
              << "SurfacePoint nump = " << nump << " numc = " << numc << std::endl ).flush();
    makeVerticies(nump);
    makeConnections(numc);

    DataType scale = 1.;
    nump = 0;
    numc = 0;
    send2CPT(nump, numc, GetVerticies(), GetConnections(), scale);
    ( mklog() << "SurfacePoint after sed2CPT() nump = " << nump << " numc = " << numc << std::endl ).flush();
    I_B = intersection_line2facet(I_A, I_B, nump, numc, GetVerticies(), GetConnections(), flag );
    I_A =(I_A-I_B);
    ( mklog() << " normalize(I_A) " << normalize(I_A) << std::endl ).flush();

    I_B = I_B + off*normal;

    return I_B;
  }

protected:
  std::vector<int> idTag; 
  std::string name;
  bool mobile;
  MType AssemblyTmatrix;
  MType MotionTmatrix;
  std::vector<base > subAssemblies;
  std::vector<PartType *> parts;

  int num_subAssemblies, num_parts;

  // For posing in a Denavit-Hartenberg kinematic chain.
  MType DHMat;
  MType DHMat_old;

  // For relative position determination through Breadth-first-search of Layout graph.
  base * parent;
  std::vector<int> edges;
  int component;
  int backedge;
  int gdistance;

  // For geometry queiries
  PType * verticies;
  multi_index_type * connections;

  //std::ostream* MSlog;
};

#endif // ASSEMBLY_H

---