[occt.git] / src / FEmTool / FEmTool_Assembly.cxx

// File:	FEmTool_Assembly.cxx
// Created:	Tue Nov 17 12:07:58 1998
// Author:	Igor FEOKTISTOV
//		<ifv@paradox.nnov.matra-dtv.fr>


#include <FEmTool_Assembly.ixx>
#include <FEmTool_ListIteratorOfListOfVectors.hxx>
#include <FEmTool_ListOfVectors.hxx>
#include <TColStd_HArray1OfReal.hxx>
#include <math_Matrix.hxx>

//----------------------------------------------------------------------------
// Purpose - to find min index of global variables and define
//----------------------------------------------------------------------------
static Standard_Integer MinIndex(const Handle(FEmTool_HAssemblyTable)& Table)
{
  Standard_Integer dim, el, nvar, Imin;
  Standard_Integer diml = Table->LowerRow(), dimu = Table->UpperRow(),
                   ell = Table->LowerCol(), elu = Table->UpperCol(), nvarl, nvaru;

  Handle(TColStd_HArray1OfInteger) T = Table->Value(diml, ell);

  nvarl = T->Lower();

  Imin = T->Value(nvarl);

  for(dim = diml; dim <= dimu; dim++)
    for(el = ell; el <= elu; el++) {
      T = Table->Value(dim, el);
      nvarl = T->Lower(); nvaru = T->Upper();
      for(nvar = nvarl; nvar <= nvaru; nvar++) {
	Imin = Min(Imin, T->Value(nvar));
      }
    }
  return Imin;
}

//----------------------------------------------------------------------------
// Purpose - to find max index of global variables 
//----------------------------------------------------------------------------
static Standard_Integer MaxIndex(const Handle(FEmTool_HAssemblyTable)& Table)
{
  Standard_Integer dim, el, nvar, Imax;
  Standard_Integer diml = Table->LowerRow(), dimu = Table->UpperRow(),
                   ell = Table->LowerCol(), elu = Table->UpperCol(), nvarl, nvaru;

  Handle(TColStd_HArray1OfInteger) T = Table->Value(diml, ell);

  nvarl = T->Lower();

  Imax = T->Value(nvarl);

  for(dim = diml; dim <= dimu; dim++)
    for(el = ell; el <= elu; el++) {
      T = Table->Value(dim, el);
      nvarl = T->Lower(); nvaru = T->Upper();
      for(nvar = nvarl; nvar <= nvaru; nvar++) {
	Imax = Max(Imax, T->Value(nvar));
      }
    }
  return Imax;
}


//=======================================================================
//function : FEmTool_Assembly
//purpose  : 
//=======================================================================
FEmTool_Assembly::FEmTool_Assembly(const TColStd_Array2OfInteger& Dependence,
				   const Handle(FEmTool_HAssemblyTable)& Table):
     myDepTable(1, Dependence.ColLength(), 1, Dependence.RowLength()),
     B(MinIndex(Table), MaxIndex(Table))
     
{
  IsSolved = Standard_False;
  myDepTable = Dependence;
  myRefTable = Table;

  TColStd_Array1OfInteger FirstIndexes(1, B.Length()); FirstIndexes.Init(B.Length());

#ifdef DEB
  Standard_Integer dim, el, nvar, Imax, Imin, I0 = 1 - B.Lower(), i;
#else
  Standard_Integer dim, el, nvar, Imin, I0 = 1 - B.Lower(), i;
#endif
  Standard_Integer diml = Table->LowerRow(), dimu = Table->UpperRow(),
                   ell = Table->LowerCol(), elu = Table->UpperCol(), nvarl, nvaru;

  Handle(TColStd_HArray1OfInteger) T;
  for(dim = diml; dim <= dimu; dim++)
    for(el = ell; el <= elu; el++) {

      T = Table->Value(dim, el);
      nvarl = T->Lower(); nvaru = T->Upper();
      Imin = T->Value(nvarl) + I0;
     
      for(nvar = nvarl; nvar <= nvaru; nvar++) 
	Imin = Min(Imin, T->Value(nvar) + I0);
      
      for(nvar = nvarl; nvar <= nvaru; nvar++) {
	i = T->Value(nvar) + I0;
	FirstIndexes(i) = Min(FirstIndexes(i), Imin);
      }
    }


  H = new FEmTool_ProfileMatrix(FirstIndexes);

  NullifyMatrix();
  NullifyVector();
}

void FEmTool_Assembly::NullifyMatrix() 
{
  H->Init(0.);
  IsSolved = Standard_False;
}


//=======================================================================
//function : AddMatrix
//purpose  : 
//=======================================================================
void FEmTool_Assembly::AddMatrix(const Standard_Integer Element,
				 const Standard_Integer Dimension1,
				 const Standard_Integer Dimension2,
				 const math_Matrix& Mat)
{

  if(myDepTable(Dimension1, Dimension2) == 0) 
    Standard_DomainError::Raise("FEmTool_Assembly::AddMatrix");

  const TColStd_Array1OfInteger & T1 = myRefTable->Value(Dimension1,Element)->Array1();
  const TColStd_Array1OfInteger & T2 = myRefTable->Value(Dimension2,Element)->Array1();

  Standard_Integer nvarl = T1.Lower(), nvaru = Min(T1.Upper(), nvarl + Mat.RowNumber() - 1);

#ifdef DEB
  Standard_Integer I, J, I0 = 1 - B.Lower(), i, ii, j, jj, 
#else
  Standard_Integer I, J, I0 = 1 - B.Lower(), i, ii, j,
#endif
                   i0 = Mat.LowerRow() - nvarl, j0 = Mat.LowerCol() - nvarl;

  for(i = nvarl; i <= nvaru; i++) {
    I = T1(i) + I0;
    ii = i0+i;
    for(j = nvarl; j <= i; j++) {
      J = T2(j) + I0;
      H->ChangeValue(I, J) += Mat(ii, j0+j);
    }
  }

  IsSolved = Standard_False;
}


