| 1 |
|
5 |
//------------------------------------------------------------
|
| 2 |
|
|
//------------------------------------------------------------
|
| 3 |
|
|
// MAGiC
|
| 4 |
|
|
// Jean Christophe Cuillière et Vincent FRANCOIS
|
| 5 |
|
|
// Département de Génie Mécanique - UQTR
|
| 6 |
|
|
//------------------------------------------------------------
|
| 7 |
|
|
// Le projet MAGIC est un projet de recherche du département
|
| 8 |
|
|
// de génie mécanique de l'Université du Québec à
|
| 9 |
|
|
// Trois Rivières
|
| 10 |
|
|
// Les librairies ne peuvent être utilisées sans l'accord
|
| 11 |
|
|
// des auteurs (contact : francois@uqtr.ca)
|
| 12 |
|
|
//------------------------------------------------------------
|
| 13 |
|
|
//------------------------------------------------------------
|
| 14 |
|
|
//
|
| 15 |
|
|
// ot_systeme.cpp
|
| 16 |
|
|
//
|
| 17 |
|
|
//------------------------------------------------------------
|
| 18 |
|
|
//------------------------------------------------------------
|
| 19 |
|
|
// COPYRIGHT 2000
|
| 20 |
|
|
// Version du 02/03/2006 à 11H23
|
| 21 |
|
|
//------------------------------------------------------------
|
| 22 |
|
|
//------------------------------------------------------------
|
| 23 |
|
|
|
| 24 |
|
|
|
| 25 |
|
|
#include "gestionversion.h"
|
| 26 |
|
|
|
| 27 |
|
|
#pragma hdrstop
|
| 28 |
|
|
#include <alloc.h>
|
| 29 |
|
|
#include <stdlib.h>
|
| 30 |
|
|
#include <math.h>
|
| 31 |
|
|
#include "ot_systeme.h"
|
| 32 |
|
|
#include "ot_mathematique.h"
|
| 33 |
|
|
|
| 34 |
|
|
//---------------------------------------------------------------------------
|
| 35 |
|
|
|
| 36 |
|
|
OT_SYSTEME::OT_SYSTEME()
|
| 37 |
|
|
{
|
| 38 |
|
|
}
|
| 39 |
|
|
|
| 40 |
|
|
|
| 41 |
|
|
|
| 42 |
|
|
OT_SYSTEME::~OT_SYSTEME()
|
| 43 |
|
|
{
|
| 44 |
|
|
}
|
| 45 |
|
|
|
| 46 |
|
|
|
| 47 |
|
|
int OT_SYSTEME::resoud_gauss(double *a,double* b,double *x,int n)
|
| 48 |
|
|
{
|
| 49 |
|
|
double *aaug=new double[(n+1)*n];
|
| 50 |
|
|
#define a(i,j) (*(a+(i)*(n)+j))
|
| 51 |
|
|
#define aaug(i,j) (*(aaug+(i)*(n+1)+j))
|
| 52 |
|
|
for (int i=0;i<n;i++)
|
| 53 |
|
|
for (int j=0;j<n;j++)
|
| 54 |
|
|
aaug(i,j)=a(i,j);
|
| 55 |
|
|
for (int k=0;k<n;k++)
|
| 56 |
|
|
aaug(k,n)=b[k];
|
| 57 |
|
|
for (int j=0;j<n-1;j++)
|
| 58 |
|
|
{
|
| 59 |
|
|
// recherche le pivot
|
| 60 |
|
|
double pvt=fabs(aaug(j,j));
|
| 61 |
|
|
int ligne_pivot_debut=j;
|
| 62 |
|
|
int ligne_pivot=j;
|
| 63 |
|
|
|
| 64 |
|
|
for (int i=j+1;i<n;i++)
|
| 65 |
|
|
if (fabs(aaug(i,j))>pvt)
|
| 66 |
|
|
{
|
| 67 |
|
|
pvt=fabs(aaug(i,j));
|
| 68 |
|
|
ligne_pivot=i;
|
| 69 |
|
|
}
|
| 70 |
|
|
if (ligne_pivot!=ligne_pivot_debut) // trouver meilleur
|
| 71 |
|
|
{
|
| 72 |
|
|
for (int col=j;col<n+1;col++)
|
| 73 |
|
|
{
|
| 74 |
|
|
double temp=aaug(ligne_pivot_debut,col);
|
| 75 |
|
|
aaug(ligne_pivot_debut,col)=aaug(ligne_pivot,col);
|
| 76 |
|
|
aaug(ligne_pivot,col)=temp;
|
| 77 |
|
|
}
|
| 78 |
|
|
}
|
| 79 |
|
|
// j ai mon pivot max
|
| 80 |
|
|
if (fabs(aaug(j,j))< 0.00000000000001) {delete [] aaug;return 0;};
|
| 81 |
|
|
// j ai mon pivot max non nul
|
| 82 |
|
|
for (int ii=j+1;ii<n;ii++) aaug(ii,j)=aaug(ii,j)/aaug(j,j);
|
| 83 |
|
|
|
| 84 |
|
|
for (int iii=j+1;iii<n;iii++)
|
| 85 |
|
|
for (int k=j+1;k<n+1;k++)
|
| 86 |
|
|
aaug(iii,k)=aaug(iii,k)-aaug(iii,j)*aaug(j,k);
|
| 87 |
|
|
}
|
| 88 |
|
|
|
| 89 |
|
|
if (fabs(aaug(n-1,n-1))<0.00000000000001) {delete [] aaug;return 0;};
|
| 90 |
|
|
// back substitution
|
| 91 |
|
|
x[n-1]=aaug(n-1,n)/aaug(n-1,n-1);
|
| 92 |
|
|
for (int j=n-2;j>-1;j--)
|
| 93 |
|
|
{
|
| 94 |
|
|
x[j]=aaug(j,n);
|
| 95 |
|
|
for (int k=n-1;k>j;k--)
|
| 96 |
|
|
x[j]=x[j]-aaug(j,k)*x[k];
|
| 97 |
|
|
x[j]=x[j]/aaug(j,j);
|
| 98 |
|
|
}
|
| 99 |
|
|
|
| 100 |
|
|
|
| 101 |
|
|
delete [] aaug;
|
| 102 |
|
|
return 1;
|
| 103 |
|
|
#undef a // enlever le (i,j) pour enlever le warning
|
| 104 |
|
|
#undef aaug
|
| 105 |
|
|
}
|
| 106 |
|
|
|
| 107 |
|
|
int OT_SYSTEME::resoud_QR(double *m, double *b, double *x, int dim1,int dim2)
|
| 108 |
|
|
{
|
| 109 |
|
|
double *mat=new double[dim1*(dim2+1)];
|
| 110 |
|
|
double *r=new double[dim2];
|
| 111 |
|
|
#define mat(i,j) (*(mat+(i)*(dim2+1)+(j)))
|
| 112 |
|
|
#define m(i,j) (*(m+(i)*dim2+(j)))
|
| 113 |
|
|
// initialisation avec mat augmentée
|
| 114 |
|
|
for (int i=0;i<dim1;i++)
|
| 115 |
|
|
{
|
| 116 |
|
|
for (int j=0;j<dim2;j++)
|
| 117 |
|
|
mat(i,j)=m(i,j);
|
| 118 |
|
|
mat(i,dim2)=b[i];
|
| 119 |
|
|
}
|
| 120 |
|
|
// QR
|
| 121 |
|
|
for (int k=0;k<dim2;k++)
|
| 122 |
|
|
{
|
| 123 |
|
|
double somme=0.;
|
| 124 |
|
|
for (int i=k;i<dim1;i++)
|
| 125 |
|
|
somme=somme+mat(i,k)*mat(i,k);
|
| 126 |
|
|
double alpha=sqrt(somme);
|
| 127 |
|
|
double pivot=mat(k,k);
|
| 128 |
|
|
if (pivot>0) alpha=-alpha;
|
| 129 |
|
|
r[k]=alpha;
|
| 130 |
|
|
if (OPERATEUR::egal(alpha,0.,0.000001))
|
| 131 |
|
|
{
|
| 132 |
|
|
delete [] mat;
|
| 133 |
|
|
delete [] r;
|
| 134 |
|
|
return 0;
|
| 135 |
|
|
}
|
| 136 |
|
|
double beta=somme-pivot*alpha;
|
| 137 |
|
|
pivot=pivot-alpha;
|
| 138 |
|
|
mat(k,k)=pivot;
|
| 139 |
|
|
for (int j=k+1;j<dim2+1;j++)
|
| 140 |
|
|
{
|
| 141 |
|
|
double somme=0.;
|
| 142 |
|
|
for (int i=k;i<dim1;i++)
|
| 143 |
|
|
somme=somme+mat(i,k)*mat(i,j);
|
| 144 |
|
|
double gamma=somme/beta;
|
| 145 |
|
|
for (int i=k;i<dim1;i++)
|
| 146 |
|
|
mat(i,j)=mat(i,j)-gamma*mat(i,k);
|
| 147 |
|
|
}
|
| 148 |
|
|
}
|
| 149 |
|
|
// resolution
|
| 150 |
|
|
x[dim2-1]=mat(dim2-1,dim2)/r[dim2-1];
|
| 151 |
|
|
for (int j=dim2-2;j>-1;j--)
|
| 152 |
|
|
{
|
| 153 |
|
|
x[j]=mat(j,dim2);
|
| 154 |
|
|
for (int k=dim2-1;k>j;k--)
|
| 155 |
|
|
x[j]=x[j]-mat(j,k)*x[k];
|
| 156 |
|
|
x[j]=x[j]/r[j];
|
| 157 |
|
|
}
|
| 158 |
|
|
|
| 159 |
|
|
|
| 160 |
|
|
|
| 161 |
|
|
#undef mat
|
| 162 |
|
|
delete [] mat;
|
| 163 |
|
|
delete [] r;
|
| 164 |
|
|
return 1;
|
| 165 |
|
|
}
|
| 166 |
|
|
|
| 167 |
|
|
|
| 168 |
|
|
|
| 169 |
|
|
|
| 170 |
|
|
|
| 171 |
|
|
|
| 172 |
|
|
int OT_SYSTEME::decompose_LU(double *a,double* res,int dim)
|
| 173 |
|
|
{
|
| 174 |
|
|
#define a(i,j) (*(a+(i)*(dim)+(j)))
|
| 175 |
|
|
#define res(i,j) (*(res+(i)*(dim)+(j)))
|
| 176 |
|
|
|
| 177 |
|
|
if (OPERATEUR::egal(a(0,0),0.,0.00001)==1) return 0;
|
| 178 |
|
|
for (int i=0;i<dim;i++)
|
| 179 |
|
|
res(i,0)=a(i,0);
|
| 180 |
|
|
for (int j=1;j<dim;j++)
|
| 181 |
|
|
res(0,j)=a(0,j)/res(0,0);
|
| 182 |
|
|
|
| 183 |
|
|
|
| 184 |
|
|
for (int j=1;j<dim;j++)
|
| 185 |
|
|
{
|
| 186 |
|
|
for (int i=j;i<dim;i++)
|
| 187 |
|
|
{
|
| 188 |
|
|
double sum=0.;
|
| 189 |
|
|
for (int k=0;k<j;k++)
|
| 190 |
|
|
sum=sum+res(i,k)*res(k,j);
|
| 191 |
|
|
res(i,j)=a(i,j)-sum;
|
| 192 |
|
|
}
|
| 193 |
|
|
for (int i=j+1;i<dim;i++)
|
| 194 |
|
|
{
|
| 195 |
|
|
double sum=0.;
|
| 196 |
|
|
if (OPERATEUR::egal(res(j,j),0.,0.00001)==1) return 0;
|
| 197 |
|
|
for (int k=0;k<j;k++)
|
| 198 |
|
|
sum=sum+res(j,k)*res(k,i);
|
| 199 |
|
|
res(j,i)=(a(j,i)-sum)/res(j,j);
|
| 200 |
|
|
}
|
| 201 |
|
|
|
| 202 |
|
|
}
|
| 203 |
|
|
return 1;
|
| 204 |
|
|
|
| 205 |
|
|
#undef a
|
| 206 |
|
|
#undef res
|
| 207 |
|
|
}
|
| 208 |
|
|
|
| 209 |
|
|
|
| 210 |
|
|
int OT_SYSTEME::resoud_LU(double *matlu,double *b,double *x,int dim)
|
| 211 |
|
|
{
|
| 212 |
|
|
#define matlu(i,j) (*(matlu+(i)*(dim)+(j)))
|
| 213 |
|
|
|
| 214 |
|
|
double* bb=new double[dim];
|
| 215 |
|
|
bb[0]=b[0]/matlu(0,0);
|
| 216 |
|
|
for (int i=1;i<dim;i++)
|
| 217 |
|
|
{
|
| 218 |
|
|
double sum=0.0;
|
| 219 |
|
|
for (int k=0;k<i;k++)
|
| 220 |
|
|
sum=sum+matlu(i,k)*bb[k];
|
| 221 |
|
|
bb[i]=(b[i]-sum)/matlu(i,i);
|
| 222 |
|
|
}
|
| 223 |
|
|
|
| 224 |
|
|
x[dim-1]=bb[dim-1];
|
| 225 |
|
|
for (int j=dim-2;j>=0;j--)
|
| 226 |
|
|
{
|
| 227 |
|
|
double sum=0.0;
|
| 228 |
|
|
for (int k=j+1;k<dim;k++)
|
| 229 |
|
|
sum=sum+matlu(j,k)*x[k];
|
| 230 |
|
|
x[j]=bb[j]-sum;
|
| 231 |
|
|
|
| 232 |
|
|
}
|
| 233 |
|
|
|
| 234 |
|
|
#undef lu
|
| 235 |
|
|
|
| 236 |
|
|
delete [] bb;
|
| 237 |
|
|
return 1;
|
| 238 |
|
|
}
|
| 239 |
|
|
|
| 240 |
|
|
|
| 241 |
|
|
|
| 242 |
|
|
|
| 243 |
|
|
|
| 244 |
|
|
|
| 245 |
|
|
void OT_SYSTEME::get_inv(double *mat1,double *mat2,int n,int *ierr)
|
| 246 |
|
|
{
|
| 247 |
|
|
#define mat(i,j) (*(mataug+(j)*n+i))
|
| 248 |
|
|
#define mat1(i,j) (*(mat1+(j)*n+i))
|
| 249 |
|
|
#define mat2(i,j) (*(mat2+(j)*n+i))
|
| 250 |
|
|
|
| 251 |
|
|
double *mataug=(double*)calloc(2*n*n,sizeof(double));
|
| 252 |
|
|
|
| 253 |
|
|
for (int i=0;i<n;i++)
|
| 254 |
|
|
{
|
| 255 |
|
|
for (int j=0;j<n;j++)
|
| 256 |
|
|
mat(i,j)=mat1(i,j);
|
| 257 |
|
|
mat(i,i+n)=1.;
|
| 258 |
|
|
}
|
| 259 |
|
|
|
| 260 |
|
|
|
| 261 |
|
|
*ierr=0;
|
| 262 |
|
|
|
| 263 |
|
|
for (int j=0;j<n;j++)
|
| 264 |
|
|
{
|
| 265 |
|
|
//recherche du pivot max total de la colonne j
|
| 266 |
|
|
double pivot=fabs(mat(j,j));
|
| 267 |
|
|
int ligne_pivot=j;
|
| 268 |
|
|
for (int i=j+1;i<n;i++)
|
| 269 |
|
|
{
|
| 270 |
|
|
if (fabs(mat(i,j))>pivot)
|
| 271 |
|
|
{
|
| 272 |
|
|
pivot=fabs(mat(i,j));
|
| 273 |
|
|
ligne_pivot=i;
|
| 274 |
|
|
}
|
| 275 |
|
|
}
|
| 276 |
|
|
//tester si le pivot max est nul
|
| 277 |
|
|
if (pivot<0.000000001) {*ierr=1;free(mataug);return;}
|
| 278 |
|
|
//tester si le pivot actuel est max
|
| 279 |
|
|
if (ligne_pivot!=j)
|
| 280 |
|
|
{
|
| 281 |
|
|
for (int i=j;i<2*n;i++)
|
| 282 |
|
|
{
|
| 283 |
|
|
double temp=mat(j,i);
|
| 284 |
|
|
mat(j,i)=mat(ligne_pivot,i);
|
| 285 |
|
|
mat(ligne_pivot,i)=temp;
|
| 286 |
|
|
}
|
| 287 |
|
|
}
|
| 288 |
|
|
//remplir la colonne j de 0
|
| 289 |
|
|
for (int i=j+1;i<n;i++)
|
| 290 |
|
|
mat(i,j)=-mat(i,j)/mat(j,j);
|
| 291 |
|
|
for (int i=j+1;i<n;i++)
|
| 292 |
|
|
for (int k=j+1;k<2*n;k++)
|
| 293 |
|
|
mat(i,k)=mat(i,k)+mat(i,j)*mat(j,k);
|
| 294 |
|
|
}
|
| 295 |
|
|
|
| 296 |
|
|
for (int i=0;i<n;i++)
|
| 297 |
|
|
{
|
| 298 |
|
|
mat2(n-1,i)=mat(n-1,n+i)/mat(n-1,n-1);
|
| 299 |
|
|
for (int j=n-2;j>-1;j--)
|
| 300 |
|
|
{
|
| 301 |
|
|
mat2(j,i)=mat(j,n+i);
|
| 302 |
|
|
for (int k=n-1;k>j;k--)
|
| 303 |
|
|
mat2(j,i)=mat2(j,i)-mat2(k,i)*mat(j,k);
|
| 304 |
|
|
mat2(j,i)=mat2(j,i)/mat(j,j);
|
| 305 |
|
|
}
|
| 306 |
|
|
}
|
| 307 |
|
|
|
| 308 |
|
|
free(mataug);
|
| 309 |
|
|
#undef mat
|
| 310 |
|
|
#undef mat1
|
| 311 |
|
|
#undef mat2
|
| 312 |
|
|
}
|
| 313 |
|
|
|
