fork download
copy 
  1. // Вспомогательный модуль определеняи пересечения некоторых фигур в пространстве
  2.  
  3. #ifndef COLLIDING_H
  4. #define COLLIDING_H
  5.  
  6. #include "plane3d.h"
  7.  
  8. template < class T >
  9. Vertex3D< T > AnglesToDirection( const Vertex3D< T > &ang )
  10. {
  11.   return Vertex3D< T >( (T) (sin( ang.y ) * cosf( ang.x )), 
  12.                         (T) sinf( -ang.x ), 
  13.                         (T) (-cosf( ang.x ) * cosf( ang.y )) );
  14. };
  15.  
  16. template < class T >
  17. Vertex3D< T > DirectionToAngles( const Vertex3D< T > &dir )
  18. {
  19.   Vertex3D< T > ret;
  20.   if ( dir.x > 0 )
  21.     ret.y = (T) (atan( dir.z / dir.x ) + M_PI / 2);
  22.   else if ( dir.x < 0 )
  23.     ret.y = (T) (atan( dir.z / dir.x ) + 3 * M_PI / 2);
  24.   else
  25.     ret.y = dir.z < 0 ? 0 : (T) M_PI;
  26.   T z = (T) sqrt( dir.x * dir.x + dir.z * dir.z );
  27.   if ( dir.y > 0 )
  28.     ret.x = (T) (atan( z / dir.y ) - M_PI / 2);
  29.   else if ( dir.y < 0 )
  30.     ret.x = (T) (atan( z / dir.y ) - 3 * M_PI / 2);
  31.   else
  32.     ret.x = z < 0 ? 0 : (T) M_PI;
  33.   return ret;
  34. };
  35.  
  36. // TrisContainsPoint: определяет содержит ли треугольник с вершинами v1, v2, v3
  37. // с нормалью n точку p в своих границах (точка должна лежать в плоскости треугольника).
  38. template < class T >
  39. bool TrisContainsPoint( const Vertex3D< T > &v1, 
  40.                         const Vertex3D< T > &v2, 
  41.                         const Vertex3D< T > &v3,
  42.                         const Vertex3D< T > &n,
  43.                         const Vertex3D< T > &p )
  44. {
  45.   if( n.MixedProduct( v2 - v1, p - v1 ) <= 0 ) 
  46.     return false; 
  47.   if( n.MixedProduct( v3 - v2, p - v2 ) <= 0 ) 
  48.     return false; 
  49.   if( n.MixedProduct( v1 - v3, p - v3 ) <= 0 ) 
  50.     return false; 
  51.   return true;
  52. };
  53.  
  54. // ClosestPointToSegment: возвращает ближайшую к точке p точку, лежащую
  55. // на отрезке a-b, не выходящую за концы отрезка.
  56. template < class T >
  57. Vertex3D< T > ClosestPointToSegment( const Vertex3D< T > &p, const Vertex3D< T > &a, const Vertex3D< T > &b )
  58. {
  59.   Vertex3D< T > c = p - a;
  60.   Vertex3D< T > v = b - a;
  61.   T d = v.Length();
  62.   if ( fabs( d ) < Plane3D< T >::EPS )
  63.     return a;
  64.   v /= d;
  65.   T t = v & c;
  66.   if ( t < 0.0f )
  67.     return a;
  68.   else if ( t > d )
  69.     return b;
  70.   return a + t * v;
  71. };
  72.  
  73. // ClosestPointToTris: находит ближайшую к точке p точку, лежащую
  74. // на сторонах треугольника с вершинами v1, v2, v3. Возвращает
  75. // минимальное расстояние между найденной точкой и границей треугольника.
  76. // Найденную точку записывает в результат res.
  77. template < class T >
  78. T ClosestPointToTris( const Vertex3D< T > &v1, const Vertex3D< T > &v2, 
  79.                       const Vertex3D< T > &v3, const Vertex3D< T > &p, 
  80.                       Vertex3D< T > *res )
  81. {
  82.   *res = ClosestPointToSegment( p, v1, v2 );
  83.   T bestDist = p.DistanceToSqr( *res );
  84.  
  85.   Vertex3D< T > pt = ClosestPointToSegment( p, v1, v3 );
  86.   T dist = p.DistanceToSqr( pt );
  87.   if ( dist < bestDist )
  88.   {
  89.     bestDist = dist;
  90.     *res = pt;
  91.   };
  92.   pt = ClosestPointToSegment( p, v2, v3 );
  93.   dist = p.DistanceToSqr( pt );
  94.   if ( dist < bestDist )
  95.   {
  96.     bestDist = dist;
  97.     *res = pt;
  98.   };
  99.  
  100.   return (T) sqrt( bestDist );
  101. }
  102.  
  103. // IntersectTrisByRay: возвращает true, если луч, испущенный из точки
  104. // org в направлении dir пересекает треугольник в его границах, 
  105. // образованного вершинами v1-v2-v3, или false в противном случае. 
  106. // Если возвращает true, то в параметр resDist сохраняется расстояние
  107. // до точки пересечения от точки org в единицах, равных длине вектора dir
  108. // (что соответствует времени которое пройдет до пересечения точки org с 
  109. // треугольником, если точка двигается со скоростью dir), а в
  110. // параметр resPoint сохраняются координаты точки пересечения.
  111. template < class T >
  112. bool IntersectTrisByRay( const Vertex3D< T > &v1,
  113.                          const Vertex3D< T > &v2,
  114.                          const Vertex3D< T > &v3,
  115.                          const Vertex3D< T > &org,
  116.                          const Vertex3D< T > &dir,
  117.                          Vertex3D< T > *resPoint, T *resDist )
  118. {
  119.   Plane3D< T > plane( v1, v2, v3 );
  120.   if ( !plane.IntersectByRay( org, dir, resDist ) )
  121.     return false;
  122.   *resPoint = org + *resDist * dir;
  123.   // проверяем, находится ли точка пересечения внутри треугольника.
  124.   return TrisContainsPoint( v1, v2, v3, plane.n, *resPoint ); 
  125. };
  126.  
  127. // IntersectSphereByRay: возвращает true, если луч испущенный  из
  128. // точки ord в направлении dir пересекает сферу радиусов radius,
  129. // центр которой находится в точке center. 
  130. // Если возвращает true, то в параметр time сохраняется расстояние от 
  131. // точки org до точки пересечения в единицах, равных длине вектора dir
  132. // (что соответствует времени которое пройдет до пересечения точки org со
  133. // сферой, если точка двигается со скоростью dir).
  134. template < class T >
  135. bool IntersectSphereByRay( const Vertex3D< T > &center, T radius, 
  136.                            const Vertex3D< T > &org, const Vertex3D< T > &dir,
  137.                            T *time )
  138. {
  139.   Vertex3D< T > q = center - org;
  140.   T c = q & q;             // squared length
  141.   Vertex3D< T > nDir = dir;
  142.   T l = dir.Length();
  143.   if ( fabs( l ) < Plane3D< T >::EPS )
  144.     return false;
  145.   nDir /= l;
  146.   T v = q & nDir;
  147.   T d = radius * radius - (c - v * v);
  148.   if ( d < 0.0f )
  149.     return false;
  150.   *time = (T)(v - sqrt( d )) / l;
  151.   return true;
  152. };
  153.  
  154. // IntersectSphereBySphere: возвращает true, если окружности с радиусами
  155. // r1 и r2 с центрами в точках c1 и с2, движущиеся со скоростями v1 и v2
  156. // соприкоснутся в результате своего движения. 
  157. // Если возвращает true, то в параметр time сохраняется время которое должно
  158. // пройти до момента соприкосновения, а в параметр normal - нормаль соприкоснования, т.е.
  159. // единичный вектор, лежащий на отрезке, соединяющей центры окружностей в момент
  160. // соприкосновения, направленный в сторону центра первой окружности c1.
  161. template < class T >
  162. bool IntersectSphereBySphere( const Vertex3D< T > &c1, T r1, const Vertex3D< T > &v1,
  163.                               const Vertex3D< T > &c2, T r2, const Vertex3D< T > &v2,
  164.                               T *time, Vertex3D< T > *normal )
  165. {
  166.   Vertex3D< T > nPos = c1 - c2;
  167.   Vertex3D< T > nSpeed = v1 - v2;
  168.   T nSpeedLen = nSpeed.Length();
  169.   if ( nSpeedLen <= 0 )
  170.     return false;
  171.   Vertex3D< T > nSpeedDir = nSpeed / nSpeedLen;
  172.   T dist = (-nPos) & nSpeedDir;
  173.   if ( dist <= 0 )
  174.     return false;
  175.   T nPosLen = nPos.Length();
  176.   T r = r1 + r2;
  177.   T x = nPosLen * nPosLen - dist * dist;
  178.   if ( x > r * r )
  179.     return false;
  180.   T z = dist - sqrtf( r * r - x );
  181.   *time = z / nSpeedLen;
  182.   *normal = +nPos + nSpeedDir * z;
  183.   normal->Normalize();
  184.   return true;
  185. };
  186.  
  187. // IntersectTrisBySphere: возвращает true, если окружность с центром в точке center,
  188. // радиусом radius, движущаяся со скоростью speed пересечет границы
  189. // треугольника, образованного вершинами v1, v2, v3. 
  190. // Если вовзращает true, то в параметр time сохраняется время до момента
  191. // соприкоснования, а в параметр normal - нормаль соприкоснования, т.е.
  192. // единичный вектор, лежащий на отрезке, соединяющем центр окружности с точкой
  193. // соприкоснования (в момент соприкосноваения), направленный в сторону центра окружности.
  194. template < class T >
  195. bool IntersectTrisBySphere( const Vertex3D< T > &v1, const Vertex3D< T > &v2, const Vertex3D< T > &v3,
  196.                             const Vertex3D< T > &center, T radius, const Vertex3D< T > &speed,
  197.                             T *time, Vertex3D< T > *normal )
  198. {
  199.   Plane3D< T > plane( v1, v2, v3 );
  200.   Vertex3D< T > pt = center - plane.n * radius;
  201.   if ( !plane.IntersectByRay( pt, speed, time ) )
  202.     return false;
  203.   Vertex3D< T > planePt = pt + (*time) * speed;
  204.   *normal = plane.n;
  205.   // проверяем, находится ли точка пересечения внутри треугольника. 
  206.   if ( !TrisContainsPoint( v1, v2, v3, plane.n, planePt ) )
  207.   {
  208.     // находим ближайшую точку на треугольнике
  209.     Vertex3D< T > trisPt;
  210.     ClosestPointToTris( v1, v2, v3, planePt, &trisPt );
  211.     if ( !IntersectSphereByRay( center, radius, trisPt, -speed, time ) )
  212.       return false;
  213.     *normal = center - (trisPt - (*time) * speed);
  214.     normal->Normalize();
  215.   };
  216.   return true;
  217. };
  218.  
  219. #endif
Compilation error #stdin compilation error #stdout 0s 0KB
comments (?)
stdin
copy 
Standard input is empty
compilation info 
prog.cpp:6:10: fatal error: plane3d.h: No such file or directory
 #include "plane3d.h"
          ^~~~~~~~~~~
compilation terminated.
stdout
copy 
Standard output is empty
https://ideone.com/s6rXuc
language:
C++14 (gcc 8.3)
created:
3 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!