//=======================================================================
//function : NullifyVector
//purpose  : 
//=======================================================================
void FEmTool_Assembly::NullifyVector() 
{
  B.Init(0.);
}

//=======================================================================
//function : AddVector
//purpose  : 
//=======================================================================
void FEmTool_Assembly::AddVector(const Standard_Integer Element,
				 const Standard_Integer Dimension,
				 const math_Vector& Vec) 
{
  const TColStd_Array1OfInteger & T = myRefTable->Value(Dimension,Element)->Array1();
  Standard_Integer nvarl = T.Lower(), nvaru = Min(T.Upper(), nvarl + Vec.Length() - 1),
                   i0 = Vec.Lower() - nvarl;

//  Standard_Integer I, i;
  Standard_Integer i;
  
  for(i = nvarl; i <= nvaru; i++) 
    B(T(i)) += Vec(i0 + i);
  
}


//=======================================================================
//function : Solve
//purpose  : 
//=======================================================================
Standard_Boolean FEmTool_Assembly::Solve() 
{
  IsSolved = H->Decompose();

#if DEB
  if (!IsSolved) {
    cout << "Solve Echec  H = " << endl;
    H->OutM();
  }
#endif

/* ????
  while(!IsSolved && count < 5) {
    Standard_Integer i;
    for(i = 1; i <= H->RowNumber(); i++) {
      H->ChangeValue(i,i) *= 2.;
    }
    IsSolved = H->Decompose();
    count++;
  }
*/    

// Standard_Integer count = 0;
  if(!G.IsEmpty() && IsSolved) {
    // calculating H-1 Gt
    math_Vector gi(B.Lower(), B.Upper()), qi(B.Lower(), B.Upper());

    if(GHGt.IsNull() || GHGt->RowNumber() != G.Length()) {
      TColStd_Array1OfInteger FirstIndexes(1, G.Length());
//-----------------------------------------------------------------
      TColStd_Array2OfInteger H1(1, NbGlobVar(), 1, NbGlobVar()); H1.Init(1);
      Standard_Integer i, j, k, l, BlockBeg = 1, BlockEnd;
      Standard_Boolean Block, Zero;
      for(i = 2; i <= NbGlobVar(); i++) {
	BlockEnd = i - 1;
	if(!H->IsInProfile(i, BlockEnd)) {
	  // Maybe, begin of block
	  Block = Standard_True;
	  for(j = i + 1; j <= NbGlobVar(); j++) {
	    if(H->IsInProfile(j, BlockEnd)) {
	      Block = Standard_False;
	      break;
	    }
	  }
	  if(Block) {
	    for(j = i; j <= NbGlobVar(); j++) {
	      for(k = BlockBeg; k <= BlockEnd; k++) {
		H1(j, k) = 0; H1(k, j) = 0;
	      } 
	    }
	    BlockBeg = BlockEnd + 1;
	  }
	  else i = j;
	}
      }
      
      FEmTool_ListIteratorOfListOfVectors Iter1;
      FEmTool_ListIteratorOfListOfVectors Iter2;
      for(i = 1; i <= G.Length(); i++) {
	const FEmTool_ListOfVectors& Gi = G.Value(i);
	for(j = 1; j <= i; j++) {
	  const FEmTool_ListOfVectors& Gj = G.Value(j);
	  Zero = Standard_True;
	  for(Iter1.Initialize(Gi); Iter1.More(); Iter1.Next()) {
	    const Handle(TColStd_HArray1OfReal)& a = Iter1.Value();
	    for(k = a->Lower(); k <= a->Upper(); k++) {
	      for(Iter2.Initialize(Gj); Iter2.More(); Iter2.Next()) {
		const Handle(TColStd_HArray1OfReal)& b = Iter2.Value();
		for(l = b->Lower(); l <= b->Upper(); l++) {
		  if(H1(k, l) != 0) {
		    Zero = Standard_False;
		    break;
		  }
		}
		if(!Zero) break;
	      }
	      if(!Zero) break;
	    }
	    if(!Zero) break;
	  }
	  if(!Zero) {
	    FirstIndexes(i) = j;
	    break;
	  }
	}
      }
//-----------------------------------------------------------------------
//      for(i = FirstIndexes.Lower(); i <= FirstIndexes.Upper(); i++)
//	cout << "FirstIndexes(" << i << ") = " << FirstIndexes(i) << endl;
      //      FirstIndexes.Init(1); // temporary GHGt is full matrix
      GHGt = new FEmTool_ProfileMatrix(FirstIndexes);
    }

    GHGt->Init(0.);
    Standard_Integer i, j, k;
    FEmTool_ListIteratorOfListOfVectors Iter;
    
    for(i = 1; i <= G.Length(); i++) {

      const FEmTool_ListOfVectors& L = G.Value(i);
      gi.Init(0.);
      // preparing i-th line of G (or column of Gt) 
      for(Iter.Initialize(L); Iter.More(); Iter.Next()) {

	const Handle(TColStd_HArray1OfReal)& a = Iter.Value();

	for(j = a->Lower(); j <= a->Upper(); j++) gi(j) = a->Value(j); // gi - full line of G 

      }
                        //                                     -1 t
      H->Solve(gi, qi); // solving H*qi = gi, qi is column of H  G


      //                               -1 t
      // Calculation of product M = G H G
      // for each i all elements of i-th column of M are calculated for k >= i
      for(k = i; k <= G.Length(); k++) {

	if(GHGt->IsInProfile(k, i)) {
	  Standard_Real m = 0.; // m = M(k,i)
	
	  const FEmTool_ListOfVectors& L = G.Value(k);
	
	  for(Iter.Initialize(L); Iter.More(); Iter.Next()) {

	    const Handle(TColStd_HArray1OfReal)& a = Iter.Value();
	    for(j = a->Lower(); j <= a->Upper(); j++) m += qi(j) * a->Value(j); // scalar product of
	                                             // k-th line of G and i-th column of H-1 Gt 

	  }
	
	  GHGt->ChangeValue(k, i) = m;
	}
      }

    }


    IsSolved = GHGt->Decompose();
/*    count = 0;
    while(!IsSolved && count < 5) {
      for(i = 1; i <= GHGt->RowNumber(); i++) {
	GHGt->ChangeValue(i,i) *= 2.;
      }
      IsSolved = GHGt->Decompose();
      count++;
    }*/

  }

  return IsSolved;

}


