StVKReducedInternalForces::StVKReducedInternalForces(int r, double * U, VolumetricMesh * volumetricMesh, StVKElementABCD * precomputedABCDIntegrals, int initOnly, bool addGravity_, double g_, int verbose_): precomputedIntegrals(precomputedABCDIntegrals), unitReducedGravityForce(NULL), reducedGravityForce(NULL), addGravity(addGravity_), g(g_), useSingleThread(0), shallowCopy(0), verbose(verbose_)
{
  int numElements = volumetricMesh->getNumElements();
  lambdaLame = (double*) malloc (sizeof(double) * numElements);
  muLame = (double*) malloc (sizeof(double) * numElements);

  for(int el=0; el<numElements; el++)
  {
    VolumetricMesh::Material * material = volumetricMesh->getElementMaterial(el);
    VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
    if (eNuMaterial == NULL)
    {
      printf("Error: StVKReducedInternalForces: mesh does not consist of E, nu materials.\n");
      throw 1;
    }

    lambdaLame[el] = eNuMaterial->getLambda();
    muLame[el] = eNuMaterial->getMu();
  }

  InitComputation(r, U, volumetricMesh);
  if (!initOnly)
    ProcessElements(0, volumetricMesh->getNumElements());
  InitGravity();
}
Beispiel #2
0
void MyFrame::ScaleYoungsModulus(double factor)
{
  for(int i=0; i < precomputationState.simulationMesh->getNumMaterials(); i++)
  {
    VolumetricMesh::Material * material = precomputationState.simulationMesh->getMaterial(i);
    VolumetricMesh::ENuMaterial * eNuMaterial =  downcastENuMaterial(material);
    if (eNuMaterial == NULL)
      printf("Internal error: eNuMaterial is NULL);");
    else
      eNuMaterial->setE(factor * eNuMaterial->getE());
  }
}
void RenderVolumetricMesh::DetermineMaxMin(VolumetricMesh * volumetricMesh)
{
  maxE = 0;
  maxnu = 0;
  maxDensity = 0;
  minE = DBL_MAX;
  minnu = DBL_MAX;
  minDensity = DBL_MAX;
  for(int i=0; i < volumetricMesh->getNumElements(); i++)
  {
    // set color based on Young's modulus, Poisson ratio, density
    VolumetricMesh::Material * material = volumetricMesh->getElementMaterial(i);
    double density = material->getDensity();

    VolumetricMesh::ENuMaterial * eNuMaterial =  downcastENuMaterial(material);
    double E;
    double nu;
    if (eNuMaterial == NULL)
    {
      E = 1E6;
      nu = 0.45;
    }
    else
    {
      E = eNuMaterial->getE();
      nu = eNuMaterial->getNu();
    }

    if (E > maxE)
      maxE = E;

    if (nu > maxnu)
      maxnu = nu;

    if(density>maxDensity)
      maxDensity = density;

    if (E < minE)
      minE = E;

    if (nu < minnu)
      minnu = nu;

    if (density<minDensity)
      minDensity = density;
  }

  printf("MaxE: %G MinE: %G\n",maxE,minE);
  printf("Maxnu: %G Minnu: %G\n",maxnu,minnu);
  printf("MaxDensity: %G MinDensity: %G\n",maxDensity,minDensity);
}
StVKStiffnessMatrix::StVKStiffnessMatrix(StVKInternalForces *  stVKInternalForces)
{
  precomputedIntegrals = stVKInternalForces->GetPrecomputedIntegrals();
  volumetricMesh = stVKInternalForces->GetVolumetricMesh();
  numElementVertices = volumetricMesh->getNumElementVertices();
  int numElements = volumetricMesh->getNumElements();

  lambdaLame = (double*) malloc (sizeof(double) * numElements);
  muLame = (double*) malloc (sizeof(double) * numElements);

  for(int el=0; el<numElements; el++)
  {
    VolumetricMesh::Material * material = volumetricMesh->getElementMaterial(el);
    VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
    if (eNuMaterial == NULL)
    {
      printf("Error: mesh does not consist of E, nu materials.\n");
      throw 1;
    }

    lambdaLame[el] = eNuMaterial->getLambda();
    muLame[el] = eNuMaterial->getMu();
  }

  // build stiffness matrix skeleton 
  SparseMatrix * stiffnessMatrixTopology;
  GetStiffnessMatrixTopology(&stiffnessMatrixTopology);

  // build acceleration indices
  row_ = (int**) malloc (sizeof(int*) * numElements);
  column_ = (int**) malloc (sizeof(int*) * numElements);

  for (int el=0; el < numElements; el++)
  {
    row_[el] = (int*) malloc (sizeof(int) * numElementVertices);
    column_[el] = (int*) malloc (sizeof(int) * numElementVertices * numElementVertices);

    for(int ver=0; ver<numElementVertices; ver++)
      row_[el][ver] = volumetricMesh->getVertexIndex(el, ver);

    // seek for value row[j] in list associated with row[i]
    for(int i=0; i<numElementVertices; i++)
      for(int j=0; j<numElementVertices; j++)
        column_[el][numElementVertices * i + j] =
          stiffnessMatrixTopology->GetInverseIndex(3*row_[el][i],3*row_[el][j]) / 3;
  }

  delete(stiffnessMatrixTopology);

}
StVKHessianTensor::StVKHessianTensor(StVKStiffnessMatrix * stVKStiffnessMatrix_): stVKStiffnessMatrix(stVKStiffnessMatrix_), volumetricMesh(stVKStiffnessMatrix->GetVolumetricMesh()), precomputedIntegrals(stVKStiffnessMatrix->GetPrecomputedIntegrals())
{
  numVertices_ = volumetricMesh->getNumVertices();
  numElements_ = volumetricMesh->getNumElements();
  numElementVertices = volumetricMesh->getNumElementVertices();

  lambdaLame = (double*) malloc (sizeof(double) * numElements_);
  muLame = (double*) malloc (sizeof(double) * numElements_);

  for(int el=0; el<numElements_; el++)
  {
    VolumetricMesh::Material * material = volumetricMesh->getElementMaterial(el);
    VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
    if (eNuMaterial == NULL)
    {
      printf("Error: StVKHessianTensor: mesh does not consist of E, nu materials.\n");
      throw 1;
    }

    lambdaLame[el] = eNuMaterial->getLambda();
    muLame[el] = eNuMaterial->getMu();
  }
}
StVKInternalForces::StVKInternalForces(VolumetricMesh * volumetricMesh_, StVKElementABCD * precomputedABCDIntegrals_, bool addGravity_, double g_): volumetricMesh(volumetricMesh_), precomputedIntegrals(precomputedABCDIntegrals_), gravityForce(NULL), addGravity(addGravity_), g(g_) 
{
  int numElements = volumetricMesh->getNumElements();
  lambdaLame = (double*) malloc (sizeof(double) * numElements);
  muLame = (double*) malloc (sizeof(double) * numElements);

  for(int el=0; el<numElements; el++)
  {
    VolumetricMesh::Material * material = volumetricMesh->getElementMaterial(el);
    VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
    if (eNuMaterial == NULL)
    {
      printf("Error: mesh does not consist of E, nu materials.\n");
      throw 1;
    }

    lambdaLame[el] = eNuMaterial->getLambda();
    muLame[el] = eNuMaterial->getMu();
  }

  buffer = (double*) malloc (sizeof(double) * 3 * volumetricMesh->getNumVertices());
  numElementVertices = volumetricMesh->getNumElementVertices();
  InitGravity();
}
void RenderVolumetricMesh::Render(VolumetricMesh * volumetricMesh, int wireframe, double * u)
{
  glDisable(GL_LIGHTING);

  int meshType = 0;
  if (volumetricMesh->getElementType() == CubicMesh::elementType())
    meshType = 1;
  if (volumetricMesh->getElementType() == TetMesh::elementType())
    meshType = 2;

  if (renderingMode == RENDERVOLUMETRICMESH_RENDERINGMODE_DISCRETECOLORS)
  {
    if (volumetricMesh->getNumMaterials() == 0)
    {
      printf("Error: discrete color rendering mode in renderVolumetricMesh called with zero materials.\n");
    }

    map<int,int> actualMaterials;
    for(int ss=0; ss < volumetricMesh->getNumRegions(); ss++)
    {
      int materialIndex = volumetricMesh->getRegion(ss)->getMaterialIndex();
      int setIndex = volumetricMesh->getRegion(ss)->getSetIndex();
      VolumetricMesh::Set * elementSet = volumetricMesh->getSet(setIndex);
      int numElements = elementSet->getNumElements();

      map<int,int> :: iterator iter = actualMaterials.find(materialIndex);
      if (iter == actualMaterials.end())
      {
        // new material
        actualMaterials.insert(make_pair(materialIndex, numElements));
      }
      else
      {
        // existing material
        iter->second += numElements;
      }
    }
    int numActualMaterials = (int)actualMaterials.size();

    multimap<int, int> materialOrderReverse;
    for(map<int, int> :: iterator iter = actualMaterials.begin(); iter != actualMaterials.end(); iter++)
      materialOrderReverse.insert(make_pair(iter->second, iter->first));

    // sort by the number of elements
    map<int,int> materialOrder;
    int counter = 0;
    for(multimap<int, int> :: iterator iter = materialOrderReverse.begin(); iter != materialOrderReverse.end(); iter++)
    {
      materialOrder.insert(make_pair(iter->second, counter));
      counter++;
    }

    double multiplicator = (numActualMaterials == 1 ? 1.0 : 1.0 / (numActualMaterials - 1));
    for(int ss=0; ss < volumetricMesh->getNumRegions(); ss++)
    {
      VolumetricMesh::Region * region = volumetricMesh->getRegion(ss);

      int materialIndex = region->getMaterialIndex(); 
      double color[3];
      //JetColorMap(materialIndex * multiplicator, color); 
      double gray = 1.0 - 0.5 * (numActualMaterials - 1 - materialOrder[materialIndex]) * multiplicator;
      color[0] = gray;
      color[1] = gray;
      color[2] = gray;

      int setIndex = region->getSetIndex(); 
      VolumetricMesh::Set * elementSet = volumetricMesh->getSet(setIndex);
      set<int> elements;
      elementSet->getElements(elements);

      for(set<int> :: iterator iter = elements.begin(); iter != elements.end(); iter++)
      {
        if (wireframe)
          glColor4d(0, 0, 0, 0.8);
        else
          glColor3f(color[0], color[1], color[2]);

        int el = *iter;
   
        if (u == NULL)
        {
          if (meshType == 1)
            RenderCube(volumetricMesh, el, wireframe);

          if (meshType == 2)
            RenderTet(volumetricMesh, el, wireframe);
        }
        else
        {
          #define VER(j) (*volumetricMesh->getVertex(el,j))[0] + u[3*volumetricMesh->getVertexIndex(el, j)+0],\
		         (*volumetricMesh->getVertex(el,j))[1] + u[3*volumetricMesh->getVertexIndex(el, j)+1],\
		         (*volumetricMesh->getVertex(el,j))[2] + u[3*volumetricMesh->getVertexIndex(el, j)+2]
          if (wireframe)
          {
            if (meshType == 1)
              CubeWireframeDeformable(VER(0),VER(1),VER(2),VER(3),
                                      VER(4),VER(5),VER(6),VER(7));

            if (meshType == 2)
              TetWireframeDeformable(VER(0),VER(1),VER(2),VER(3));
          }
          else
          {
            if (meshType == 1)
              CubeDeformable(VER(0),VER(1),VER(2),VER(3),
                             VER(4),VER(5),VER(6),VER(7));

            if (meshType == 2)
              TetDeformable(VER(0),VER(1),VER(2),VER(3));
          }
        }
      }
    }
  }
  else
  {
    for (int i=0; i < volumetricMesh->getNumElements(); i++)
    {
      // set color based on Young's modulus, Poisson ratio, density
      VolumetricMesh::Material * material = volumetricMesh->getElementMaterial(i);
      double density = material->getDensity();

      VolumetricMesh::ENuMaterial * eNuMaterial =  downcastENuMaterial(material);
      double E;
      double nu;
      if (eNuMaterial == NULL)
      {
        E = 1E6;
        nu = 0.45;
      }
      else
      {
        E = eNuMaterial->getE();
        nu = eNuMaterial->getNu();
      }

      double colorR=0.0, colorG=0.0, colorB=0.0;

      if (renderingMode == RENDERVOLUMETRICMESH_RENDERINGMODE_FLAT)
      {
        colorR = colorG = colorB = 1.0;
      }
      else if (renderingMode == RENDERVOLUMETRICMESH_RENDERINGMODE_GRADEDCOLORS)
      {
        if (maxE > minE + 1E-10)
          colorR = (E - minE) / (maxE - minE);
        else
          colorR = 1;
    
        if (maxnu > minnu + 1E-10)
          colorG = (nu - minnu) / (maxnu - minnu);
        else
          colorG = 1;
  
        if (maxDensity > minDensity + 1E-10)
          colorB = (density - minDensity) / (maxDensity - minDensity);
        else
          colorB = 1;
      }
      else
      {
        printf("Error: invalid rendering mode in renderVolumetricMesh!\n");
      }

      if (wireframe)
        glColor4d(0, 0, 0, 0.8);
      else
        glColor3f(colorR, colorG, colorB);

      if (u == NULL)
      {
        if (meshType == 1)
          RenderCube(volumetricMesh, i, wireframe);

        if (meshType == 2)
          RenderTet(volumetricMesh, i, wireframe);
      }
      else
      {
        #define VERA(j) (*volumetricMesh->getVertex(i,j))[0] + u[3*volumetricMesh->getVertexIndex(i, j)+0],\
                        (*volumetricMesh->getVertex(i,j))[1] + u[3*volumetricMesh->getVertexIndex(i, j)+1],\
	                (*volumetricMesh->getVertex(i,j))[2] + u[3*volumetricMesh->getVertexIndex(i, j)+2]

        if (wireframe)
        {
          if (meshType == 1)
            CubeWireframeDeformable(VERA(0),VERA(1),VERA(2),VERA(3),
                           VERA(4),VERA(5),VERA(6),VERA(7));

          if (meshType == 2)
            TetWireframeDeformable(VERA(0),VERA(1),VERA(2),VERA(3));
        }
        else
        {
          if (meshType == 1)
            CubeDeformable(VERA(0),VERA(1),VERA(2),VERA(3),
                           VERA(4),VERA(5),VERA(6),VERA(7));

          if (meshType == 2)
            TetDeformable(VERA(0),VERA(1),VERA(2),VERA(3));
        }
      }
    }
  }
}
CorotationalLinearFEM::CorotationalLinearFEM(TetMesh * tetMesh_) : tetMesh(tetMesh_) 
{
  numVertices = tetMesh->getNumVertices();

  // store the undeformed positions
  undeformedPositions = (double*) malloc (sizeof(double) * 3 * numVertices);
  for(int i=0; i < numVertices; i++)
  {
    Vec3d * v = tetMesh->getVertex(i);
    for(int j=0; j<3; j++)
      undeformedPositions[3*i+j] = (*v)[j];
  }

  int numElements = tetMesh->getNumElements();

  MInverse = (double**) malloc (sizeof(double*) * numElements);
  for(int el = 0; el < numElements; el++)
  {
    // get the integer indices of the tet vertices
    int vtxIndex[4];
    for(int vtx=0; vtx<4; vtx++)
      vtxIndex[vtx] = tetMesh->getVertexIndex(el, vtx);
    /*
       Form matrix: 
       M = [ v0   v1   v2   v3 ]
           [  1    1    1    1 ]
    */
    double M[16]; // row-major
    for(int vtx=0; vtx<4; vtx++)
      for(int dim=0; dim<3; dim++)
        M[4 * dim + vtx] = undeformedPositions[3 * vtxIndex[vtx] + dim];
    M[12] = M[13] = M[14] = M[15] = 1.0;

    // invert M and cache inverse (see [Mueller 2004])
    MInverse[el] = (double*) malloc (sizeof(double) * 16);
    inverse4x4(M, MInverse[el]);
  }

  // build acceleration indices for fast writing to the global stiffness matrix
  SparseMatrix * sparseMatrix;
  GetStiffnessMatrixTopology(&sparseMatrix);
  BuildRowColumnIndices(sparseMatrix);
  delete(sparseMatrix);

  // compute stiffness matrices for all the elements in the undeformed configuration
  KElementUndeformed = (double**) malloc (sizeof(double*) * numElements);
  for (int el = 0; el < numElements; el++)
  {
    double * MInv = MInverse[el];

    // Form stiffness matrix of the element in the undeformed configuration.
    // The procedure below is standard in FEM solid mechanics.
    // This code implements the equations given in Ahmed A. Shabana: Theory of Vibration, Volume II: Discrete and Continuous Systems, Springer--Verlag, New York, NY, 1990.

    double B[72] = 
      { MInv[0], 0, 0, MInv[4], 0, 0, MInv[8], 0, 0, MInv[12], 0, 0,
        0, MInv[1], 0, 0, MInv[5], 0, 0, MInv[9], 0, 0, MInv[13], 0,
	0, 0, MInv[2], 0, 0, MInv[6], 0, 0, MInv[10], 0, 0, MInv[14],
        MInv[1], MInv[0], 0, MInv[5], MInv[4], 0, MInv[9], MInv[8], 
        0, MInv[13], MInv[12], 0, 0, MInv[2], MInv[1], 0, MInv[6], MInv[5], 
        0, MInv[10], MInv[9], 0, MInv[14], MInv[13], MInv[2], 0, MInv[0], 
        MInv[6], 0, MInv[4], MInv[10], 0, MInv[8], MInv[14], 0, MInv[12] };

    // compute elasticity stiffness tensor
    double E[36]; // 6 x 6 matrix, stored row-major (symmetric, so row vs column-major storage makes no difference anyway)
    VolumetricMesh::Material * material = tetMesh->getElementMaterial(el);

    // check if material is ENuMaterial (i.e., isotropic)
    VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
    if (eNuMaterial != NULL)
    {
      // material is isotropic, specified by E, nu
      // compute Lame coefficients
      double lambda = eNuMaterial->getLambda();
      double mu = eNuMaterial->getMu();

      double Et[36] = { lambda + 2 * mu, lambda, lambda, 0, 0, 0,
                        lambda, lambda + 2 * mu, lambda, 0, 0, 0,
                        lambda, lambda, lambda + 2 * mu, 0, 0, 0,
                        0, 0, 0, mu, 0, 0,
                        0, 0, 0, 0, mu, 0,
                        0, 0, 0, 0, 0, mu };

      memcpy(E, Et, sizeof(double) * 36);
    }
    else
    {
      // orthotropic material
      // we follow the following references:
      // Yijing Li and Jernej Barbic: Stable Orthotropic Materials, Symposium on Computer Animation 2014
      // http://en.wikipedia.org/wiki/Orthotropic_material
      // http://www.solidmechanics.org/text/Chapter3_2/Chapter3_2.htm

      // test if material is OrthotropicMaterial (i.e., orthotropic)
      VolumetricMesh::OrthotropicMaterial * orthotropicMaterial = downcastOrthotropicMaterial(material);
      if (orthotropicMaterial != NULL)
      {
        double E1 = orthotropicMaterial->getE1();
        double E2 = orthotropicMaterial->getE2();
        double E3 = orthotropicMaterial->getE3();
        double nu12 = orthotropicMaterial->getNu12();
        double nu23 = orthotropicMaterial->getNu23();
        double nu31 = orthotropicMaterial->getNu31();
        double G12 = orthotropicMaterial->getG12();
        double G23 = orthotropicMaterial->getG23();
        double G31 = orthotropicMaterial->getG31();

        double nu21 = nu12 * E2 / E1;
        double nu32 = nu23 * E3 / E2;
        double nu13 = nu31 * E1 / E3;
        
        double Y = 1.0 / (1.0 - nu12 * nu21 - nu23 * nu32 - nu31 * nu13 - 2.0 * nu21 * nu32 * nu13);

        double ELocal[36] = { E1 * (1.0 - nu23 * nu32) * Y, E1 * (nu21 + nu31 * nu23) * Y, E1 * (nu31 + nu21 * nu32) * Y, 0.0, 0.0, 0.0,
                              E1 * (nu21 + nu31 * nu23) * Y, E2 * (1.0 - nu13 * nu31) * Y, E2 * (nu32 + nu12 * nu31) * Y, 0.0, 0.0, 0.0,
                              E1 * (nu31 + nu21 * nu32) * Y, E2 * (nu32 + nu12 * nu31) * Y, E3 * (1.0 - nu12 * nu21) * Y, 0.0, 0.0, 0.0,
                              0, 0, 0, G12, 0, 0,
                              0, 0, 0, 0, G23, 0,
                              0, 0, 0, 0, 0, G31 };

        //memcpy(E, ELocal, sizeof(double) * 36); // debug

        double R[9]; // row-major
        orthotropicMaterial->getR(R);

        // rotate Elocal into the basis given by the columns of R
        #define Relt(i,j) (R[3*(i)+(j)])
        double rotator[36];
        for(int i=0; i<3; i++)
          for(int j=0; j<3; j++)
          {
            rotator[6 * i + j] = Relt(i,j) * Relt(i,j);
            rotator[6 * i + 3 + j] = 2.0 * Relt(i, j) * Relt(i, (j+1) % 3);
            rotator[6 * (i + 3) + j] = Relt(i, j) * Relt((i+1) % 3, j);
            rotator[6 * (i + 3) + 3 + j] = Relt(i, j) * Relt((i+1) % 3, (j+1) % 3) + Relt(i, (j+1) % 3) * Relt((i+1) % 3, j);
          }
        #undef Relt

        // debug
        //memset(rotator, 0, sizeof(double) * 36);
        //for(int i=0; i<6; i++)
          //rotator[6*i+i] = 1.0;

        // E = rotator * ELocal * rotator^T
        double buffer[36]; 
        memset(buffer, 0, sizeof(double) * 36);
        // buffer = ELocal * rotator^T
        for(int i=0; i<6; i++)
          for(int j=0; j<6; j++)
            for(int k=0; k<6; k++)
              buffer[6 * i + j] += ELocal[6 * i + k] * rotator[6 * j + k];

        // E = rotator * buffer
        memset(E, 0, sizeof(double) * 36);
        for(int i=0; i<6; i++)
          for(int j=0; j<6; j++)
            for(int k=0; k<6; k++)
              E[6 * i + j] += rotator[6 * i + k] * buffer[6 * k + j];

      }
      else
      {
        printf("Error: CorotationalLinearFEM: unknown material encounted in the mesh.\n");
        throw 1;
      }
    }

    // EB = E * B
    double EB[72];
    memset(EB, 0, sizeof(double) * 72);
    for (int i=0; i<6; i++)
      for (int j=0; j<12; j++)
	for (int k=0; k<6; k++)
	  EB[12 * i + j] += E[6 * i + k] * B[12 * k + j];
 
    // KElementUndeformed[el] = B^T * EB
    KElementUndeformed[el] = (double*) calloc (144, sizeof(double)); // element stiffness matrix
    for (int i=0; i<12; i++)
      for (int j=0; j<12; j++)
	for (int k=0; k<6; k++)
          KElementUndeformed[el][12 * i + j] += B[12 * k + i] * EB[12 * k + j];

    // KElementUndeformed[el] *= volume
    double volume = TetMesh::getTetVolume(tetMesh->getVertex(el,0), tetMesh->getVertex(el,1), tetMesh->getVertex(el,2), tetMesh->getVertex(el,3));
    for(int i=0; i<144; i++)
      KElementUndeformed[el][i] *= volume;
  }
}
ReducedStVKCubatureForceModel::ReducedStVKCubatureForceModel(const int &r,VolumetricMesh *volumetricMesh,double *U,
                            const int &cubica_num, const double *cubica_weights,const unsigned int *cubica_elements,
                            double **restpos,bool addGravity, double g)
{
    r_=r;
    add_gravity_=addGravity;
    g_=g;
    cubica_num_=cubica_num;
    cubica_weights_ = new double[cubica_num_];
    cubica_elements_ = new double[cubica_num_];
    // std::cout<<"b\n";
    for(int i=0;i<cubica_num_;++i)
    {
        cubica_weights_[i]=cubica_weights[i];
        cubica_elements_[i]=cubica_elements[i];
    }
    // std::cout<<"c\n";
    volumetric_mesh_ = volumetricMesh;
    U_ = new double*[3*volumetricMesh->getNumVertices()];
    for(int i=0;i<3*volumetricMesh->getNumVertices();++i)
    {
        U_[i] = new double[r_];
        for(int j=0;j<r_;++j)
            U_[i][j]=0.0;
    }
    // std::cout<<"d\n";
    int num=volumetricMesh->getNumVertices();
    //change *U to **U_,U_[vert_idx][basis_idx]
// Matrix<double> Umatrix((int)(3*num),(int)r_);
// Matrix<double> M((int)(3*num),(int)(3*num));
    for(int j=0;j<r_;++j)
        for(int i=0;i<3*num;++i)
        {
            U_[i][j]=U[3*num*j+i];
            // Umatrix(i,j)=U_[i][j];
        }
    // for(int i=0;i<3*num;++i)
    //     for(int j=0;j<3*num;++j)
    //     {
    //         if(i==j)
    //             M(i,j)=1.0/8144;
    //         else
    //             M(i,j)=0.0;
    //     }
    //
    //         Matrix<double> temp=Umatrix.MultiplyT(M);
    //         Matrix<double> temp1=temp*Umatrix;
    //         for(int i=0;i<r_;++i)
    //             {
    //                 for(int j=0;j<r_;++j)
    //                 {
    //                     std::cout<<temp1(i,j)<<",";
    //                 }
    //                 std::cout<<"\n";
    //             }
    //             getchar();
    //test U is normalized or not
    // double test=0.0;
    // for(int i=0;i<3*num;++i)
    // {
    //     test+=U_[1][i]*U_[2][i]*(1.0/8144);
    // }
    // std::cout<<test<<"\n";
    restpos_ = new double*[cubica_num_];
    for(int i=0;i<cubica_num_;++i)
    {
        restpos_[i]=new double[12];
        for(int j=0;j<12;++j)
            restpos_[i][j]=restpos[i][j];
    }
    //init cubica subbasis
    cubica_subBasis_=new double**[(int)cubica_num_];
    for(int i=0;i<cubica_num_;++i)
    {
        cubica_subBasis_[i]=new double*[12];
        for(int j=0;j<12;++j)
        {
            cubica_subBasis_[i][j]=new double[(int)r_];
        }
    }
    for(int i=0;i<cubica_num_;++i)
    {
        int ele=cubica_elements_[i];
        for(int j=0;j<4;++j)
        {
            int vertID=volumetric_mesh_->getVertexIndex(ele,j);
            for(int k=0;k<r_;++k)
            {
                cubica_subBasis_[i][3*j][k]=U_[3*vertID][k];
                cubica_subBasis_[i][3*j+1][k]=U_[3*vertID+1][k];
                cubica_subBasis_[i][3*j+2][k]=U_[3*vertID+2][k];
            }
        }
    }
    deformed_=new double[12];
	memset(deformed_,0.0,sizeof(double)*12);
    // modal_matrix_ = new ModalMatrix(volumetric_mesh_->getNumVertices(),r_,U);
    VolumetricMesh::Material * material = volumetric_mesh_->getElementMaterial(0);
	VolumetricMesh::ENuMaterial * eNuMaterial = downcastENuMaterial(material);
	if (eNuMaterial == NULL)
	{
		std::cout<<"Error: mesh does not consist of E, nu materials.\n";
		exit(0);
	}
	lamda_ = eNuMaterial->getLambda();
	mu_ = eNuMaterial->getMu();
    gf_=new double[(int)r];
    // std::cout<<"f\n";
    InitGravity(U);
}