00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
#include <stdlib.h>
00037
#include "idmath.h"
00038
#include "idmatric.h"
00039
#include "iderr.h"
00040
#include "idprint.h"
00041
#include "idcalc.h"
00042
00043 #define sqr(x) ((x)*(x) )
00044
00045
00054
double
00055 IdCalcDistance2Points(
double x1,
double y1,
double x2,
double y2)
00056 {
00057
double carre;
00058 carre = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
00059
if (carre == 0.0)
00060
return (0.0);
00061
else
00062
return sqrt(carre);
00063 }
00064
00065
00079
double
00080 IdCalcDistancePointDroite(
double x,
double y,
00081
double x1,
double y1,
00082
double x2,
double y2)
00083 {
00084
double dist, num, denom;
00085
00086 num = abs((
x - x1) * (y2 - y1) - (x2 - x1) * (y - y1));
00087 denom = sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
00088
if ((x1 == x2) && (y1 == y2))
00089 dist = 0.0;
00090
else
00091 dist = num / denom;
00092
00093
return (dist);
00094 }
00095
00102
double
00103 IdCalcAngleFromCotes(
double dx,
double dy)
00104 {
00105
double pente;
00106
if (dx == 0) {
00107
if (dy > 0)
00108 pente = M_PI / 2;
00109
else
00110 pente = 3. * M_PI / 2;
00111 }
else {
00112 pente = ((
float) dy) / ((
float) dx);
00113
if (dx > 0) {
00114
if (dy > 0)
00115 pente = atan(pente);
00116
else
00117 pente = atan(pente) + 2 * M_PI;
00118 }
else
00119 pente = M_PI + atan(pente);
00120 }
00121
return (pente);
00122 }
00123
00137
double
00138 IdCalcAngleFromPoints(
double xB,
double yB,
00139
double xA,
double yA,
00140
double xC,
double yC)
00141 {
00142
double alpha, beta, gamma;
00143
double xAn, yAn, xBn, yBn, xCn, yCn;
00144
00145
00146
00147
00148
00149 xAn = 0.;
00150 yAn = 0.;
00151 xBn = xB - xA;
00152 yBn = yB - yA;
00153 xCn = xC - xA;
00154 yCn = yC - yA;
00155
00156
00157
00158
00159
00160 beta = atan2(yBn, xBn);
00161
00162
00163
00164
00165
00166 gamma = atan2(yCn, xCn);
00167
00168
00169
00170
00171 alpha = beta - gamma;
00172
00173 alpha = -alpha;
00174
return (alpha);
00175 }
00176
00188
int
00189 IdCalcRotatePoint(
double x,
double y,
00190
double centreX,
double centreY,
00191
double angle,
00192
double *nouvX,
double *nouvY)
00193 {
00194
double sinAlph, cosAlph;
00195
00196
if (angle == 0.0) {
00197 *nouvX =
x;
00198 *nouvY = y;
00199
return (1);
00200 }
00201
00202 sinAlph = sin(angle);
00203 cosAlph = cos(angle);
00204
00205 *nouvX =
00206 cosAlph * (
x - centreX) - sinAlph * (y - centreY) + centreX;
00207 *nouvY =
00208 sinAlph * (
x - centreX) + cosAlph * (y - centreY) + centreY;
00209
00210
return (1);
00211 }
00212
00213
00226
int
00227 IdCalcZoomPoint(
double x,
double y,
00228
double centreX,
double centreY,
00229
double percent,
double *nouvX,
double *nouvY)
00230 {
00231
double angle;
00232
double oldDist, newDist;
00233
00234
if (percent == 0.0) {
00235 *nouvX =
x;
00236 *nouvY = y;
00237
return (1);
00238 }
00239
00240 angle = atan2((
double) (y - centreY), (
double) (
x - centreX));
00241
00242 oldDist =
IdCalcDistance2Points((
double) centreX, (
double) centreY,
00243 (
double)
x, (
double) y);
00244 newDist = oldDist * percent;
00245
00246 *nouvX = centreX + newDist * cos(angle);
00247 *nouvY = centreY + newDist * sin(angle);
00248
00249
return (1);
00250 }
00251
00252
static double sqrarg;
00253 #define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)
00254
00261
double
00262 IdCalcPythagore(
double a,
double b)
00263 {
00264
double absa, absb;
00265
00266 absa = fabs(a);
00267 absb = fabs(
b);
00268
if (absa > absb)
00269
return (absa * sqrt(1.0 +
SQR(absb / absa)));
00270
else
00271
return (absb ==
00272 0.0 ? 0.0 : absb * sqrt(1.0 +
SQR(absa / absb)));
00273 }
00274
00283
PVECTOR_COMPLEX
00284 IdCalcRacinesPoly2(
double a,
double b,
double c)
00285 {
00286
double d, signe, den;
00287
COMPLEX q;
00288
PVECTOR_COMPLEX racines;
00289
00290 racines = (
PVECTOR_COMPLEX)
IdVectAlloc(2,
VECT_COMPLEX);
00291
if (!racines) {
00292
IdErrPrintf(
"\n Erreur allocation solution du 2nd degre");
00293
IdExit(1);
00294 }
00295
00296
d = (
double) (
b *
b - 4.0 * a * c);
00297
if (
b >= 0.0)
00298 signe = 1.0;
00299
else
00300 signe = -1.0;
00301
00302
if (
d >= 0.0) {
00303 q.
re = -(
b + signe * sqrt(
d)) / 2;
00304 q.
im = 0.0;
00305 }
else {
00306 q.
re = -
b / 2;
00307 q.
im = -signe * sqrt(-
d) / 2;
00308 }
00309 den = q.
im * q.
im + q.
re * q.
re;
00310 racines[0].
re = q.
re / a;
00311 racines[0].
im = q.
im / a;
00312 racines[1].
re = c * q.
re / den;
00313 racines[1].
im = -c * q.
im / den;
00314
00315
return (racines);
00316 }
00317
00327
PVECTOR_COMPLEX
00328 IdCalcRacinesPoly3(
double a,
double b,
double c,
double d)
00329 {
00330
PVECTOR_COMPLEX racines;
00331
double q2, Q, R, A=0., B=0., C=0., S, T;
00332
double teta, teta3, A3, Q3, KPI = (
double) M_PI * 2 / 3, signe;
00333
00334
if (!a) {
00335
IdErrPrintf(
"\n Erreur degre du polynome");
00336
IdExit(1);
00337 }
else {
00338 A =
b / a;
00339 B = c / a;
00340 C =
d / a;
00341 }
00342
00343 racines = (
PVECTOR_COMPLEX)
IdSigAlloc(3,
VECT_COMPLEX);
00344
00345
if (!racines) {
00346
IdErrPrintf(
"\n Erreur allocation solution 3eme degre");
00347
IdExit(1);
00348 }
00349
00350 R = (A * A * A * 2 - A * B * 9 + C * 27) / 54;
00351 Q = (A * A - B * 3) / 9;
00352 Q3 = Q * Q * Q;
00353 A3 = A / 3;
00354
00355
if (R * R < Q3) {
00356 teta = acos(R / sqrt(Q3));
00357 teta3 = teta / 3;
00358 q2 = -sqrt(Q) * 2;
00359
00360 racines[0].
re = q2 * cos(teta3) - A3;
00361 racines[0].
im = (
double) 0.0;
00362
00363 racines[1].
re = q2 * cos(teta3 + KPI) - A3;
00364 racines[1].
im = (
double) 0.0;
00365
00366 racines[2].
re = q2 * cos(teta3 - KPI) - A3;
00367 racines[2].
im = (
double) 0.0;
00368
00369 }
else {
00370
if (R >= (
double) 0.0)
00371 signe = (
double) 1.0;
00372
else {
00373 signe = (
double) (-1.0);
00374 R = -R;
00375 }
00376 S =
00377 -signe * pow((R + sqrt(R * R - Q3)),
00378 (
double) (1.0 / 3.0));
00379
if (S != (
double) 0.0)
00380 T = Q / S;
00381
else
00382 T = (
double) 0.0;
00383
00384 racines[0].
re = S + T - A3;
00385 racines[0].
im = (
double) 0.0;
00386
00387 racines[1].
re = -(S + T) / 2 - A3;
00388 racines[1].
im = (S - T) * sqrt((
double) 3.0) / 2;
00389
00390 racines[2].
re = -(S + T) / 2 - A3;
00391 racines[2].
im = -(S - T) * sqrt((
double) 3.0) / 2;
00392 }
00393
00394
return (racines);
00395 }
00396
00407
int
00408 IdCalcNbElemIntEnum(
int *tabInterv,
int nbInterv)
00409 {
00410
int nbElem = 0;
00411
int i;
00412
for (i = 0; i < nbInterv * 2; i += 2)
00413 nbElem = nbElem + tabInterv[i + 1] - tabInterv[i] + 1;
00414
return (nbElem);
00415 }
00416
00422
int
00423 IdCalcArrondInt(
double x)
00424 {
00425
int sgn;
00426
double xabs = fabs(
x);
00427
00428 sgn = (
x < 0.0) ? -1 : 1;
00429
if ((xabs - floor(xabs)) >= 0.5)
00430
return (sgn * (
int) ceil(xabs));
00431
else
00432
return (sgn * (
int) floor(xabs));
00433
00434 }
00435
00442 float IdCalcFloatFromSun(
float *origine)
00443 {
00444
typedef struct {
00445
unsigned signe:1;
00446
unsigned caracteristique:7;
00447
unsigned mantisse:24;
00448 } FORME_SUN;
00449
00450 FORME_SUN number;
00451
float result;
00452
int i;
00453
00454
00455
00456
00457
for (i = 0; i < 4; i++)
00458 *((
char *) (&number) + i) = *((
char *) origine + i);
00459
00460
if (number.signe == 0 && number.caracteristique == 0
00461 && number.mantisse == 0)
00462
return (0.0);
00463 result = (
float) number.mantisse / (1 << 24) *
00464 pow(16.0, number.caracteristique - 64.0);
00465 return ((number.signe == 0) ? result : -result);
00466 }
00467
00474
float
00475 IdCalcFloatFromIEEE754(
float *origine)
00476 {
00477
float result;
00478
unsigned int mantisse, signe, caracteristique;
00479
00480 signe = (((
char *) origine)[0] & 0x80) >> 7;
00481
00482 mantisse = ((
char *) origine)[0] & 0x7f;
00483 mantisse = mantisse << 1;
00484
00485 caracteristique = ((
char *) origine)[1] & 0x80;
00486
if (caracteristique != 0)
00487 mantisse++;
00488 caracteristique = (((
char *) origine)[1] & 0x7f) << 17;
00489 caracteristique +=
00490 (((
char *) origine)[2] << 9) + ((
char *) origine)[3];
00491
00492
if (signe == 0 && caracteristique == 0 && mantisse == 0)
00493
return (0.0);
00494
00495 result = ((
float) caracteristique / (1 << 24) + 1.)
00496 * pow(2.0, mantisse - 127.0);
00497
00498
return ((signe == 0) ? result : -result);
00499 }
00500
00501
00502
00503 #define IA 16807
00504 #define IM 2147483647
00505 #define AM (1.0/IM)
00506 #define IQ 127773
00507 #define IR 2836
00508 #define NTAB 32
00509 #define NDIV (1+(IM-1)/NTAB)
00510 #define EPS 1.2e-7
00511 #define RNMX (1.0-EPS)
00512
00519
float
00520 IdCalcRand1(
long *idum)
00521 {
00522
int j;
00523
long k;
00524
static long iy = 0;
00525
static long iv[
NTAB];
00526
float temp;
00527
00528
if (*idum <= 0 || !iy) {
00529
if (-(*idum) < 1)
00530 *idum = 1;
00531
else
00532 *idum = -(*idum);
00533
for (j =
NTAB + 7; j >= 0; j--) {
00534 k = (*idum) /
IQ;
00535 *idum =
IA * (*idum - k *
IQ) -
IR * k;
00536
if (*idum < 0)
00537 *idum +=
IM;
00538
if (j <
NTAB)
00539 iv[j] = *idum;
00540 }
00541 iy = iv[0];
00542 }
00543 k = (*idum) /
IQ;
00544 *idum =
IA * (*idum - k *
IQ) -
IR * k;
00545
if (*idum < 0)
00546 *idum +=
IM;
00547 j = iy /
NDIV;
00548 iy = iv[j];
00549 iv[j] = *idum;
00550
if ((temp =
AM * iy) >
RNMX)
00551
return RNMX;
00552
else
00553
return temp;
00554 }
00555
00556
#undef IA
00557
#undef IM
00558
#undef AM
00559
#undef IQ
00560
#undef IR
00561
#undef NTAB
00562
#undef NDIV
00563
#undef EPS
00564
#undef RNMX
00565
00566
00575
short int
00576 IdCalcSwapInt16(
short int *a,
int sw)
00577 {
00578
short int b=0;
00579
00580
if (!sw)
00581
return (*a);
00582
00583
if (sw == 4321)
00584
b = (((*a << 8) & 0x0ff00) | ((*a >> 8) & 0x00ff));
00585
return (
b);
00586 }
00587
00596
int
00597 IdCalcSwapInt32(
int * a,
int sw)
00598 {
00599
int b = 0;
00600
00601
00602
00603
if (!sw)
00604
return (*a);
00605
00606
if (sw == 4321)
00607
b = (((*a << 24) & 0xff000000) | ((*a << 8) & 0x00ff0000) |
00608 ((*a >> 8) & 0x0000ff00) | ((*a >> 24) & 0x000000ff));
00609
if (sw == 3412)
00610
b =
00611 (((*a << 16) & 0xffff0000) |
00612 ((*a >> 16) & 0x0000ffff));
00613
00614
if (sw == 2143)
00615
b = (((*a << 8) & 0xff00ff00) | ((*a >> 8) & 0x00ff00ff));
00616
00617
return (
b);
00618 }
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00645
double
00646 IdGetDistanceFromPlane(
int x,
int y,
int z,
int px,
int py,
int pz)
00647 {
00648
double rval;
00649
00650 rval =
00651 fabs(((
double)
x * px) + ((
double) y * py) +
00652 ((
double) z * pz));
00653
return rval / sqrt(
sqr((
double) px) +
sqr((
double) py) +
00654
sqr((
double) pz));
00655 }