//=======================================================================
//function : Solution
//purpose  : 
//=======================================================================
void FEmTool_Assembly::Solution(math_Vector& Solution) const
{
  if(!IsSolved) StdFail_NotDone::Raise("FEmTool_Assembly::Solution");

  if(G.IsEmpty()) H->Solve(B, Solution);
  else {

    math_Vector v1(B.Lower(), B.Upper());
    H->Solve(B, v1);
    
    math_Vector l(1, G.Length()), v2(1, G.Length());
    Standard_Integer i, j;
    FEmTool_ListIteratorOfListOfVectors Iter;
    
    for(i = 1; i <= G.Length(); i++) {
      
      const FEmTool_ListOfVectors& L = G.Value(i);
      Standard_Real m = 0.;
      
      for(Iter.Initialize(L); Iter.More(); Iter.Next()) {
	
	const Handle(TColStd_HArray1OfReal)& a = Iter.Value();
	for(j = a->Lower(); j <= a->Upper(); j++) 
	  m += v1(j) * a->Value(j); // scalar product
	// G v1
      }
      
      v2(i) = m - C.Value(i);
    }
    
    GHGt->Solve(v2, l); // Solving M*l = v2
    
    v1 = B;
    // Calculation v1 = B-Gt*l
    // v1(j) = B(j) - Gt(j,i)*l(i) = B(j) - G(i,j)*l(i)
    // 
      for(i = 1; i <= G.Length(); i++) {
	
	const FEmTool_ListOfVectors& L = G.Value(i);
	
	for(Iter.Initialize(L); Iter.More(); Iter.Next()) {
	  
	  const Handle(TColStd_HArray1OfReal)& a = Iter.Value();
	  for(j = a->Lower(); j <= a->Upper(); j++) v1(j) -= l(i) * a->Value(j);
	  
	}
	
      }
      
    H->Solve(v1, Solution);
  }
  
}

Standard_Integer FEmTool_Assembly::NbGlobVar() const
{

  return B.Length();

}

void FEmTool_Assembly::GetAssemblyTable(Handle(FEmTool_HAssemblyTable)& AssTable) const
{
  AssTable = myRefTable;
}


void FEmTool_Assembly::ResetConstraint() 
{
  G.Clear();
  C.Clear();
}

void FEmTool_Assembly::NullifyConstraint() 
{
  FEmTool_ListIteratorOfListOfVectors Iter;
  Standard_Integer i;

  for(i = 1; i <= G.Length(); i++) {
  
    C.SetValue(i, 0.);

    for(Iter.Initialize(G.Value(i)); Iter.More(); Iter.Next()) 
      Iter.Value()->Init(0.);

  }

}


