PageRenderTime 1161ms CodeModel.GetById 18ms RepoModel.GetById 1ms app.codeStats 1ms

/src/CollisionResponse.as

https://github.com/Lindi/Collision-Detection
ActionScript | 568 lines | 369 code | 91 blank | 108 comment | 47 complexity | e244e8fe7d88e85357f180620aee20f2 MD5 | raw file
  1. package
  2. {
  3. //import SeparatingAxes ;
  4. import flash.display.Sprite;
  5. import flash.events.Event;
  6. import flash.events.TimerEvent;
  7. import flash.utils.Timer;
  8. import flash.utils.getTimer;
  9. import geometry.AABB;
  10. import geometry.Polygon2d;
  11. import geometry.Vector2d;
  12. import physics.PolygonDistance;
  13. import physics.PolygonIntersection;
  14. import physics.lcp.LcpSolver;
  15. [SWF(width='400',height='400')]
  16. public class CollisionResponse extends Sprite
  17. {
  18. private var polygons:Vector.<Polygon2d> = new Vector.<Polygon2d>(2,true);
  19. private var aabbs:Vector.<AABB> = new Vector.<AABB>(2,true);
  20. private var velocity:Vector.<Vector2d> = new Vector.<Vector2d>(2,true);
  21. private var t:Number ;
  22. private var timer:Timer ;
  23. public function CollisionResponse()
  24. {
  25. super();
  26. init( );
  27. }
  28. private function init( ):void
  29. {
  30. // Velocity components
  31. var dx:Number, dy:Number ;
  32. // Create a polygon
  33. var centroid:Vector2d = new Vector2d( stage.stageWidth/2, stage.stageHeight/2 ) ;
  34. polygons[0] = createPolygon( centroid );
  35. dx = 80 + int( Math.random() * 10 ) * ( 1 - 2 * int( Math.random() * 2 ));
  36. dy = 80 + int( Math.random() * 10 ) * ( 1 - 2 * int( Math.random() * 2 ));
  37. velocity[0] = new Vector2d( dx, dy );
  38. // Create another polygon, and make sure they don't intersect
  39. centroid = new Vector2d( centroid.x + 100, centroid.y + 100 ) ;
  40. polygons[1] = createPolygon( centroid );
  41. do
  42. {
  43. dx = 80 + int( Math.random() * 10 ) * ( 1 - 2 * int( Math.random() * 2 ));
  44. } while ( dx == velocity[0].x )
  45. do
  46. {
  47. dy = 80 + int( Math.random() * 10 ) * ( 1 - 2 * int( Math.random() * 2 ));
  48. } while ( dy == velocity[0].y )
  49. velocity[1] = new Vector2d( dx, dy );
  50. // Mark the time
  51. t = getTimer() ;
  52. // Enter frame draws everything
  53. addEventListener( Event.ENTER_FRAME, frame );
  54. //draw( 0x000000 ) ;
  55. }
  56. /**
  57. * Frame event handler
  58. * @param event
  59. *
  60. */
  61. private function frame( event:Event ):void
  62. {
  63. // Get the time since the last frame
  64. var t:Number = getTimer();
  65. var dt:Number = ( t - this.t )/1000 ;
  66. this.t = t ;
  67. // Get the axis-aligned bounding boxes
  68. for ( var i:int = 0; i < polygons.length; i++ )
  69. {
  70. // Grab a polygon
  71. var polygon:Polygon2d = polygons[i];
  72. // Get the AABB
  73. var aabb:AABB = aabbs[i] = getAABB( polygon ) ;
  74. }
  75. // Do collision detection with stage edges
  76. for ( i = 0; i < polygons.length; i++ )
  77. {
  78. // Grab a polygon
  79. polygon = polygons[i];
  80. polygon.moved = false ;
  81. // Get the AABB
  82. aabb = aabbs[i] ;
  83. var offstage:Boolean = ( aabb.xmin <= 0 )
  84. || ( aabb.xmax >= stage.stageWidth )
  85. || ( aabb.ymin <= 0 )
  86. || ( aabb.ymax >= stage.stageHeight );
  87. // Get the centroid
  88. var centroid:Vector2d = polygon.centroid.clone() ;
  89. if ( aabb.xmin <= 0 )
  90. {
  91. if ( velocity[i].dot( new Vector2d( 1, 0 )) < 0 )
  92. {
  93. velocity[i].x = -velocity[i].x ;
  94. centroid.x -= aabb.xmin ;
  95. polygon.moved = true ;
  96. }
  97. }
  98. if ( aabb.xmax >= stage.stageWidth )
  99. {
  100. if ( velocity[i].dot( new Vector2d( -1, 0 )) < 0 )
  101. {
  102. velocity[i].x = -velocity[i].x ;
  103. centroid.x -= ( aabb.xmax - stage.stageWidth ) ;
  104. polygon.moved = true ;
  105. }
  106. }
  107. if ( aabb.ymin <= 0 )
  108. {
  109. if ( velocity[i].dot( new Vector2d( 0, 1 )) < 0 )
  110. {
  111. velocity[i].y = -velocity[i].y ;
  112. centroid.y -= aabb.ymin ;
  113. polygon.moved = true ;
  114. }
  115. }
  116. if ( aabb.ymax >= stage.stageHeight )
  117. {
  118. if ( velocity[i].dot( new Vector2d( 0, -1 )) < 0 )
  119. {
  120. velocity[i].y = -velocity[i].y ;
  121. centroid.x -= ( aabb.ymax - stage.stageHeight ) ;
  122. polygon.moved = true ;
  123. }
  124. }
  125. updateCentroid( polygon, centroid );
  126. }
  127. // Get the axis-aligned bounding boxes
  128. for ( i = 0; i < polygons.length; i++ )
  129. {
  130. // Grab a polygon
  131. polygon = polygons[i];
  132. // Get the AABB
  133. aabb = aabbs[i] = getAABB( polygon ) ;
  134. }
  135. // Do their AABBs intersect?
  136. var aabbsIntersect:Boolean = true ;
  137. aabbsIntersect &&= !( aabbs[0].xmin > aabbs[1].xmax ) ;
  138. aabbsIntersect &&= !( aabbs[1].xmin > aabbs[0].xmax ) ;
  139. aabbsIntersect &&= !( aabbs[0].ymin > aabbs[1].ymax ) ;
  140. aabbsIntersect &&= !( aabbs[1].ymin > aabbs[0].ymax ) ;
  141. var color:Number = 0x000000 ;
  142. // Do the collision response here ...
  143. if ( aabbsIntersect )
  144. {
  145. var polygonsIntersect:Boolean = SeparatingAxes.testIntersection( polygons[0], polygons[1] );
  146. var distance:Number, minDistance:Number ;
  147. if ( polygonsIntersect )
  148. {
  149. trace( "polygonsIntersect");
  150. // Separate the AABBs
  151. var intersection:AABB = new AABB();
  152. aabbs[0].findIntersection( aabbs[1], intersection );
  153. var w:Number = intersection.width ;
  154. var h:Number = intersection.height ;
  155. var d:Number = Math.max( w, h );//Math.sqrt( w * w + h * h ) ;
  156. // Normalize the vector between the centroids and
  157. // move the polygons by this distance
  158. var v:Vector2d = polygons[1].centroid.Subtract( polygons[0].centroid );
  159. v.normalize();
  160. // Move them both half the distance
  161. // Move polygon 0
  162. centroid = polygons[0].centroid.Add( v.Negate().ScaleBy( d/2 ));
  163. updateCentroid( polygons[0], centroid ) ;
  164. polygons[0].moved = true ;
  165. // Move polygon 1
  166. centroid = polygons[1].centroid.Add( v.ScaleBy( d/2 ));
  167. updateCentroid( polygons[1], centroid ) ;
  168. polygons[1].moved = true ;
  169. }
  170. polygonsIntersect = SeparatingAxes.testIntersection( polygons[0], polygons[1] );
  171. if ( polygonsIntersect )
  172. {
  173. throw new Error("Polygons should not be intersecting after we've moved their AABBs apart.");
  174. }
  175. var solution:Object = PolygonDistance.distance( polygons[0], polygons[1] );
  176. if ( solution.status == LcpSolver.FOUND_SOLUTION )
  177. {
  178. var a:Vector2d = new Vector2d( solution.Z[0], solution.Z[1] );
  179. var b:Vector2d = new Vector2d( solution.Z[2], solution.Z[3] );
  180. distance = b.Subtract( a ).length ;
  181. minDistance = .4 ;
  182. if ( distance < minDistance )
  183. {
  184. {
  185. // Some variables
  186. var normal:Vector2d, relativePosition:Vector2d,
  187. relativeVelocity:Vector2d, perp:Vector2d, newVelocity:Vector2d ;
  188. // Check to see if the closest point a is a vertex point or an edge point
  189. var aIsVertex:Boolean = isVertex( polygons[0], a );
  190. var bIsVertex:Boolean = isVertex( polygons[1], b );
  191. // Check to see if the closest point b is a vertex point or an edge point
  192. if ( bIsVertex && aIsVertex )
  193. {
  194. } else if ( bIsVertex )
  195. {
  196. // Move b towards a
  197. relativePosition = polygons[0].centroid.Subtract( polygons[1].centroid );
  198. relativePosition.normalize();
  199. centroid = polygons[1].centroid.clone();
  200. centroid.Add( relativePosition.ScaleBy( distance ));
  201. updateCentroid( polygons[1], centroid ) ;
  202. // Collide
  203. collide( 0, 1, [a] ) ;
  204. } else if ( aIsVertex )
  205. {
  206. // Get the normal from b, and move a to b
  207. // Move b half the distance
  208. relativePosition = polygons[1].centroid.Subtract( polygons[0].centroid );
  209. relativePosition.normalize();
  210. centroid = polygons[0].centroid.clone();
  211. centroid.Add( relativePosition.ScaleBy( distance ));
  212. updateCentroid( polygons[0], centroid ) ;
  213. // Collide
  214. collide( 1, 0, [b] ) ;
  215. } else
  216. {
  217. relativePosition = polygons[1].centroid.Subtract( polygons[0].centroid );
  218. relativePosition.normalize();
  219. // Move b half the distance
  220. centroid = polygons[0].centroid.clone();
  221. centroid.Add( relativePosition.Negate().ScaleBy( distance/2 ));
  222. updateCentroid( polygons[0], centroid ) ;
  223. // Move a half the distance
  224. centroid = polygons[1].centroid.clone();
  225. centroid.Add( relativePosition.ScaleBy( distance/2 ));
  226. updateCentroid( polygons[1], centroid ) ;
  227. // Collide
  228. collide( 0, 1, [a,b] ) ;
  229. }
  230. }
  231. }
  232. }
  233. }
  234. // Move and rotate polygons
  235. for ( i = 0; i < polygons.length; i++)
  236. {
  237. //
  238. polygon = polygons[i] ;
  239. centroid = polygon.centroid.clone() ;
  240. // Move the polygons
  241. centroid.x += ( velocity[i].x * dt ) ;
  242. centroid.y += ( velocity[i].y * dt ) ;
  243. // trace( "velocity["+i+"]", velocity[i] );
  244. // trace( "centroid["+i+"]", centroid );
  245. updateCentroid( polygon, centroid ) ;
  246. }
  247. draw( 0x000000 );
  248. }
  249. /**
  250. * Handle the collision response
  251. * @param a
  252. * @param b
  253. * @param closestPoint
  254. *
  255. */
  256. private function collide( a:int, b:int, closestPoints:Array ):void
  257. {
  258. return ;
  259. // Some variables
  260. var normal:Vector2d, relativePosition:Vector2d,
  261. relativeVelocity:Vector2d, perp:Vector2d, newVelocity:Vector2d ;
  262. // Variable v
  263. var v:Vector2d ;
  264. // Get the relative velocity
  265. relativeVelocity = velocity[b].Subtract( velocity[a] );
  266. // Get the normal
  267. normal = getNormal( polygons[a], closestPoints[0] as Vector2d );
  268. // If the normal dot the relative velocity is greater than zero
  269. // the polygons are separating, so don't do anything
  270. if ( normal.dot( relativeVelocity ) > 0 )
  271. {
  272. return ;
  273. }
  274. // We know we're colliding, so calculate the change in velocity
  275. // and update accordingly
  276. perp = normal.perp();
  277. if ( relativeVelocity.dot( perp ) < 0 )
  278. {
  279. perp.negate();
  280. }
  281. newVelocity = perp.Subtract( normal.ScaleBy(normal.dot( relativeVelocity )));
  282. v = newVelocity.clone() ;
  283. // We know we're colliding, so calculate the change in velocity
  284. // and update accordingly
  285. relativeVelocity = velocity[a].Subtract( velocity[b] );
  286. if ( closestPoints.length > 1 )
  287. {
  288. // Get the normal
  289. normal = getNormal( polygons[b], closestPoints[1] as Vector2d );
  290. // We know we're colliding, so calculate the change in velocity
  291. // and update accordingly
  292. perp = normal.perp();
  293. if ( relativeVelocity.dot( perp ) < 0 )
  294. {
  295. perp.negate();
  296. }
  297. } else
  298. {
  299. normal.negate();
  300. perp.negate();
  301. }
  302. if ( relativeVelocity.dot( perp ) < 0 )
  303. {
  304. perp.negate();
  305. }
  306. newVelocity = perp.Subtract( normal.ScaleBy(normal.dot( relativeVelocity )));
  307. velocity[a].x = newVelocity.x ;
  308. velocity[a].y = newVelocity.y ;
  309. velocity[b].x = v.x ;
  310. velocity[b].y = v.y ;
  311. }
  312. private function draw( color:Number ):void
  313. {
  314. // Draw each polygon
  315. //var color:Number = 0 ;
  316. graphics.clear();
  317. for ( var i:int = 0; i < polygons.length; i++ )
  318. {
  319. var polygon:Polygon2d = polygons[i];
  320. // Draw the polygon
  321. graphics.lineStyle( 3, color );
  322. for ( var j:int = 0; j < polygon.vertices.length; j++ )
  323. {
  324. var x:Number = polygon.vertices[j].x ;
  325. var y:Number = polygon.vertices[j].y ;
  326. var vertex:Vector2d = polygon.getVertex( j-1);
  327. graphics.moveTo( vertex.x, vertex.y);
  328. graphics.lineTo( x, y ) ;
  329. }
  330. // // Draw the aabb
  331. // var aabb:AABB = aabbs[i] ;
  332. // graphics.lineStyle( 1, 0xff0000 );
  333. // graphics.moveTo( aabb.xmin, aabb.ymin );
  334. // graphics.lineTo( aabb.xmax, aabb.ymin );
  335. // graphics.lineTo( aabb.xmax, aabb.ymax );
  336. // graphics.lineTo( aabb.xmin, aabb.ymax );
  337. // graphics.lineTo( aabb.xmin, aabb.ymin );
  338. }
  339. }
  340. /**
  341. * Returns true if the closest point
  342. * @param polygon
  343. * @param closestPoint
  344. * @return
  345. *
  346. */
  347. private function isVertex( polygon:Polygon2d, closestPoint:Vector2d ):Boolean
  348. {
  349. var vertices:Vector.<Vector2d> = polygon.vertices ;
  350. for ( var i:int = 0; i < vertices.length; i++ )
  351. {
  352. var vertex:Vector2d = vertices[i] ;
  353. var diff:Vector2d = closestPoint.Subtract( vertex );
  354. if ( diff.length < .00001 )
  355. {
  356. return true ;
  357. }
  358. }
  359. return false ;
  360. }
  361. /**
  362. * Returns the distance
  363. * @return
  364. *
  365. */
  366. private function getDistance( polygon:Polygon2d, point:Vector2d ):Vector2d
  367. {
  368. var vertices:Vector.<Vector2d> = polygon.vertices ;
  369. var dist:Number = Number.MAX_VALUE ;
  370. var index:int ;
  371. for ( var i:int = 0; i < vertices.length; i++ )
  372. {
  373. var b:Vector2d = vertices[(i + 1) % vertices.length];
  374. var a:Vector2d = vertices[i] ;
  375. var edge:Vector2d = b.Subtract( a ) ;
  376. edge.normalize();
  377. var u:Vector2d = point.Subtract( a );
  378. u.normalize();
  379. var d:Number = Math.abs( edge.dot( u ) - 1.0 );
  380. if ( d < dist )
  381. {
  382. index = i ;
  383. dist = d;
  384. }
  385. }
  386. return polygon.getNormal( index ) ;
  387. }
  388. /**
  389. * Returns the edge that's closest to the given point
  390. * @return
  391. *
  392. */
  393. private function getNormal( polygon:Polygon2d, point:Vector2d ):Vector2d
  394. {
  395. var vertices:Vector.<Vector2d> = polygon.vertices ;
  396. var dist:Number = Number.MAX_VALUE ;
  397. var index:int ;
  398. for ( var i:int = 0; i < vertices.length; i++ )
  399. {
  400. var b:Vector2d = vertices[(i + 1) % vertices.length];
  401. var a:Vector2d = vertices[i] ;
  402. var edge:Vector2d = b.Subtract( a ) ;
  403. edge.normalize();
  404. var u:Vector2d = point.Subtract( a );
  405. u.normalize();
  406. var d:Number = Math.abs( edge.dot( u ) - 1.0 );
  407. if ( d < dist )
  408. {
  409. index = i ;
  410. dist = d;
  411. }
  412. }
  413. return polygon.getNormal( index ) ;
  414. }
  415. public function updateCentroid( polygon:Polygon2d, centroid:Vector2d ):void
  416. {
  417. // Update the vertices
  418. var vertices:Vector.<Vector2d> = polygon.vertices ;
  419. for ( var j:int = 0; j < vertices.length; j++ )
  420. {
  421. var vertex:Vector2d = vertices[j] ;
  422. vertex.x -= polygon.centroid.x ;
  423. vertex.y -= polygon.centroid.y ;
  424. vertex.x += centroid.x ;
  425. vertex.y += centroid.y ;
  426. }
  427. polygon.centroid = centroid ;
  428. polygon.updateLines();
  429. }
  430. private function getAABB( polygon:Polygon2d ):AABB
  431. {
  432. var xmin:Number = Number.MAX_VALUE ;
  433. var ymin:Number = Number.MAX_VALUE ;
  434. var xmax:Number = Number.MIN_VALUE ;
  435. var ymax:Number = Number.MIN_VALUE ;
  436. for each ( var vertex:Vector2d in polygon.vertices )
  437. {
  438. xmin = Math.min( xmin, vertex.x );
  439. xmax = Math.max( xmax, vertex.x );
  440. ymin = Math.min( ymin, vertex.y );
  441. ymax = Math.max( ymax, vertex.y );
  442. }
  443. var aabb:AABB = new AABB( xmin, ymin, xmax, ymax );
  444. return aabb ;
  445. }
  446. /**
  447. * Creates a polygon with a number of vertices between
  448. * 3 and 6 ;
  449. *
  450. */
  451. private function createPolygon( centroid:Vector2d ):Polygon2d
  452. {
  453. // Create a polygon
  454. var polygon:Polygon2d = new Polygon2d( );
  455. // The polygon should have at least 3 and at most six points
  456. var points:int = 5;//3 + int( Math.random() * 3 );
  457. // Add points to the polygon
  458. var angle:Number = ( Math.PI / 180 ) * ( 360 / points ) ;
  459. var scale:Number = 40 ;
  460. for ( var i:int = 0; i < points; i++ )
  461. {
  462. var alpha:Number = angle * i ;
  463. var x:Number = Math.cos( alpha ) - Math.sin( alpha ) ;
  464. var y:Number = Math.sin( alpha ) + Math.cos( alpha ) ;
  465. polygon.addVertex( new Vector2d(( x * scale ) + centroid.x, ( y * scale ) + centroid.y ));
  466. }
  467. // Order the polygon vertices counter-clockwise
  468. polygon.orderVertices();
  469. // Create the collection of polygon edges
  470. polygon.updateLines();
  471. // Create a true
  472. polygon.createTree();
  473. // Return the polygon
  474. return polygon ;
  475. }
  476. }
  477. }