map.c 1.3 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162
  1. /* $Id: map.c,v 1.6 2001/05/06 01:23:05 kilobug Exp $ */
  2. #include <math.h>
  3. /*
  4. ** General map functions
  5. */
  6. float dist(float x1, float y1, float x2, float y2)
  7. {
  8. return (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
  9. }
  10. /* float a, b, c, res;
  11. a = sqrt(dist(x, y, x1, y1));
  12. b = sqrt(dist(x, y, x2, y2));
  13. c = fabs((x1 - x) * (x2 - x) + (y1 - y) * (y2 - y)) * b;
  14. if (a)
  15. res = acos(CLAMP(c / a, -1, 1));
  16. else
  17. res = M_PI;
  18. return res;
  19. float ac2, ab2, bc2, ac, ab, cosin;
  20. bc2 = SQR(x1 - x2) + SQR(y1 - y2);
  21. ab2 = SQR(x - x1) + SQR(y - y1);
  22. ac2 = SQR(x - x2) + SQR(y - y2);
  23. ac = (float)sqrt(ac2);
  24. ab = (float)sqrt(ab2);
  25. printf("BC2:%f AB2:%f AC2:%f AC:%f AB:%f\n", bc2, ab2, ac2, ac, ab);
  26. if (!ac | !ab)
  27. return 0;
  28. cosin = (bc2 - ab2 - ac2) / (2 * ac * ab);
  29. printf("NUM: %f DEN: %f\n", bc2 - ab2 - ac2, 2 * ac * ab);
  30. printf("COS:%f\n", cosin);
  31. if (ABS(cosin) <= 1)
  32. return (float)acos(cosin);
  33. return 0;
  34. */
  35. float angle(float x, float y, float x1, float y1, float x2, float y2)
  36. { // a // b // c
  37. float f, d1, d2;
  38. d1 = sqrtf(dist(x1, y1, x, y));
  39. d2 = sqrtf(dist(x1, y1, x2, y2));
  40. if ((d1 == 0) || (d2 == 0))
  41. {
  42. return 0;
  43. }
  44. f = ((x1 - x) * (x1 - x2) + (y1 - y) * (y1 - y2)) / (d1 * d2);
  45. return acosf(f);
  46. }