//=======================================================================
//function : AddConstraint
//purpose  : 
//=======================================================================
void FEmTool_Assembly::AddConstraint(const Standard_Integer IndexofConstraint,
				     const Standard_Integer Element,
				     const Standard_Integer Dimension,
				     const math_Vector& LinearForm,
				     const Standard_Real Value) 
{
  while(G.Length() < IndexofConstraint) {
    // Add new lines in G
    FEmTool_ListOfVectors L;
    G.Append(L);
    C.Append(0.);
  }

  FEmTool_ListOfVectors& L = G.ChangeValue(IndexofConstraint);
  
  Handle(TColStd_HArray1OfInteger) Indexes = myRefTable->Value(Dimension,Element);
  Standard_Integer i, Imax = 0, Imin = NbGlobVar();

  for(i = Indexes->Lower(); i <= Indexes->Upper(); i++) {
    Imin = Min(Imin, Indexes->Value(i));
    Imax = Max(Imax, Indexes->Value(i));
  }

  Handle(TColStd_HArray1OfReal) Coeff;

  if(L.IsEmpty()) {
    Coeff = new TColStd_HArray1OfReal(Imin,Imax);
    Coeff->Init(0.);
    L.Append(Coeff);
  }
  else {
    FEmTool_ListIteratorOfListOfVectors Iter(L);
    Standard_Integer i;
    Standard_Real  s1 = 0, s2 = 0;
    Handle(TColStd_HArray1OfReal) Aux1, Aux2;
    for(i=1; Iter.More(); Iter.Next(), i++) {
      if(Imin >= Iter.Value()->Lower()) {
	s1 = i;
	Aux1 = Iter.Value();
	if(Imax <= Iter.Value()->Upper()) {
	  s2 = s1;
	  Coeff = Iter.Value();
	  break;
	}
      }

      if(Imax <= Iter.Value()->Upper()) {
	s2 = i;
	Aux2 = Iter.Value();
      }
    }

    if(s1 != s2) {
      if(s1 == 0) {
	if(Imax < Aux2->Lower()) {
	  // inserting before first segment
	  Coeff = new TColStd_HArray1OfReal(Imin,Imax);
	  Coeff->Init(0.);
	  L.Prepend(Coeff);
	}
	else {
	  // merge new and first segment
	  Coeff = new TColStd_HArray1OfReal(Imin, Aux2->Upper());
	  for(i = Imin; i <= Aux2->Lower() - 1; i++) Coeff->SetValue(i, 0.);
	  for(i = Aux2->Lower(); i <= Aux2->Upper(); i++) Coeff->SetValue(i, Aux2->Value(i));
	  L.First() = Coeff;
	}
      }
      else if(s2 == 0) {
	if(Imin > Aux1->Upper()) {
	  // append new
	  Coeff = new TColStd_HArray1OfReal(Imin,Imax);
	  Coeff->Init(0.);
	  L.Append(Coeff);
	}
	else {
	  // merge new and last segment
	  Coeff = new TColStd_HArray1OfReal(Aux1->Lower(), Imax);
	  for(i = Aux1->Lower(); i <= Aux1->Upper(); i++) Coeff->SetValue(i, Aux1->Value(i));
	  for(i = Aux1->Upper() + 1; i <= Imax; i++) Coeff->SetValue(i, 0.);
	  L.Last() = Coeff;
	}
      }
      else if(Imin <= Aux1->Upper() && Imax < Aux2->Lower()) {
	// merge s1 and new
	Coeff = new TColStd_HArray1OfReal(Aux1->Lower(), Imax);
	for(i = Aux1->Lower(); i <= Aux1->Upper(); i++) Coeff->SetValue(i, Aux1->Value(i));
	for(i = Aux1->Upper() + 1; i <= Imax; i++) Coeff->SetValue(i, 0.);
	Iter.Initialize(L);
	for(i = 1; i < s1; Iter.Next(), i++);
	Iter.Value() = Coeff;
      }
      else if(Imin > Aux1->Upper() && Imax >= Aux2->Lower()) { 
	// merge new and first segment
	Coeff = new TColStd_HArray1OfReal(Imin, Aux2->Upper());
	for(i = Imin; i <= Aux2->Lower() - 1; i++) Coeff->SetValue(i, 0.);
	for(i = Aux2->Lower(); i <= Aux2->Upper(); i++) Coeff->SetValue(i, Aux2->Value(i));
	Iter.Initialize(L);
	for(i = 1; i < s2; Iter.Next(), i++);
	Iter.Value() = Coeff;
     }
      else if(Imin > Aux1->Upper() && Imax < Aux2->Lower()) {
	// inserting new between s1 and s2
	Coeff = new TColStd_HArray1OfReal(Imin,Imax);
	Coeff->Init(0.);
	Iter.Initialize(L);
	for(i = 1; i < s1; Iter.Next(), i++);
	L.InsertAfter(Coeff,Iter);
      }
      else {
	// merge s1, new, s2 and remove s2 
	Coeff = new TColStd_HArray1OfReal(Aux1->Lower(), Aux2->Upper());
	for(i = Aux1->Lower(); i <= Aux1->Upper(); i++) Coeff->SetValue(i, Aux1->Value(i));
	for(i = Aux1->Upper() + 1; i <= Aux2->Lower() - 1; i++) Coeff->SetValue(i, 0.);
	for(i = Aux2->Lower(); i <= Aux2->Upper(); i++) Coeff->SetValue(i, Aux2->Value(i));
	Iter.Initialize(L);
	for(i = 1; i < s1; Iter.Next(), i++);
	Iter.Value() = Coeff;
	Iter.Next();
	L.Remove(Iter);
      }
    }
  }

  // adding 
  Standard_Integer j = LinearForm.Lower();
  for(i = Indexes->Lower(); i <= Indexes->Upper(); i++, j++) {
    Coeff->ChangeValue(Indexes->Value(i)) += LinearForm(j);
  }
  
  C.ChangeValue(IndexofConstraint) += Value;


}
Commit	Line	Data
7fd59977	1	// File: FEmTool_Assembly.cxx
	2	// Created: Tue Nov 17 12:07:58 1998
	3	// Author: Igor FEOKTISTOV
	4	// <ifv@paradox.nnov.matra-dtv.fr>
	5
	6
	7	#include <FEmTool_Assembly.ixx>
	8	#include <FEmTool_ListIteratorOfListOfVectors.hxx>
	9	#include <FEmTool_ListOfVectors.hxx>
	10	#include <TColStd_HArray1OfReal.hxx>
	11	#include <math_Matrix.hxx>
	12
	13	//----------------------------------------------------------------------------
	14	// Purpose - to find min index of global variables and define
	15	//----------------------------------------------------------------------------
	16	static Standard_Integer MinIndex(const Handle(FEmTool_HAssemblyTable)& Table)
	17	{
	18	Standard_Integer dim, el, nvar, Imin;
	19	Standard_Integer diml = Table->LowerRow(), dimu = Table->UpperRow(),
	20	ell = Table->LowerCol(), elu = Table->UpperCol(), nvarl, nvaru;
	21
	22	Handle(TColStd_HArray1OfInteger) T = Table->Value(diml, ell);
	23
	24	nvarl = T->Lower();
	25
	26	Imin = T->Value(nvarl);
	27
	28	for(dim = diml; dim <= dimu; dim++)
	29	for(el = ell; el <= elu; el++) {
	30	T = Table->Value(dim, el);
	31	nvarl = T->Lower(); nvaru = T->Upper();
	32	for(nvar = nvarl; nvar <= nvaru; nvar++) {
	33	Imin = Min(Imin, T->Value(nvar));
	34	}
	35	}
	36	return Imin;
	37	}
	38
	39	//----------------------------------------------------------------------------
	40	// Purpose - to find max index of global variables
	41	//----------------------------------------------------------------------------
	42	static Standard_Integer MaxIndex(const Handle(FEmTool_HAssemblyTable)& Table)
	43	{
	44	Standard_Integer dim, el, nvar, Imax;
	45	Standard_Integer diml = Table->LowerRow(), dimu = Table->UpperRow(),
	46	ell = Table->LowerCol(), elu = Table->UpperCol(), nvarl, nvaru;
	47
	48	Handle(TColStd_HArray1OfInteger) T = Table->Value(diml, ell);
	49
	50	nvarl = T->Lower();
	51
	52	Imax = T->Value(nvarl);
	53
	54	for(dim = diml; dim <= dimu; dim++)
	55	for(el = ell; el <= elu; el++) {
	56	T = Table->Value(dim, el);
	57	nvarl = T->Lower(); nvaru = T->Upper();
	58	for(nvar = nvarl; nvar <= nvaru; nvar++) {
	59	Imax = Max(Imax, T->Value(nvar));
	60	}
	61	}
	62	return Imax;
	63	}
	64
