Advertisement
runewalsh

Тупой колижн

Jul 11th, 2012
1,144
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. {$mode objfpc}
  2.  
  3. uses
  4.   rwSystem, rwMath;
  5.  
  6. // плоскость по точке и нормали
  7. // нормализована, если normal.length = 1
  8. function PlanePN(const point: tVec3; const normal: tVec3): tPlane;
  9. begin
  10.   // нормаль (A, B, C)
  11.   // точка (x0, y0, z0)
  12.   // тогда A(x - x0) + B(y - y0) + C(z - z0) = 0
  13.   result.a := normal.x;
  14.   result.b := normal.y;
  15.   result.c := normal.z;
  16.   result.d := -(result.a * point.x + result.b * point.y + result.c * point.z);
  17. end;
  18.  
  19. // площадь треугольника по 3 точкам
  20. function TriSq(const pt1, pt2, pt3: tVec3): float;
  21. begin
  22.   result := 0.5 * ((pt2 - pt1) >< (pt3 - pt1)).Length;
  23. end;
  24.  
  25. // проецирует точку p на линию, заданную точкой и _направляющим_ вектором
  26. function ProjOnLine(const p: tVec3; const lineP, lineN: tVec3): tVec3;
  27. var
  28.   k: float;
  29. begin
  30.   k := Dot(lineN, p - lineP);
  31.   result := k * lineN + lineP;
  32. end;
  33.  
  34. // pl должна быть нормализована, или будет верен знак, но не расстояние!!
  35. function SignedDistance(const pl: tPlane; const pt: tVec3): float;
  36. begin
  37.   result := pl.a * pt.x + pl.b * pt.y + pl.c * pt.z + pl.d;
  38. end;
  39.  
  40. // проецирует точку на плоскость
  41. function ProjectPointOnPlane(const p: tVec3; const plane: tPlane): tVec3;
  42. var
  43.   k: float;
  44. begin
  45.   k := SignedDistance(plane, p);
  46.   result := p - plane.v3 * k;
  47. end;
  48.  
  49. // to-do: optimize
  50. function CollideSphereToTriangle(
  51.   const center: tVec3; radius: float;
  52.   const p1, p2, p3: tVec3; out ofs: tVec3): boolean;
  53. var
  54.   plane: tPlane; // плоскость треугольника
  55.   square: float; // площадь треугольника
  56.   v1, v2, v3: tVec3; // единичные векторы сторон треугольника
  57.   normal: tVec3; // нормаль к треугольнику
  58.   len1, len2, len3: float; // длины соотв. сторон
  59.  
  60.   dist: float; // какое-нибудь расстояние
  61.   proj: tVec3; // проекция center на плоскость треугольника
  62. begin
  63.   // подготовка
  64.   // это стоит посчитать лишь однажды, при инициализации треугольника
  65.   v1 := p2 - p1;
  66.   v2 := p3 - p2;
  67.   v3 := p1 - p3;
  68.   normal := v1 >< v2;
  69.   square := 0.5 * normal.Length; // normal - векторное произведение же!
  70.   len1 := v1.Length;
  71.   len2 := v2.Length;
  72.   len3 := v3.Length;
  73.   v1.Normalize;
  74.   v2.Normalize;
  75.   v3.Normalize;
  76.   normal.Normalize;
  77.   plane := PlanePN(p1, normal);
  78.  
  79.   // сфера слишком далеко от плоскости треугольника?
  80.   dist := SignedDistance(plane, center);
  81.   if abs(dist) > radius then exit(false);
  82.  
  83.   // центр сферы проецируется внутрь треугольника?
  84.   // если да - она может сталкиваться только с плоскостью
  85.   proj := ProjectPointOnPlane(center, plane);
  86.   if Equals(square, TriSq(p1, p2, proj) + TriSq(p2, p3, proj) + TriSq(p3, p1, proj)) then
  87.   begin
  88.     //if dist > 0 then
  89.     ofs := normal * (radius - dist){
  90.     else
  91.     ofs := -normal * (radius + dist)};
  92.     exit(true);
  93.   end;
  94.  
  95.   // проецируем на каждую сторону. Проецируется - значит, может сталкиваться
  96.   // с ней.
  97.   proj := ProjOnLine(center, p1, v1);
  98.   dist := Distance(center, proj);
  99.   if (dist < radius) and (Equals(len1, Distance(p1, proj) + Distance(proj, p2))) then
  100.   begin
  101.     ofs := (center - proj) * (1.0 - dist / radius);
  102.     exit(true);
  103.   end;
  104.  
  105.   proj := ProjOnLine(center, p2, v2);
  106.   dist := Distance(center, proj);
  107.   if (dist < radius) and (Equals(len2, Distance(p2, proj) + Distance(proj, p3))) then
  108.   begin
  109.     ofs := (center - proj) * (1.0 - dist / radius);
  110.     exit(true);
  111.   end;
  112.  
  113.   proj := ProjOnLine(center, p3, v3);
  114.   dist := SqrDistance(center, proj);
  115.   if (dist < radius) and (Equals(len3, Distance(p3, proj) + Distance(proj, p1))) then
  116.   begin
  117.     ofs := (center - proj) * (1.0 - dist / radius);
  118.     exit(true);
  119.   end;
  120.  
  121.   // наконец, сталкиваем с вершинами
  122.   // это всё можно опустить, но тогда можно будет, если ОЧЕНЬ постараться,
  123.   // проходить сквозь подобные стыки ОЧЕНЬ БОЛЬШИХ полигонов.
  124.   // Кажется, я обычно опускал. :3
  125.   dist := Distance(center, p1);
  126.   if Dist < radius then
  127.   begin
  128.     ofs := (center - p1) * (1.0 - dist / radius);
  129.     exit(true);
  130.   end;
  131.   dist := Distance(center, p2);
  132.   if dist < radius then
  133.   begin
  134.     ofs := (center - p2) * (1.0 - dist / radius);
  135.     exit(true);
  136.   end;
  137.   dist := Distance(center, p3);
  138.   if dist < radius then
  139.   begin
  140.     ofs := (center - p3) * (1.0 - dist / radius);
  141.     exit(true);
  142.   end;
  143.  
  144.   // ничего не сталкивается лол
  145.   result := false;
  146. end;
  147.  
  148. begin
  149. end.
Advertisement
RAW Paste Data Copied
Advertisement