65
66
67	//=======================================================================
68	//function : FEmTool_Assembly
69	//purpose :
70	//=======================================================================
71	FEmTool_Assembly::FEmTool_Assembly(const TColStd_Array2OfInteger& Dependence,
72	const Handle(FEmTool_HAssemblyTable)& Table):
73	myDepTable(1, Dependence.ColLength(), 1, Dependence.RowLength()),
74	B(MinIndex(Table), MaxIndex(Table))
75
76	{
77	IsSolved = Standard_False;
78	myDepTable = Dependence;
79	myRefTable = Table;
80
81	TColStd_Array1OfInteger FirstIndexes(1, B.Length()); FirstIndexes.Init(B.Length());
82
83	#ifdef DEB
84	Standard_Integer dim, el, nvar, Imax, Imin, I0 = 1 - B.Lower(), i;
85	#else
86	Standard_Integer dim, el, nvar, Imin, I0 = 1 - B.Lower(), i;
87	#endif
88	Standard_Integer diml = Table->LowerRow(), dimu = Table->UpperRow(),
89	ell = Table->LowerCol(), elu = Table->UpperCol(), nvarl, nvaru;
90
91	Handle(TColStd_HArray1OfInteger) T;
92	for(dim = diml; dim <= dimu; dim++)
93	for(el = ell; el <= elu; el++) {
94
95	T = Table->Value(dim, el);
96	nvarl = T->Lower(); nvaru = T->Upper();
97	Imin = T->Value(nvarl) + I0;
98
99	for(nvar = nvarl; nvar <= nvaru; nvar++)
100	Imin = Min(Imin, T->Value(nvar) + I0);
101
102	for(nvar = nvarl; nvar <= nvaru; nvar++) {
103	i = T->Value(nvar) + I0;
104	FirstIndexes(i) = Min(FirstIndexes(i), Imin);
105	}
106	}
107
108
109	H = new FEmTool_ProfileMatrix(FirstIndexes);
110
111	NullifyMatrix();
112	NullifyVector();
113	}
114
115	void FEmTool_Assembly::NullifyMatrix()
116	{
117	H->Init(0.);
118	IsSolved = Standard_False;
119	}
120
121
122	//=======================================================================
123	//function : AddMatrix
124	//purpose :
125	//=======================================================================
126	void FEmTool_Assembly::AddMatrix(const Standard_Integer Element,
127	const Standard_Integer Dimension1,
128	const Standard_Integer Dimension2,
129	const math_Matrix& Mat)
130	{
131
132	if(myDepTable(Dimension1, Dimension2) == 0)
133	Standard_DomainError::Raise("FEmTool_Assembly::AddMatrix");
134
135	const TColStd_Array1OfInteger & T1 = myRefTable->Value(Dimension1,Element)->Array1();
136	const TColStd_Array1OfInteger & T2 = myRefTable->Value(Dimension2,Element)->Array1();
137
138	Standard_Integer nvarl = T1.Lower(), nvaru = Min(T1.Upper(), nvarl + Mat.RowNumber() - 1);
139
140	#ifdef DEB
141	Standard_Integer I, J, I0 = 1 - B.Lower(), i, ii, j, jj,
142	#else
143	Standard_Integer I, J, I0 = 1 - B.Lower(), i, ii, j,
144	#endif
145	i0 = Mat.LowerRow() - nvarl, j0 = Mat.LowerCol() - nvarl;
146
147	for(i = nvarl; i <= nvaru; i++) {
148	I = T1(i) + I0;
149	ii = i0+i;
150	for(j = nvarl; j <= i; j++) {
151	J = T2(j) + I0;
152	H->ChangeValue(I, J) += Mat(ii, j0+j);
153	}
154	}
155
156	IsSolved = Standard_False;
157	}
158
159
160	//=======================================================================
161	//function : NullifyVector
162	//purpose :
163	//=======================================================================
164	void FEmTool_Assembly::NullifyVector()
165	{
166	B.Init(0.);
167	}
168
169	//=======================================================================
170	//function : AddVector
171	//purpose :
172	//=======================================================================
173	void FEmTool_Assembly::AddVector(const Standard_Integer Element,
174	const Standard_Integer Dimension,
175	const math_Vector& Vec)
176	{
177	const TColStd_Array1OfInteger & T = myRefTable->Value(Dimension,Element)->Array1();
178	Standard_Integer nvarl = T.Lower(), nvaru = Min(T.Upper(), nvarl + Vec.Length() - 1),
179	i0 = Vec.Lower() - nvarl;
180
181	// Standard_Integer I, i;
182	Standard_Integer i;
183
184	for(i = nvarl; i <= nvaru; i++)
185	B(T(i)) += Vec(i0 + i);
186
187	}
188
189
190	//=======================================================================
191	//function : Solve
192	//purpose :
193	//=======================================================================
194	Standard_Boolean FEmTool_Assembly::Solve()
195	{
196	IsSolved = H->Decompose();
197
198	#if DEB
199	if (!IsSolved) {
200	cout << "Solve Echec H = " << endl;
201	H->OutM();
202	}
203	#endif
204
205	/* ????
206	while(!IsSolved && count < 5) {
207	Standard_Integer i;
208	for(i = 1; i <= H->RowNumber(); i++) {
209	H->ChangeValue(i,i) *= 2.;
210	}
211	IsSolved = H->Decompose();
212	count++;
213	}
214	*/
215
216	// Standard_Integer count = 0;
217	if(!G.IsEmpty() && IsSolved) {
218	// calculating H-1 Gt
219	math_Vector gi(B.Lower(), B.Upper()), qi(B.Lower(), B.Upper());
220
221	if(GHGt.IsNull() \|\| GHGt->RowNumber() != G.Length()) {
222	TColStd_Array1OfInteger FirstIndexes(1, G.Length());
223	//-----------------------------------------------------------------
224	TColStd_Array2OfInteger H1(1, NbGlobVar(), 1, NbGlobVar()); H1.Init(1);
225	Standard_Integer i, j, k, l, BlockBeg = 1, BlockEnd;
226	Standard_Boolean Block, Zero;
227	for(i = 2; i <= NbGlobVar(); i++) {
228	BlockEnd = i - 1;
229	if(!H->IsInProfile(i, BlockEnd)) {
230	// Maybe, begin of block
231	Block = Standard_True;
232	for(j = i + 1; j <= NbGlobVar(); j++) {
233	if(H->IsInProfile(j, BlockEnd)) {
234	Block = Standard_False;
235	break;
236	}
237	}
238	if(Block) {
239	for(j = i; j <= NbGlobVar(); j++) {
240	for(k = BlockBeg; k <= BlockEnd; k++) {
241	H1(j, k) = 0; H1(k, j) = 0;
242	}
243	}
244	BlockBeg = BlockEnd + 1;
245	}
246	else i = j;
247	}
248	}
249
250	FEmTool_ListIteratorOfListOfVectors Iter1;
251	FEmTool_ListIteratorOfListOfVectors Iter2;
252	for(i = 1; i <= G.Length(); i++) {
253	const FEmTool_ListOfVectors& Gi = G.Value(i);
254	for(j = 1; j <= i; j++) {
255	const FEmTool_ListOfVectors& Gj = G.Value(j);
256	Zero = Standard_True;
257	for(Iter1.Initialize(Gi); Iter1.More(); Iter1.Next()) {
258	const Handle(TColStd_HArray1OfReal)& a = Iter1.Value();
259	for(k = a->Lower(); k <= a->Upper(); k++) {
260	for(Iter2.Initialize(Gj); Iter2.More(); Iter2.Next()) {
261	const Handle(TColStd_HArray1OfReal)& b = Iter2.Value();
262	for(l = b->Lower(); l <= b->Upper(); l++) {
263	if(H1(k, l) != 0) {
264	Zero = Standard_False;
265	break;
266	}
267	}
268	if(!Zero) break;
269	}
270	if(!Zero) break;
271	}
272	if(!Zero) break;
273	}
274	if(!Zero) {
275	FirstIndexes(i) = j;
276	break;
277	}
278	}
279	}
280	//-----------------------------------------------------------------------
281	// for(i = FirstIndexes.Lower(); i <= FirstIndexes.Upper(); i++)
282	// cout << "FirstIndexes(" << i << ") = " << FirstIndexes(i) << endl;
283	// FirstIndexes.Init(1); // temporary GHGt is full matrix
284	GHGt = new FEmTool_ProfileMatrix(FirstIndexes);
285	}
286
287	GHGt->Init(0.);
288	Standard_Integer i, j, k;
289	FEmTool_ListIteratorOfListOfVectors Iter;
290
291	for(i = 1; i <= G.Length(); i++) {
292
293	const FEmTool_ListOfVectors& L = G.Value(i);
294	gi.Init(0.);
295	// preparing i-th line of G (or column of Gt)
296	for(Iter.Initialize(L); Iter.More(); Iter.Next()) {
297
298	const Handle(TColStd_HArray1OfReal)& a = Iter.Value();
299
300	for(j = a->Lower(); j <= a->Upper(); j++) gi(j) = a->Value(j); // gi - full line of G
301
302	}
303	// -1 t
304	H->Solve(gi, qi); // solving H*qi = gi, qi is column of H G
305
306
307	// -1 t
308	// Calculation of product M = G H G
309	// for each i all elements of i-th column of M are calculated for k >= i
310	for(k = i; k <= G.Length(); k++) {
311
312	if(GHGt->IsInProfile(k, i)) {
313	Standard_Real m = 0.; // m = M(k,i)
314
315	const FEmTool_ListOfVectors& L = G.Value(k);
316
317	for(Iter.Initialize(L); Iter.More(); Iter.Next()) {
318
319	const Handle(TColStd_HArray1OfReal)& a = Iter.Value();
320	for(j = a->Lower(); j <= a->Upper(); j++) m += qi(j) * a->Value(j); // scalar product of
321	// k-th line of G and i-th column of H-1 Gt
322
323	}
324
325	GHGt->ChangeValue(k, i) = m;
326	}
327	}
328
329	}
330
331
332	IsSolved = GHGt->Decompose();
333	/* count = 0;
334	while(!IsSolved && count < 5) {
335	for(i = 1; i <= GHGt->RowNumber(); i++) {
336	GHGt->ChangeValue(i,i) *= 2.;
337	}
338	IsSolved = GHGt->Decompose();
339	count++;
340	}*/
341
342	}
343
344	return IsSolved;
345
346	}
347
348
349	//=======================================================================
350	//function : Solution
351	//purpose :
352	//=======================================================================
353	void FEmTool_Assembly::Solution(math_Vector& Solution) const
354	{
355	if(!IsSolved) StdFail_NotDone::Raise("FEmTool_Assembly::Solution");
356
357	if(G.IsEmpty()) H->Solve(B, Solution);
358	else {
359
360	math_Vector v1(B.Lower(), B.Upper());
361	H->Solve(B, v1);
362
363	math_Vector l(1, G.Length()), v2(1, G.Length());
364	Standard_Integer i, j;
365	FEmTool_ListIteratorOfListOfVectors Iter;
366
367	for(i = 1; i <= G.Length(); i++) {
368
369	const FEmTool_ListOfVectors& L = G.Value(i);
370	Standard_Real m = 0.;
371
372	for(Iter.Initialize(L); Iter.More(); Iter.Next()) {
373
374	const Handle(TColStd_HArray1OfReal)& a = Iter.Value();
375	for(j = a->Lower(); j <= a->Upper(); j++)
376	m += v1(j) * a->Value(j); // scalar product
377	// G v1
378	}
379
380	v2(i) = m - C.Value(i);
381	}
382
383	GHGt->Solve(v2, l); // Solving M*l = v2
384
385	v1 = B;
386	// Calculation v1 = B-Gt*l
387	// v1(j) = B(j) - Gt(j,i)l(i) = B(j) - G(i,j)l(i)
388	//
389	for(i = 1; i <= G.Length(); i++) {
390
391	const FEmTool_ListOfVectors& L = G.Value(i);
392
393	for(Iter.Initialize(L); Iter.More(); Iter.Next()) {
394
395	const Handle(TColStd_HArray1OfReal)& a = Iter.Value();
396	for(j = a->Lower(); j <= a->Upper(); j++) v1(j) -= l(i) * a->Value(j);
397
398	}
399
400	}
401
402	H->Solve(v1, Solution);
403	}
404
405	}
406
407	Standard_Integer FEmTool_Assembly::NbGlobVar() const
408	{
409
410	return B.Length();
411
412	}
413
414	void FEmTool_Assembly::GetAssemblyTable(Handle(FEmTool_HAssemblyTable)& AssTable) const
415	{
416	AssTable = myRefTable;
417	}
418
419
420	void FEmTool_Assembly::ResetConstraint()
421	{
422	G.Clear();
423	C.Clear();
424	}
425
426	void FEmTool_Assembly::NullifyConstraint()
427	{
428	FEmTool_ListIteratorOfListOfVectors Iter;
429	Standard_Integer i;
430
431	for(i = 1; i <= G.Length(); i++) {
432
433	C.SetValue(i, 0.);
434
435	for(Iter.Initialize(G.Value(i)); Iter.More(); Iter.Next())
436	Iter.Value()->Init(0.);
437
438	}
439
440	}
441
442
443	//=======================================================================
444	//function : AddConstraint
445	//purpose :
446	//=======================================================================
447	void FEmTool_Assembly::AddConstraint(const Standard_Integer IndexofConstraint,
448	const Standard_Integer Element,
449	const Standard_Integer Dimension,
450	const math_Vector& LinearForm,
451	const Standard_Real Value)
452	{
453	while(G.Length() < IndexofConstraint) {
454	// Add new lines in G
455	FEmTool_ListOfVectors L;
456	G.Append(L);
457	C.Append(0.);
458	}
459
460	FEmTool_ListOfVectors& L = G.ChangeValue(IndexofConstraint);
461
462	Handle(TColStd_HArray1OfInteger) Indexes = myRefTable->Value(Dimension,Element);
463	Standard_Integer i, Imax = 0, Imin = NbGlobVar();
464
465	for(i = Indexes->Lower(); i <= Indexes->Upper(); i++) {
466	Imin = Min(Imin, Indexes->Value(i));
467	Imax = Max(Imax, Indexes->Value(i));
468	}
469
470	Handle(TColStd_HArray1OfReal) Coeff;
471
472	if(L.IsEmpty()) {
473	Coeff = new TColStd_HArray1OfReal(Imin,Imax);
474	Coeff->Init(0.);
475	L.Append(Coeff);
476	}
477	else {
478	FEmTool_ListIteratorOfListOfVectors Iter(L);
479	Standard_Integer i;
480	Standard_Real s1 = 0, s2 = 0;
481	Handle(TColStd_HArray1OfReal) Aux1, Aux2;
482	for(i=1; Iter.More(); Iter.Next(), i++) {
483	if(Imin >= Iter.Value()->Lower()) {
484	s1 = i;
485	Aux1 = Iter.Value();
486	if(Imax <= Iter.Value()->Upper()) {
487	s2 = s1;
488	Coeff = Iter.Value();
489	break;
490	}
491	}
492
493	if(Imax <= Iter.Value()->Upper()) {
494	s2 = i;
495	Aux2 = Iter.Value();
496	}
497	}
498
499	if(s1 != s2) {
500	if(s1 == 0) {
501	if(Imax < Aux2->Lower()) {
502	// inserting before first segment
503	Coeff = new TColStd_HArray1OfReal(Imin,Imax);
504	Coeff->Init(0.);
505	L.Prepend(Coeff);
506	}
507	else {
508	// merge new and first segment
509	Coeff = new TColStd_HArray1OfReal(Imin, Aux2->Upper());
510	for(i = Imin; i <= Aux2->Lower() - 1; i++) Coeff->SetValue(i, 0.);
511	for(i = Aux2->Lower(); i <= Aux2->Upper(); i++) Coeff->SetValue(i, Aux2->Value(i));
512	L.First() = Coeff;
513	}
514	}
515	else if(s2 == 0) {
516	if(Imin > Aux1->Upper()) {
517	// append new
518	Coeff = new TColStd_HArray1OfReal(Imin,Imax);
519	Coeff->Init(0.);
520	L.Append(Coeff);
521	}
522	else {
523	// merge new and last segment
524	Coeff = new TColStd_HArray1OfReal(Aux1->Lower(), Imax);
525	for(i = Aux1->Lower(); i <= Aux1->Upper(); i++) Coeff->SetValue(i, Aux1->Value(i));
526	for(i = Aux1->Upper() + 1; i <= Imax; i++) Coeff->SetValue(i, 0.);
527	L.Last() = Coeff;
528	}
529	}
530	else if(Imin <= Aux1->Upper() && Imax < Aux2->Lower()) {
531	// merge s1 and new
532	Coeff = new TColStd_HArray1OfReal(Aux1->Lower(), Imax);
533	for(i = Aux1->Lower(); i <= Aux1->Upper(); i++) Coeff->SetValue(i, Aux1->Value(i));
534	for(i = Aux1->Upper() + 1; i <= Imax; i++) Coeff->SetValue(i, 0.);
535	Iter.Initialize(L);
536	for(i = 1; i < s1; Iter.Next(), i++);
537	Iter.Value() = Coeff;
538	}
539	else if(Imin > Aux1->Upper() && Imax >= Aux2->Lower()) {
540	// merge new and first segment
541	Coeff = new TColStd_HArray1OfReal(Imin, Aux2->Upper());
542	for(i = Imin; i <= Aux2->Lower() - 1; i++) Coeff->SetValue(i, 0.);
543	for(i = Aux2->Lower(); i <= Aux2->Upper(); i++) Coeff->SetValue(i, Aux2->Value(i));
544	Iter.Initialize(L);
545	for(i = 1; i < s2; Iter.Next(), i++);
546	Iter.Value() = Coeff;
547	}
548	else if(Imin > Aux1->Upper() && Imax < Aux2->Lower()) {
549	// inserting new between s1 and s2
550	Coeff = new TColStd_HArray1OfReal(Imin,Imax);
551	Coeff->Init(0.);
552	Iter.Initialize(L);
553	for(i = 1; i < s1; Iter.Next(), i++);
554	L.InsertAfter(Coeff,Iter);
555	}
556	else {
557	// merge s1, new, s2 and remove s2
558	Coeff = new TColStd_HArray1OfReal(Aux1->Lower(), Aux2->Upper());
559	for(i = Aux1->Lower(); i <= Aux1->Upper(); i++) Coeff->SetValue(i, Aux1->Value(i));
560	for(i = Aux1->Upper() + 1; i <= Aux2->Lower() - 1; i++) Coeff->SetValue(i, 0.);
561	for(i = Aux2->Lower(); i <= Aux2->Upper(); i++) Coeff->SetValue(i, Aux2->Value(i));
562	Iter.Initialize(L);
563	for(i = 1; i < s1; Iter.Next(), i++);
564	Iter.Value() = Coeff;
565	Iter.Next();
566	L.Remove(Iter);
567	}
568	}
569	}
570
571	// adding
572	Standard_Integer j = LinearForm.Lower();
573	for(i = Indexes->Lower(); i <= Indexes->Upper(); i++, j++) {
574	Coeff->ChangeValue(Indexes->Value(i)) += LinearForm(j);
575	}
576
577	C.ChangeValue(IndexofConstraint) += Value;
578
579
580
581	}
582