Author Topic: 2D Physics Engine  (Read 400 times)

0 Members and 1 Guest are viewing this topic.

Offline justsomeguy

  • Newbie
  • Posts: 38
2D Physics Engine
« on: April 20, 2021, 04:07:08 PM »
Hello

I've ported a simple 2D physics engine from JAVA/C++ to QB. I was thinking about using it for a larger project I have in mind, but I thought others might find some use in it or just use it to mess around.

It's a bit slow and it only handles convex polygons and circles. In future iterations I hope to add more features and optimzations.

The orginal code was written by Randy Gaul, and he has nothing to do with this project. His website is http://RandyGaul.net and it has a lot programming resources and articles.


Code: QB64: [Select]
  1. '/*
  2. '    Copyright (c) 2013 Randy Gaul http://RandyGaul.net
  3.  
  4. '    This software is provided 'as-is', without any express or implied
  5. '    warranty. In no event will the authors be held liable for any damages
  6. '    arising from the use of this software.
  7.  
  8. '    Permission is granted to anyone to use this software for any purpose,
  9. '    including commercial applications, and to alter it and redistribute it
  10. '    freely, subject to the following restrictions:
  11. '      1. The origin of this software must not be misrepresented; you must not
  12. '         claim that you wrote the original software. If you use this software
  13. '         in a product, an acknowledgment in the product documentation would be
  14. '         appreciated but is not required.
  15. '      2. Altered source versions must be plainly marked as such, and must not be
  16. '         misrepresented as being the original software.
  17. '      3. This notice may not be removed or altered from any source distribution.
  18.  
  19. '    Port to Java by Philip Diffenderfer http://magnos.org
  20. '    Port to QB64 by justsomeguy
  21. '*/
  22. '
  23.  
  24. '**********************************************************************************************
  25. '   Setup Types and Variables
  26. '**********************************************************************************************
  27. Type tVECTOR2d
  28.     x As _Float
  29.     y As _Float
  30.  
  31. Type tLINE2d ' Not used
  32.     a As tVECTOR2d
  33.     b As tVECTOR2d
  34.  
  35. Type tFACE2d ' Not used
  36.     f0 As tVECTOR2d
  37.     f1 As tVECTOR2d
  38.  
  39. Type tTRIANGLE ' Not used
  40.     a As tVECTOR2d
  41.     b As tVECTOR2d
  42.     c As tVECTOR2d
  43.  
  44. Type tMATRIX2d
  45.     m00 As _Float
  46.     m01 As _Float
  47.     m10 As _Float
  48.     m11 As _Float
  49.  
  50. Type tSHAPE
  51.     ty As Integer ' cSHAPE_CIRCLE = 1, cSHAPE_POLYGON = 2
  52.     radius As _Float ' Only necessary for circle shapes
  53.     u As tMATRIX2d ' Only neceassary for polygons
  54.  
  55. Type tPOLY 'list of vertices for all objects in simulation
  56.     vert As tVECTOR2d
  57.     norm As tVECTOR2d
  58.  
  59. Type tPOLYATTRIB 'keep track of polys in monlithic list of vertices
  60.     start As Integer ' starting vertex of the polygon
  61.     count As Integer ' number of vertices in polygon
  62.  
  63. Type tBODY
  64.     position As tVECTOR2d
  65.     velocity As tVECTOR2d
  66.     force As tVECTOR2d
  67.     angularVelocity As _Float
  68.     torque As _Float
  69.     orient As _Float
  70.     mass As _Float
  71.     invMass As _Float
  72.     inertia As _Float
  73.     invInertia As _Float
  74.     staticFriction As _Float
  75.     dynamicFriction As _Float
  76.     restitution As _Float
  77.     shape As tSHAPE
  78.     c As Long ' color
  79.     visible As Integer 'Hide a body ;)
  80.  
  81. Type tMANIFOLD
  82.     A As Integer
  83.     B As Integer
  84.     penetration As _Float
  85.     normal As tVECTOR2d
  86.     contactCount As Integer
  87.     e As _Float
  88.     df As _Float
  89.     sf As _Float
  90.  
  91. Const cSHAPE_CIRCLE = 1
  92. Const cSHAPE_POLYGON = 2
  93. Const cPRECISION = 100
  94. Const cMAXNUMOFTRIANGLES = 100
  95. Const cMAXNUMBEROFOBJECTS = 1000 ' Max number of objects at one time
  96. Const cMAXNUMBEROFPOLYGONS = 10000 ' Max number of total vertices included in all objects
  97. Const cPI = 3.14159
  98. Const cEPSILON = 0.0001
  99. Const cEPSILON_SQ = cEPSILON * cEPSILON
  100. Const cBIAS_RELATIVE = 0.95
  101. Const cBIAS_ABSOLUTE = 0.01
  102. Const cDT = 1.0 / 60.0
  103. Const cITERATIONS = 10
  104. Const cPENETRATION_ALLOWANCE = 0.05
  105. Const cPENETRATION_CORRECTION = 0.4 ' misspelled in original code
  106.  
  107. Dim Shared sGRAVITY As tVECTOR2d: Call vectorSet(sGRAVITY, 0.0, 100.0)
  108. Dim Shared sRESTING As _Float: Dim o As tVECTOR2d: Call vectorMultiplyScalarND(o, sGRAVITY, cDT): sRESTING = vectorLengthSq(o) + cEPSILON
  109. Dim Shared sNUMBEROFBODIES As Integer: sNUMBEROFBODIES = 80 ' 0 is included - Don't go under 10 for the current scene
  110.  
  111. Dim poly(cMAXNUMBEROFPOLYGONS) As tPOLY
  112. Dim polyattributes(cMAXNUMBEROFPOLYGONS) As tPOLYATTRIB
  113. Dim body(cMAXNUMBEROFOBJECTS) As tBODY
  114.  
  115.  
  116. Const cSCENE_USER = 0
  117. Const cSCENE_FLOOR = 1
  118. Const cSCENE_SPINNEROFDEATH = 2
  119. Const cSCENE_RIGHTRAMP = 3
  120. Const cSCENE_LEFTRAMP = 4
  121. Const cSCENE_DOZEROFDOOM = 5
  122. Const cSCENE_ELEVATOROFTERROR = 6
  123. Const cSCENE_RIGHTWALL = 7
  124. Const cSCENE_ELEVATORKICKER = 8
  125.  
  126.  
  127. '**********************************************************************************************
  128. _Title "phyNGN"
  129. Screen _NewImage(1024, 768, 32)
  130. Call buildSimpleScene(poly(), polyattributes(), body())
  131. '**********************************************************************************************
  132.     Cls , _RGB32(28, 28, 22)
  133.     Call animateScene(body())
  134.     Call renderBodies(poly(), polyattributes(), body())
  135.     Call impulseStep(poly(), polyattributes(), body(), cDT, cITERATIONS)
  136.     _Display
  137.     _Limit 240
  138. '**********************************************************************************************
  139. '   End of Main loop
  140. '**********************************************************************************************
  141.  
  142. '**********************************************************************************************
  143. '   Scene Creation and Handling Ahead
  144. '**********************************************************************************************
  145.  
  146. Sub animateScene (body() As tBODY)
  147.     If _MouseInput Then ' Terrible use interaction
  148.         Call vectorSet(body(cSCENE_USER).velocity, _MouseMovementX * 100, _MouseMovementY * 100)
  149.     End If
  150.  
  151.     body(cSCENE_SPINNEROFDEATH).orient = body(cSCENE_SPINNEROFDEATH).orient + (cPI / 90): If body(cSCENE_SPINNEROFDEATH).orient > 2 * cPI Then body(cSCENE_SPINNEROFDEATH).orient = 0
  152.     Call matrixSetRadians(body(cSCENE_SPINNEROFDEATH).shape.u, body(cSCENE_SPINNEROFDEATH).orient)
  153.     body(cSCENE_DOZEROFDOOM).position.x = body(cSCENE_DOZEROFDOOM).position.x - 1
  154.     If body(cSCENE_DOZEROFDOOM).position.x < 120 Then body(cSCENE_DOZEROFDOOM).position.x = _Width
  155.  
  156.     body(cSCENE_ELEVATOROFTERROR).position.y = body(cSCENE_ELEVATOROFTERROR).position.y - 1
  157.     If body(cSCENE_ELEVATOROFTERROR).position.y < 50 Then body(cSCENE_ELEVATOROFTERROR).position.y = _Height - 25
  158.  
  159. Sub buildSimpleScene (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY)
  160.     Dim i, ty As Integer
  161.  
  162.     Call createBoxBodies(p(), pa(), body(), cSCENE_USER, 10, 10)
  163.     Call vectorSet(body(cSCENE_USER).position, (_Width / 2), (_Height / 2))
  164.     Call vectorSet(body(cSCENE_USER).velocity, 0.0, 0.0)
  165.  
  166.     Call createBoxBodies(p(), pa(), body(), cSCENE_FLOOR, 1000, 5)
  167.     Call vectorSet(body(cSCENE_FLOOR).position, (_Width / 2), (_Height - 20))
  168.     Call bodySetStatic(body(cSCENE_FLOOR))
  169.  
  170.     Call createBoxBodies(p(), pa(), body(), cSCENE_SPINNEROFDEATH, 75, 5)
  171.     Call vectorSet(body(cSCENE_SPINNEROFDEATH).position, (_Width / 2), (_Height - 100))
  172.     Call bodySetStatic(body(cSCENE_SPINNEROFDEATH))
  173.  
  174.     Call createBoxBodies(p(), pa(), body(), cSCENE_RIGHTRAMP, 350, 5)
  175.     Call vectorSet(body(cSCENE_RIGHTRAMP).position, (_Width * .75) + 35, (_Height / 2) + 105)
  176.     body(cSCENE_RIGHTRAMP).orient = (cPI * .75)
  177.     Call matrixSetRadians(body(cSCENE_RIGHTRAMP).shape.u, body(cSCENE_RIGHTRAMP).orient)
  178.     Call bodySetStatic(body(cSCENE_RIGHTRAMP))
  179.  
  180.     Call createBoxBodies(p(), pa(), body(), cSCENE_LEFTRAMP, 225, 5)
  181.     Call vectorSet(body(cSCENE_LEFTRAMP).position, (_Width * .25) + 55, (_Height / 2))
  182.     body(cSCENE_LEFTRAMP).orient = (cPI * .1)
  183.     Call matrixSetRadians(body(cSCENE_LEFTRAMP).shape.u, body(cSCENE_LEFTRAMP).orient)
  184.     Call bodySetStatic(body(cSCENE_LEFTRAMP))
  185.  
  186.     Call createBoxBodies(p(), pa(), body(), cSCENE_DOZEROFDOOM, 5, 50)
  187.     Call vectorSet(body(cSCENE_DOZEROFDOOM).position, 0, _Height - 75)
  188.     Call bodySetStatic(body(cSCENE_DOZEROFDOOM))
  189.  
  190.     Call createBoxBodies(p(), pa(), body(), cSCENE_ELEVATOROFTERROR, 50, 5)
  191.     Call vectorSet(body(cSCENE_ELEVATOROFTERROR).position, 55, _Height - 25)
  192.     body(cSCENE_ELEVATOROFTERROR).orient = (cPI * .98)
  193.     Call matrixSetRadians(body(cSCENE_ELEVATOROFTERROR).shape.u, body(cSCENE_ELEVATOROFTERROR).orient)
  194.     Call bodySetStatic(body(cSCENE_ELEVATOROFTERROR))
  195.  
  196.     Call createBoxBodies(p(), pa(), body(), cSCENE_RIGHTWALL, 5, 375)
  197.     Call vectorSet(body(cSCENE_RIGHTWALL).position, 5, _Height / 2 - 20)
  198.     Call bodySetStatic(body(cSCENE_RIGHTWALL))
  199.  
  200.     Call createBoxBodies(p(), pa(), body(), cSCENE_ELEVATORKICKER, 100, 5)
  201.     Call vectorSet(body(cSCENE_ELEVATORKICKER).position, 55, 120)
  202.     body(cSCENE_ELEVATORKICKER).orient = (cPI * .70)
  203.     Call matrixSetRadians(body(cSCENE_ELEVATORKICKER).shape.u, body(cSCENE_ELEVATORKICKER).orient)
  204.     Call bodySetStatic(body(cSCENE_ELEVATORKICKER))
  205.  
  206.     For i = 9 To sNUMBEROFBODIES
  207.         If Rnd > .25 Then ' Circles are must faster than polygons
  208.             ty = cSHAPE_CIRCLE
  209.         Else
  210.             ty = cSHAPE_POLYGON
  211.         End If
  212.         Call createBodies(p(), pa(), body(), i, ty)
  213.         Call vectorSet(body(i).position, (Rnd * _Width / 2) + (_Width / 4), ((Rnd * _Height / 2) + (_Height / 4)) - 200)
  214.         Call vectorSet(body(i).velocity, Rnd * 100 - 50, Rnd * 100 - 50)
  215.     Next
  216.  
  217.  
  218. Sub createCircleBody (pa() As tPOLYATTRIB, body() As tBODY, index As Integer, radius As _Float)
  219.     Dim shape As tSHAPE
  220.     Call shapeCreate(shape, cSHAPE_CIRCLE, radius)
  221.     Call bodyCreate(body(), index, shape)
  222.     Call circleInitialize(body(), index)
  223.     ' Even though circles do not have Polygons, they still must be included in the Vertices
  224.     If index = 0 Then
  225.         pa(index).start = 0
  226.     Else
  227.         pa(index).start = pa(index - 1).start + pa(index - 1).count + 1
  228.     End If
  229.     pa(index).count = 1
  230.     body(index).c = _RGB32(255, 255, 255)
  231.  
  232. Sub createBoxBodies (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY, index As Integer, xs As _Float, ys As _Float)
  233.     Dim shape As tSHAPE
  234.     Call shapeCreate(shape, cSHAPE_POLYGON, 0)
  235.     Call bodyCreate(body(), index, shape)
  236.     Call boxCreate(p(), pa(), index, xs, ys)
  237.     Call polygonInitialize(body(), p(), pa(), index)
  238.     body(index).c = _RGB32(255, 255, 255)
  239.  
  240. Sub createBodies (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY, index As Integer, ty As Integer)
  241.     If ty = cSHAPE_CIRCLE Then
  242.         Call createCircleBody(pa(), body(), index, 10 + Rnd * 20)
  243.     Else
  244.         If ty = cSHAPE_POLYGON Then
  245.             Call createBoxBodies(p(), pa(), body(), index, 5 + Rnd * 10, 5 + Rnd * 10)
  246.         End If
  247.     End If
  248.     body(index).c = _RGB32(55 + Rnd * 200, 55 + Rnd * 200, 55 + Rnd * 200)
  249.  
  250. '**********************************************************************************************
  251. '   Physics and Collision Stuff Ahead
  252. '**********************************************************************************************
  253.  
  254. Sub impulseIntegrateForces (b As tBODY, dt As _Float)
  255.     If b.invMass = 0.0 Then Exit Sub
  256.     Dim dts As _Float
  257.     dts = dt * .5
  258.     Call vectorAddVectorScalar(b.velocity, b.force, b.invMass * dts)
  259.     Call vectorAddVectorScalar(b.velocity, sGRAVITY, dts)
  260.     b.angularVelocity = b.angularVelocity + (b.torque * b.invInertia * dts)
  261.  
  262. Sub impulseIntegrateVelocity (body As tBODY, dt As _Float)
  263.     If body.invMass = 0.0 Then Exit Sub
  264.     Call vectorAddVectorScalar(body.position, body.velocity, dt)
  265.     body.orient = body.orient + (body.angularVelocity * dt)
  266.     Call matrixSetRadians(body.shape.u, body.orient)
  267.     Call impulseIntegrateForces(body, dt)
  268.  
  269. Sub impulseStep (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY, dt As _Float, iterations As Integer)
  270.     Dim A As tBODY
  271.     Dim B As tBODY
  272.     Dim c(cMAXNUMBEROFOBJECTS) As tVECTOR2d
  273.     Dim m As tMANIFOLD
  274.     Dim manifolds(sNUMBEROFBODIES * sNUMBEROFBODIES) As tMANIFOLD
  275.     Dim collisions(sNUMBEROFBODIES * sNUMBEROFBODIES, cMAXNUMBEROFOBJECTS) As tVECTOR2d
  276.     Dim manifoldCount As Integer: manifoldCount = 0
  277.     '    // Generate new collision info
  278.     Dim i, j, k As Integer
  279.     ' TODO: Implement hidden object logic
  280.     For i = 0 To sNUMBEROFBODIES ' number of bodies
  281.         A = body(i)
  282.         For j = i + 1 To sNUMBEROFBODIES
  283.             B = body(j)
  284.             If A.invMass = 0.0 And B.invMass = 0.0 Then _Continue
  285.             'Mainfold solve - handle collisions
  286.             If A.shape.ty = cSHAPE_CIRCLE And B.shape.ty = cSHAPE_CIRCLE Then
  287.                 Call collisionCCHandle(m, c(), A, B)
  288.             Else
  289.                 If A.shape.ty = cSHAPE_POLYGON And B.shape.ty = cSHAPE_POLYGON Then
  290.                     Call collisionPPHandle(p(), pa(), body(), m, c(), i, j)
  291.                 Else
  292.                     If A.shape.ty = cSHAPE_CIRCLE And B.shape.ty = cSHAPE_POLYGON Then
  293.                         Call collisionCPHandle(p(), pa(), body(), m, c(), i, j)
  294.                     Else
  295.                         If B.shape.ty = cSHAPE_CIRCLE And A.shape.ty = cSHAPE_POLYGON Then
  296.                             Call collisionPCHandle(p(), pa(), body(), m, c(), i, j)
  297.                         End If
  298.                     End If
  299.                 End If
  300.             End If
  301.             If m.contactCount > 0 Then
  302.                 m.A = i 'identify the index of objects
  303.                 m.B = j
  304.                 manifolds(manifoldCount) = m
  305.                 For k = 0 To m.contactCount
  306.                     collisions(manifoldCount, k) = c(k)
  307.                 Next
  308.                 manifoldCount = manifoldCount + 1
  309.             End If
  310.         Next
  311.     Next
  312.     '   // Integrate forces
  313.     For i = 0 To sNUMBEROFBODIES
  314.         Call impulseIntegrateForces(body(i), dt)
  315.     Next
  316.     '   // Initialize collision
  317.     For i = 0 To manifoldCount - 1
  318.         ' this is the stupidest thing ever since QB will not let you split arrays
  319.         For k = 0 To manifolds(i).contactCount - 1
  320.             c(k) = collisions(i, k)
  321.         Next
  322.         Call manifoldInit(manifolds(i), body(), c())
  323.     Next
  324.     '// Solve collisions
  325.     For j = 0 To iterations - 1
  326.         For i = 0 To manifoldCount - 1
  327.             For k = 0 To manifolds(i).contactCount - 1
  328.                 c(k) = collisions(i, k)
  329.             Next
  330.             Call manifoldApplyImpulse(manifolds(i), body(), c())
  331.         Next i
  332.     Next j
  333.     '// Integrate velocities
  334.     For i = 0 To sNUMBEROFBODIES
  335.         Call impulseIntegrateVelocity(body(i), dt)
  336.     Next
  337.     '// Correct positions
  338.     For i = 0 To manifoldCount - 1
  339.         Call manifoldPositionalCorrection(manifolds(i), body())
  340.     Next
  341.     '// Clear all forces
  342.     For i = 0 To sNUMBEROFBODIES
  343.         Call vectorSet(body(i).force, 0, 0)
  344.         body(i).torque = 0
  345.     Next
  346.  
  347. Sub bodyApplyImpulse (body As tBODY, impulse As tVECTOR2d, contactVector As tVECTOR2d)
  348.     Call vectorAddVectorScalar(body.velocity, impulse, body.invMass)
  349.     body.angularVelocity = body.angularVelocity + body.invInertia * vectorCross(contactVector, impulse)
  350.  
  351. Sub bodySetStatic (body As tBODY)
  352.     body.inertia = 0.0
  353.     body.invInertia = 0.0
  354.     body.mass = 0.0
  355.     body.invMass = 0.0
  356.  
  357. Sub manifoldInit (m As tMANIFOLD, body() As tBODY, contacts() As tVECTOR2d)
  358.     Dim ra As tVECTOR2d
  359.     Dim rb As tVECTOR2d
  360.     Dim rv As tVECTOR2d
  361.     Dim tv1 As tVECTOR2d
  362.     Dim tv2 As tVECTOR2d
  363.     m.e = scalarMin(body(m.A).restitution, body(m.B).restitution)
  364.     m.sf = Sqr(body(m.A).staticFriction * body(m.A).staticFriction)
  365.     m.df = Sqr(body(m.A).dynamicFriction * body(m.A).dynamicFriction)
  366.     Dim i As Integer
  367.     For i = 0 To m.contactCount - 1
  368.         Call vectorSubVectorND(contacts(i), body(m.A).position, ra)
  369.         Call vectorSubVectorND(contacts(i), body(m.B).position, rb)
  370.  
  371.         Call vectorCrossScalar(tv1, rb, body(m.B).angularVelocity)
  372.         Call vectorCrossScalar(tv2, ra, body(m.A).angularVelocity)
  373.         Call vectorAddVector(tv1, body(m.B).velocity)
  374.         Call vectorSubVectorND(tv2, body(m.A).velocity, tv2)
  375.         Call vectorSubVectorND(rv, tv1, tv2)
  376.  
  377.         If vectorLengthSq(rv) < sRESTING Then
  378.             m.e = 0.0
  379.         End If
  380.     Next
  381.  
  382. Sub manifoldApplyImpulse (m As tMANIFOLD, body() As tBODY, contacts() As tVECTOR2d)
  383.     Dim ra As tVECTOR2d
  384.     Dim rb As tVECTOR2d
  385.     Dim rv As tVECTOR2d
  386.     Dim tv1 As tVECTOR2d
  387.     Dim tv2 As tVECTOR2d
  388.     Dim contactVel As _Float
  389.  
  390.     Dim raCrossN As _Float
  391.     Dim rbCrossN As _Float
  392.     Dim invMassSum As _Float
  393.     Dim i As Integer
  394.     Dim j As _Float
  395.     Dim impulse As tVECTOR2d
  396.  
  397.     Dim t As tVECTOR2d
  398.     Dim jt As _Float
  399.     Dim tangentImpulse As tVECTOR2d
  400.  
  401.     If impulseEqual(body(m.A).invMass + body(m.B).invMass, 0.0) Then
  402.         Call manifoldInfiniteMassCorrection(body(m.A), body(m.B))
  403.         Exit Sub
  404.     End If
  405.  
  406.     For i = 0 To m.contactCount - 1
  407.         '// Calculate radii from COM to contact
  408.         '// Vec2 ra = contacts[i] - A->position;
  409.         '// Vec2 rb = contacts[i] - B->position;
  410.         Call vectorSubVectorND(ra, contacts(i), body(m.A).position)
  411.         Call vectorSubVectorND(rb, contacts(i), body(m.B).position)
  412.  
  413.         '// Relative velocity
  414.         '// Vec2 rv = B->velocity + Cross( B->angularVelocity, rb ) - A->velocity - Cross( A->angularVelocity, ra );
  415.         Call vectorCrossScalar(tv1, rb, body(m.B).angularVelocity)
  416.         Call vectorCrossScalar(tv2, ra, body(m.A).angularVelocity)
  417.         Call vectorAddVectorND(rv, tv1, body(m.B).velocity)
  418.         Call vectorSubVector(rv, body(m.A).velocity)
  419.         Call vectorSubVector(rv, tv2)
  420.  
  421.         '// Relative velocity along the normal
  422.         '// real contactVel = Dot( rv, normal );
  423.         contactVel = vectorDot(rv, m.normal)
  424.  
  425.  
  426.         '// Do not resolve if velocities are separating
  427.         If contactVel > 0 Then Exit Sub
  428.  
  429.         '// real raCrossN = Cross( ra, normal );
  430.         '// real rbCrossN = Cross( rb, normal );
  431.         '// real invMassSum = A->im + B->im + Sqr( raCrossN ) * A->iI + Sqr( rbCrossN ) * B->iI;
  432.         raCrossN = vectorCross(ra, m.normal)
  433.         rbCrossN = vectorCross(rb, m.normal)
  434.         invMassSum = body(m.A).invMass + body(m.B).invMass + (raCrossN * raCrossN) * body(m.A).invInertia + (rbCrossN * rbCrossN) * body(m.B).invInertia
  435.  
  436.  
  437.         '// Calculate impulse scalar
  438.         j = -(1.0 + m.e) * contactVel
  439.         j = j / invMassSum
  440.         j = j / m.contactCount
  441.  
  442.         '// Apply impulse
  443.         Call vectorMultiplyScalarND(impulse, m.normal, j)
  444.  
  445.         Call vectorNegND(tv1, impulse)
  446.         Call bodyApplyImpulse(body(m.A), tv1, ra)
  447.         Call bodyApplyImpulse(body(m.B), impulse, rb)
  448.  
  449.         '// Friction impulse
  450.         '// rv = B->velocity + Cross( B->angularVelocity, rb ) - A->velocity - Cross( A->angularVelocity, ra );
  451.         Call vectorCrossScalar(tv1, rb, body(m.B).angularVelocity)
  452.         Call vectorCrossScalar(tv2, ra, body(m.A).angularVelocity)
  453.         Call vectorAddVectorND(rv, tv1, body(m.B).velocity)
  454.         Call vectorSubVector(rv, body(m.A).velocity)
  455.         Call vectorSubVector(rv, tv2)
  456.  
  457.         '// Vec2 t = rv - (normal * Dot( rv, normal ));
  458.         '// t.Normalize( );
  459.         Call vectorMultiplyScalarND(t, m.normal, vectorDot(rv, m.normal))
  460.         Call vectorSubVectorND(t, rv, t)
  461.         Call vectorNormalize(t)
  462.  
  463.         '// j tangent magnitude
  464.         jt = -vectorDot(rv, t)
  465.         jt = jt / invMassSum
  466.         jt = jt / m.contactCount
  467.  
  468.         '// Don't apply tiny friction impulses
  469.         If impulseEqual(jt, 0.0) Then Exit Sub
  470.  
  471.         '// Coulumb's law
  472.         If Abs(jt) < j * m.sf Then
  473.             Call vectorMultiplyScalarND(tangentImpulse, t, jt)
  474.         Else
  475.             Call vectorMultiplyScalarND(tangentImpulse, t, -j * m.df)
  476.         End If
  477.  
  478.         '// Apply friction impulse
  479.         '// A->ApplyImpulse( -tangentImpulse, ra );
  480.         '// B->ApplyImpulse( tangentImpulse, rb );
  481.         Call vectorNegND(tv1, tangentImpulse)
  482.         Call bodyApplyImpulse(body(m.A), tv1, ra)
  483.         Call bodyApplyImpulse(body(m.B), tangentImpulse, rb)
  484.     Next i
  485.  
  486. Sub manifoldPositionalCorrection (m As tMANIFOLD, body() As tBODY)
  487.     Dim correction As _Float
  488.     correction = scalarMax(m.penetration - cPENETRATION_ALLOWANCE, 0.0) / (body(m.A).invMass + body(m.B).invMass) * cPENETRATION_CORRECTION
  489.     Call vectorAddVectorScalar(body(m.A).position, m.normal, -body(m.A).invMass * correction)
  490.     Call vectorAddVectorScalar(body(m.B).position, m.normal, body(m.B).invMass * correction)
  491.  
  492. Sub manifoldInfiniteMassCorrection (A As tBODY, B As tBODY)
  493.     Call vectorSet(A.velocity, 0, 0)
  494.     Call vectorSet(B.velocity, 0, 0)
  495.  
  496. '**********************************************************************************************
  497. '   Collision Stuff Ahead
  498. '**********************************************************************************************
  499.  
  500. Sub collisionCCHandle (m As tMANIFOLD, contacts() As tVECTOR2d, A As tBODY, B As tBODY)
  501.     Dim normal As tVECTOR2d
  502.     Dim dist_sqr As _Float
  503.     Dim radius As _Float
  504.  
  505.     Call vectorSubVectorND(normal, B.position, A.position)
  506.     dist_sqr = vectorLengthSq(normal)
  507.     radius = A.shape.radius + B.shape.radius
  508.  
  509.     If (dist_sqr >= radius * radius) Then
  510.         m.contactCount = 0
  511.     Else
  512.         Dim distance As _Float
  513.         distance = Sqr(dist_sqr)
  514.         m.contactCount = 1
  515.  
  516.         If distance = 0 Then
  517.             m.penetration = A.shape.radius
  518.             Call vectorSet(m.normal, 1.0, 0.0)
  519.             Call vectorSetVector(contacts(0), A.position)
  520.         Else
  521.             m.penetration = radius - distance
  522.             Call vectorDivideScalarND(m.normal, normal, distance)
  523.  
  524.             Call vectorMultiplyScalarND(contacts(0), m.normal, A.shape.radius)
  525.             Call vectorAddVector(contacts(0), A.position)
  526.         End If
  527.     End If
  528.  
  529. Sub collisionPCHandle (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY, m As tMANIFOLD, contacts() As tVECTOR2d, A As Integer, B As Integer)
  530.     Call collisionCPHandle(p(), pa(), body(), m, contacts(), B, A)
  531.     If m.contactCount > 0 Then
  532.         Call vectorNeg(m.normal)
  533.     End If
  534.  
  535. Sub collisionCPHandle (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY, m As tMANIFOLD, contacts() As tVECTOR2d, A As Integer, B As Integer)
  536.     'A is the Circle
  537.     'B is the POLY
  538.     m.contactCount = 0
  539.     Dim center As tVECTOR2d
  540.     Dim tm As tMATRIX2d
  541.     Dim tv As tVECTOR2d
  542.     Dim ARadius As _Float: ARadius = body(A).shape.radius
  543.  
  544.  
  545.     Call vectorSubVectorND(center, body(A).position, body(B).position)
  546.     Call matrixTranspose(body(B).shape.u, tm)
  547.     Call matrixMultiplyVector(tm, center, center)
  548.  
  549.     Dim separation As _Float: separation = -9999999
  550.     Dim faceNormal As Integer: faceNormal = 0
  551.     Dim i As Integer
  552.     Dim s As _Float
  553.     For i = 0 To pa(B).count
  554.         Call vectorSubVectorND(tv, center, p(pa(B).start + i).vert)
  555.         s = vectorDot(p(pa(B).start + i).norm, tv)
  556.         If s > ARadius Then Exit Sub
  557.         If s > separation Then
  558.             separation = s
  559.             faceNormal = i
  560.         End If
  561.     Next
  562.     Dim v1 As tVECTOR2d
  563.     v1 = p(pa(B).start + faceNormal).vert
  564.     Dim i2 As Integer
  565.     i2 = pa(B).start + arrayNextIndex(faceNormal, pa(B).count)
  566.     Dim v2 As tVECTOR2d
  567.     v2 = p(i2).vert
  568.  
  569.     If separation < cEPSILON Then
  570.         m.contactCount = 1
  571.         Call matrixMultiplyVector(body(B).shape.u, p(pa(B).start + faceNormal).norm, m.normal)
  572.         Call vectorNeg(m.normal)
  573.         Call vectorMultiplyScalarND(contacts(0), m.normal, ARadius)
  574.         Call vectorAddVector(contacts(0), body(A).position)
  575.         m.penetration = ARadius
  576.         Exit Sub
  577.     End If
  578.  
  579.     Dim dot1 As _Float
  580.     Dim dot2 As _Float
  581.  
  582.     Dim tv1 As tVECTOR2d
  583.     Dim tv2 As tVECTOR2d
  584.     Dim n As tVECTOR2d
  585.     Call vectorSubVectorND(tv1, center, v1)
  586.     Call vectorSubVectorND(tv2, v2, v1)
  587.     dot1 = vectorDot(tv1, tv2)
  588.     Call vectorSubVectorND(tv1, center, v2)
  589.     Call vectorSubVectorND(tv2, v1, v2)
  590.     dot2 = vectorDot(tv1, tv2)
  591.     m.penetration = ARadius - separation
  592.     If dot1 <= 0.0 Then
  593.         If vectorSqDist(center, v1) > ARadius * ARadius Then Exit Sub
  594.         m.contactCount = 1
  595.         Call vectorSubVectorND(n, v1, center)
  596.         Call matrixMultiplyVector(body(B).shape.u, n, n)
  597.         Call vectorNormalize(n)
  598.         m.normal = n
  599.         Call matrixMultiplyVector(body(B).shape.u, v1, v1)
  600.         Call vectorAddVectorND(v1, v1, body(B).position)
  601.         contacts(0) = v1
  602.     Else
  603.         If dot2 <= 0.0 Then
  604.             If vectorSqDist(center, v2) > ARadius * ARadius Then Exit Sub
  605.             m.contactCount = 1
  606.             Call vectorSubVectorND(n, v2, center)
  607.             Call matrixMultiplyVector(body(B).shape.u, v2, v2)
  608.             Call vectorAddVectorND(v2, v2, body(B).position)
  609.             contacts(0) = v2
  610.             Call matrixMultiplyVector(body(B).shape.u, n, n)
  611.             Call vectorNormalize(n)
  612.             m.normal = n
  613.         Else
  614.             n = p(pa(B).start + faceNormal).norm
  615.             Call vectorSubVectorND(tv1, center, v1)
  616.             If vectorDot(tv1, n) > ARadius Then Exit Sub
  617.             m.contactCount = 1
  618.             Call matrixMultiplyVector(body(B).shape.u, n, n)
  619.             Call vectorNeg(n)
  620.             m.normal = n
  621.             Call vectorMultiplyScalarND(contacts(0), m.normal, ARadius)
  622.             Call vectorAddVector(contacts(0), body(A).position)
  623.  
  624.         End If
  625.     End If
  626.  
  627. Function collisionPPClip (n As tVECTOR2d, c As _Float, face() As tVECTOR2d)
  628.     Dim sp As Integer: sp = 0
  629.     Dim o(cMAXNUMBEROFPOLYGONS) As tVECTOR2d
  630.  
  631.     o(0) = face(0)
  632.     o(1) = face(1)
  633.  
  634.     Dim d1 As _Float: d1 = vectorDot(n, face(0)) - c
  635.     Dim d2 As _Float: d2 = vectorDot(n, face(1)) - c
  636.  
  637.     If d1 <= 0.0 Then
  638.         o(sp) = face(0)
  639.         sp = sp + 1
  640.     End If
  641.  
  642.     If d2 <= 0.0 Then
  643.         o(sp) = face(1)
  644.         sp = sp + 1
  645.     End If
  646.  
  647.     If d1 * d2 < 0.0 Then
  648.         Dim alpha As _Float: alpha = d1 / (d1 - d2)
  649.         Dim tempv As tVECTOR2d
  650.         'out[sp] = face[0] + alpha * (face[1] - face[0]);
  651.         Call vectorSubVectorND(tempv, face(1), face(0))
  652.         Call vectorMultiplyScalar(tempv, alpha)
  653.         Call vectorAddVectorND(o(sp), tempv, face(0))
  654.         sp = sp + 1
  655.     End If
  656.     face(0) = o(0)
  657.     face(1) = o(1)
  658.     collisionPPClip = sp
  659.  
  660. Sub collisionPPFindIncidentFace (p() As tPOLY, pa() As tPOLYATTRIB, b() As tBODY, v() As tVECTOR2d, RefPoly As Integer, IncPoly As Integer, referenceIndex As Integer)
  661.     Dim referenceNormal As tVECTOR2d
  662.     Dim uRef As tMATRIX2d: uRef = b(RefPoly).shape.u
  663.     Dim uInc As tMATRIX2d: uInc = b(IncPoly).shape.u
  664.     Dim uTemp As tMATRIX2d
  665.     Dim i As Integer
  666.     referenceNormal = p(pa(RefPoly).start + referenceIndex).norm
  667.  
  668.     '        // Calculate normal in incident's frame of reference
  669.     '        // referenceNormal = RefPoly->u * referenceNormal; // To world space
  670.     Call matrixMultiplyVector(uRef, referenceNormal, referenceNormal)
  671.     '        // referenceNormal = IncPoly->u.Transpose( ) * referenceNormal; // To incident's model space
  672.     Call matrixTranspose(uInc, uTemp)
  673.     Call matrixMultiplyVector(uTemp, referenceNormal, referenceNormal)
  674.  
  675.     Dim incidentFace As Integer: incidentFace = 0
  676.     Dim minDot As _Float: minDot = 9999999
  677.     Dim dot As _Float
  678.     For i = 0 To pa(IncPoly).count
  679.         dot = vectorDot(referenceNormal, p(pa(IncPoly).start + i).norm)
  680.         If (dot < minDot) Then
  681.             minDot = dot
  682.             incidentFace = i
  683.         End If
  684.     Next
  685.  
  686.     '// Assign face vertices for incidentFace
  687.     '// v[0] = IncPoly->u * IncPoly->m_vertices[incidentFace] + IncPoly->body->position;
  688.     Call matrixMultiplyVector(uInc, p(pa(IncPoly).start + incidentFace).vert, v(0))
  689.     Call vectorAddVector(v(0), b(IncPoly).position)
  690.  
  691.     '// incidentFace = incidentFace + 1 >= (int32)IncPoly->m_vertexCount ? 0 : incidentFace + 1;
  692.     incidentFace = arrayNextIndex(incidentFace, pa(IncPoly).count)
  693.  
  694.     '// v[1] = IncPoly->u * IncPoly->m_vertices[incidentFace] +  IncPoly->body->position;
  695.     Call matrixMultiplyVector(uInc, p(pa(IncPoly).start + incidentFace).vert, v(1))
  696.     Call vectorAddVector(v(1), b(IncPoly).position)
  697.  
  698. Sub collisionPPHandle (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY, m As tMANIFOLD, contacts() As tVECTOR2d, A As Integer, B As Integer)
  699.     m.contactCount = 0
  700.  
  701.     Dim faceA(100) As Integer
  702.  
  703.     Dim penetrationA As _Float
  704.     penetrationA = collisionPPFindAxisLeastPenetration(p(), pa(), body(), faceA(), A, B)
  705.     If penetrationA >= 0.0 Then Exit Sub
  706.  
  707.     Dim faceB(100) As Integer
  708.  
  709.     Dim penetrationB As _Float
  710.     penetrationB = collisionPPFindAxisLeastPenetration(p(), pa(), body(), faceB(), B, A)
  711.     If penetrationB >= 0.0 Then Exit Sub
  712.  
  713.  
  714.     Dim referenceIndex As Integer
  715.     Dim flip As Integer
  716.  
  717.     Dim RefPoly As Integer
  718.     Dim IncPoly As Integer
  719.  
  720.     If impulseGT(penetrationA, penetrationB) Then
  721.         RefPoly = A
  722.         IncPoly = B
  723.         referenceIndex = faceA(0)
  724.         flip = 0
  725.     Else
  726.         RefPoly = B
  727.         IncPoly = A
  728.         referenceIndex = faceB(0)
  729.         flip = 1
  730.     End If
  731.  
  732.     Dim incidentFace(2) As tVECTOR2d
  733.  
  734.     Call collisionPPFindIncidentFace(p(), pa(), body(), incidentFace(), RefPoly, IncPoly, referenceIndex)
  735.     Dim v1 As tVECTOR2d
  736.     Dim v2 As tVECTOR2d
  737.     Dim v1t As tVECTOR2d
  738.     Dim v2t As tVECTOR2d
  739.  
  740.  
  741.     v1 = p(pa(RefPoly).start + referenceIndex).vert
  742.     referenceIndex = arrayNextIndex(referenceIndex, pa(RefPoly).count)
  743.     v2 = p(pa(RefPoly).start + referenceIndex).vert
  744.     '// Transform vertices to world space
  745.     '// v1 = RefPoly->u * v1 + RefPoly->body->position;
  746.     '// v2 = RefPoly->u * v2 + RefPoly->body->position;
  747.     Call matrixMultiplyVector(body(RefPoly).shape.u, v1, v1t)
  748.     Call vectorAddVectorND(v1, v1t, body(RefPoly).position)
  749.     Call matrixMultiplyVector(body(RefPoly).shape.u, v2, v2t)
  750.     Call vectorAddVectorND(v2, v2t, body(RefPoly).position)
  751.     ' Circle (v1.x, v1.y), 5, _RGB32(255, 255, 0)
  752.     'Circle (v2.x, v2.y), 5, _RGB32(255, 255, 0)
  753.  
  754.     '// Calculate reference face side normal in world space
  755.     '// Vec2 sidePlaneNormal = (v2 - v1);
  756.     '// sidePlaneNormal.Normalize( );
  757.     Dim sidePlaneNormal As tVECTOR2d
  758.     Call vectorSubVectorND(sidePlaneNormal, v2, v1)
  759.     Call vectorNormalize(sidePlaneNormal)
  760.  
  761.     '// Orthogonalize
  762.     '// Vec2 refFaceNormal( sidePlaneNormal.y, -sidePlaneNormal.x );
  763.     Dim refFaceNormal As tVECTOR2d
  764.     Call vectorSet(refFaceNormal, sidePlaneNormal.y, -sidePlaneNormal.x)
  765.  
  766.     '// ax + by = c
  767.     '// c is distance from origin
  768.     '// real refC = Dot( refFaceNormal, v1 );
  769.     '// real negSide = -Dot( sidePlaneNormal, v1 );
  770.     '// real posSide = Dot( sidePlaneNormal, v2 );
  771.     Dim refC As _Float: refC = vectorDot(refFaceNormal, v1)
  772.     Dim negSide As _Float: negSide = -vectorDot(sidePlaneNormal, v1)
  773.     Dim posSide As _Float: posSide = vectorDot(sidePlaneNormal, v2)
  774.  
  775.  
  776.     '// Clip incident face to reference face side planes
  777.     '// if(Clip( -sidePlaneNormal, negSide, incidentFace ) < 2)
  778.     Dim negSidePlaneNormal As tVECTOR2d
  779.     Call vectorNegND(negSidePlaneNormal, sidePlaneNormal)
  780.  
  781.  
  782.     If collisionPPClip(negSidePlaneNormal, negSide, incidentFace()) < 2 Then Exit Sub
  783.  
  784.     If collisionPPClip(sidePlaneNormal, posSide, incidentFace()) < 2 Then Exit Sub
  785.  
  786.     Call vectorSet(m.normal, refFaceNormal.x, refFaceNormal.y)
  787.     If flip Then Call vectorNeg(m.normal)
  788.  
  789.     '// Keep points behind reference face
  790.     Dim cp As Integer: cp = 0 '// clipped points behind reference face
  791.     Dim separation As _Float
  792.     separation = vectorDot(refFaceNormal, incidentFace(0)) - refC
  793.     If separation <= 0.0 Then
  794.         contacts(cp) = incidentFace(0)
  795.         m.penetration = -separation
  796.         cp = cp + 1
  797.     Else
  798.         m.penetration = 0
  799.     End If
  800.  
  801.     separation = vectorDot(refFaceNormal, incidentFace(1)) - refC
  802.     If separation <= 0.0 Then
  803.         contacts(cp) = incidentFace(1)
  804.         m.penetration = m.penetration + -separation
  805.         cp = cp + 1
  806.         m.penetration = m.penetration / cp
  807.     End If
  808.     m.contactCount = cp
  809.  
  810. Function collisionPPFindAxisLeastPenetration (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY, faceIndex() As Integer, A As Integer, B As Integer)
  811.     Dim bestDistance As _Float: bestDistance = -9999999
  812.     Dim bestIndex As Integer: bestIndex = 0
  813.  
  814.     Dim n As tVECTOR2d
  815.     Dim nw As tVECTOR2d
  816.     Dim buT As tMATRIX2d
  817.     Dim s As tVECTOR2d
  818.     Dim nn As tVECTOR2d
  819.     Dim v As tVECTOR2d
  820.     Dim tv As tVECTOR2d
  821.     Dim d As _Float
  822.     Dim i, k As Integer
  823.  
  824.     For i = 0 To pa(A).count
  825.         k = pa(A).start + i
  826.  
  827.         '// Retrieve a face normal from A
  828.         '// Vec2 n = A->m_normals[i];
  829.         '// Vec2 nw = A->u * n;
  830.         n = p(k).norm
  831.         Call matrixMultiplyVector(body(A).shape.u, n, nw)
  832.  
  833.  
  834.         '// Transform face normal into B's model space
  835.         '// Mat2 buT = B->u.Transpose( );
  836.         '// n = buT * nw;
  837.         Call matrixTranspose(body(B).shape.u, buT)
  838.         Call matrixMultiplyVector(buT, nw, n)
  839.  
  840.         '// Retrieve support point from B along -n
  841.         '// Vec2 s = B->GetSupport( -n );
  842.         Call vectorNegND(nn, n)
  843.         Call vectorGetSupport(p(), pa(), B, nn, s)
  844.  
  845.         '// Retrieve vertex on face from A, transform into
  846.         '// B's model space
  847.         '// Vec2 v = A->m_vertices[i];
  848.         '// v = A->u * v + A->body->position;
  849.         '// v -= B->body->position;
  850.         '// v = buT * v;
  851.  
  852.         v = p(k).vert
  853.         Call matrixMultiplyVector(body(A).shape.u, v, tv)
  854.         Call vectorAddVectorND(v, tv, body(A).position)
  855.  
  856.         Call vectorSubVector(v, body(B).position)
  857.         Call matrixMultiplyVector(buT, v, tv)
  858.  
  859.         Call vectorSubVector(s, tv)
  860.         d = vectorDot(n, s)
  861.  
  862.         If d > bestDistance Then
  863.             bestDistance = d
  864.             bestIndex = i
  865.         End If
  866.  
  867.     Next i
  868.  
  869.     faceIndex(0) = bestIndex
  870.  
  871.     collisionPPFindAxisLeastPenetration = bestDistance
  872.  
  873. '**********************************************************************************************
  874. '   Shape Creation Ahead
  875. '**********************************************************************************************
  876.  
  877. Sub shapeCreate (sh As tSHAPE, ty As Integer, radius As _Float)
  878.     Dim u As tMATRIX2d
  879.     Call matrixSetScalar(u, 1, 0, 0, 1)
  880.     sh.ty = ty
  881.     sh.radius = radius
  882.     sh.u = u
  883.  
  884. Sub bodyCreate (body() As tBODY, index As Integer, shape As tSHAPE)
  885.     Call vectorSet(body(index).position, 0, 0)
  886.     Call vectorSet(body(index).velocity, 0, 0)
  887.     body(index).angularVelocity = 0.0
  888.     body(index).torque = 0.0
  889.     body(index).orient = impulseRandom_float(-cPI, cPI)
  890.  
  891.     Call vectorSet(body(index).force, 0, 0)
  892.     body(index).staticFriction = 0.5
  893.     body(index).dynamicFriction = 0.3
  894.     body(index).restitution = 0.2
  895.     body(index).shape = shape
  896.  
  897. Sub boxCreate (p() As tPOLY, pa() As tPOLYATTRIB, index As Integer, sizex As _Float, sizey As _Float)
  898.     Dim vertlength As Integer: vertlength = 3
  899.     Dim verts(vertlength) As tVECTOR2d
  900.  
  901.     Call vectorSet(verts(0), -sizex, -sizey)
  902.     Call vectorSet(verts(1), sizex, -sizey)
  903.     Call vectorSet(verts(2), sizex, sizey)
  904.     Call vectorSet(verts(3), -sizex, sizey)
  905.  
  906.     Call vertexSet(p(), pa(), index, verts(), vertlength)
  907.  
  908. Sub vShapeCreate (p() As tPOLY, pa() As tPOLYATTRIB, index As Integer, sizex As _Float, sizey As _Float)
  909.     Dim vertlength As Integer: vertlength = 7
  910.     Dim verts(vertlength) As tVECTOR2d
  911.  
  912.     Call vectorSet(verts(0), -sizex, -sizey)
  913.     Call vectorSet(verts(1), sizex, -sizey)
  914.     Call vectorSet(verts(2), sizex, sizey)
  915.     Call vectorSet(verts(3), -sizex, sizey)
  916.     Call vectorSet(verts(4), -sizex, sizey / 2)
  917.     Call vectorSet(verts(5), sizex / 2, sizey / 2)
  918.     Call vectorSet(verts(6), sizex / 2, -sizey / 2)
  919.     Call vectorSet(verts(7), -sizex, sizey / 2)
  920.  
  921.     Call vertexSet(p(), pa(), index, verts(), vertlength)
  922.  
  923. Sub vertexSet (p() As tPOLY, pa() As tPOLYATTRIB, index As Integer, verts() As tVECTOR2d, vertLength As Integer)
  924.     Dim rightMost As Integer: rightMost = 0
  925.     Dim highestXCoord As _Float: highestXCoord = verts(0).x
  926.     Dim i As Integer
  927.     Dim x As _Float
  928.     For i = 1 To vertLength
  929.         x = verts(i).x
  930.         If x > highestXCoord Then
  931.             highestXCoord = x
  932.             rightMost = i
  933.         Else
  934.             If x = highestXCoord Then
  935.                 If verts(i).y < verts(rightMost).y Then
  936.                     rightMost = i
  937.                 End If
  938.             End If
  939.         End If
  940.     Next
  941.     Dim hull(cMAXNUMBEROFPOLYGONS) As Integer
  942.     Dim outCount As Integer: outCount = 0
  943.     Dim indexHull As Integer: indexHull = rightMost
  944.     Dim nextHullIndex As Integer
  945.     Dim e1 As tVECTOR2d
  946.     Dim e2 As tVECTOR2d
  947.     Dim c As _Float
  948.     Do
  949.         hull(outCount) = indexHull
  950.         nextHullIndex = 0
  951.         For i = 1 To vertLength
  952.             If nextHullIndex = indexHull Then
  953.                 nextHullIndex = i
  954.                 _Continue
  955.             End If
  956.             Call vectorSubVectorND(e1, verts(nextHullIndex), verts(hull(outCount)))
  957.             Call vectorSubVectorND(e2, verts(i), verts(hull(outCount)))
  958.             c = vectorCross(e1, e2)
  959.             If c < 0.0 Then nextHullIndex = i
  960.             If c = 0.0 And (vectorLengthSq(e2) > vectorLengthSq(e1)) Then
  961.                 nextHullIndex = i
  962.             End If
  963.         Next
  964.         outCount = outCount + 1
  965.         indexHull = nextHullIndex
  966.         If nextHullIndex = rightMost Then
  967.             pa(index).count = outCount - 1
  968.             Exit Do
  969.         End If
  970.     Loop
  971.  
  972.     If index = 0 Then
  973.         pa(index).start = 0
  974.     Else
  975.         pa(index).start = pa(index - 1).start + pa(index - 1).count + 1
  976.     End If
  977.  
  978.     For i = 0 To vertLength
  979.         p(pa(index).start + i).vert = verts(hull(i))
  980.     Next
  981.     Dim face As tVECTOR2d
  982.     For i = 0 To vertLength
  983.         Call vectorSubVectorND(face, p(pa(index).start + arrayNextIndex(i, pa(index).count)).vert, p(pa(index).start + i).vert)
  984.         Call vectorSet(p(pa(index).start + i).norm, face.y, -face.x)
  985.         Call vectorNormalize(p(pa(index).start + i).norm)
  986.     Next
  987.  
  988. Function arrayNextIndex (i As Integer, count As Integer)
  989.     arrayNextIndex = ((i + 1) Mod (count + 1))
  990.  
  991. Sub renderBodies (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY)
  992.     Dim i As Integer
  993.     For i = 0 To sNUMBEROFBODIES
  994.         'TODO: Put hidden object logic
  995.         If body(i).shape.ty = cSHAPE_CIRCLE Then
  996.             Call renderCircle(body(), i)
  997.         Else If body(i).shape.ty = cSHAPE_POLYGON Then
  998.                 Call renderPoly(p(), pa(), body(), i)
  999.             End If
  1000.         End If
  1001.     Next
  1002.  
  1003. Sub renderPoly (p() As tPOLY, pa() As tPOLYATTRIB, b() As tBODY, index As Integer)
  1004.     Dim a As tVECTOR2d ' dummy vertices
  1005.     Dim b As tVECTOR2d
  1006.  
  1007.     Dim i, element, element_next As Integer
  1008.     For i = 0 To pa(index).count
  1009.         element = pa(index).start + i
  1010.         element_next = pa(index).start + arrayNextIndex(i, pa(index).count) ' wrap around back to first element
  1011.         a = p(element).vert
  1012.         b = p(element_next).vert
  1013.  
  1014.         Call matrixMultiplyVector(b(index).shape.u, a, a)
  1015.         Call matrixMultiplyVector(b(index).shape.u, b, b)
  1016.         Call vectorAddVector(a, b(index).position)
  1017.         Call vectorAddVector(b, b(index).position)
  1018.  
  1019.         Line (a.x, a.y)-(b.x, b.y), b(index).c
  1020.     Next
  1021.     If b(index).position.x < 0 Then b(index).position.x = _Width
  1022.     If b(index).position.x > _Width Then b(index).position.x = 0
  1023.     ' If b(index).position.y < 0 Then b(index).position.y = _Height
  1024.     If b(index).position.y > _Height Then b(index).position.y = 0
  1025.  
  1026. Sub renderCircle (b() As tBODY, index As Integer)
  1027.     Circle (b(index).position.x, b(index).position.y), b(index).shape.radius, b(index).c
  1028.     Line (b(index).position.x, b(index).position.y)-(b(index).position.x + Cos(b(index).orient) * b(index).shape.radius, b(index).position.y + Sin(b(index).orient) * b(index).shape.radius), b(index).c
  1029.     If b(index).position.x < 0 Then b(index).position.x = _Width
  1030.     If b(index).position.x > _Width Then b(index).position.x = 0
  1031.     ' If b(index).position.y < 0 Then b(index).position.y = _Height
  1032.     If b(index).position.y > _Height Then b(index).position.y = 0
  1033.  
  1034. Sub polygonSetOrient (b As tBODY, radians As _Float)
  1035.     Call matrixSetRadians(b.shape.u, radians)
  1036.  
  1037. '**********************************************************************************************
  1038. '   Object initialization Ahead
  1039. '**********************************************************************************************
  1040.  
  1041. Sub circleInitialize (b() As tBODY, index As Integer)
  1042.     Call circleComputeMass(b(), index, 1.0)
  1043.  
  1044. Sub circleComputeMass (b() As tBODY, index As Integer, density As _Float)
  1045.     b(index).mass = cPI * b(index).shape.radius * b(index).shape.radius * density
  1046.     If b(index).mass <> 0 Then
  1047.         b(index).invMass = 1.0 / b(index).mass
  1048.     Else
  1049.         b(index).invMass = 0.0
  1050.     End If
  1051.  
  1052.     b(index).inertia = b(index).mass * b(index).shape.radius * b(index).shape.radius
  1053.  
  1054.     If b(index).inertia <> 0 Then
  1055.         b(index).invInertia = 1.0 / b(index).inertia
  1056.     Else
  1057.         b(index).invInertia = 0.0
  1058.     End If
  1059.  
  1060. Sub polygonInitialize (body() As tBODY, p() As tPOLY, pa() As tPOLYATTRIB, index As Integer)
  1061.     Call polygonComputeMass(body(), p(), pa(), index, 1.0)
  1062.  
  1063. Sub polygonComputeMass (b() As tBODY, p() As tPOLY, pa() As tPOLYATTRIB, index As Integer, density As _Float)
  1064.     Dim c As tVECTOR2d ' centroid
  1065.     Dim p1 As tVECTOR2d
  1066.     Dim p2 As tVECTOR2d
  1067.     Dim area As _Float
  1068.     Dim I As _Float
  1069.     Dim k_inv3 As _Float
  1070.     Dim D As _Float
  1071.     Dim triangleArea As _Float
  1072.     Dim weight As _Float
  1073.     Dim intx2 As _Float
  1074.     Dim inty2 As _Float
  1075.     Dim ii As Integer
  1076.  
  1077.     k_inv3 = 1.0 / 3.0
  1078.  
  1079.     For ii = 0 To pa(index).count
  1080.         p1 = p(pa(index).start + ii).vert
  1081.         p2 = p(pa(index).start + arrayNextIndex(ii, pa(index).count)).vert
  1082.         D = vectorCross(p1, p2)
  1083.         triangleArea = .5 * D
  1084.         area = area + triangleArea
  1085.         weight = triangleArea * k_inv3
  1086.         Call vectorAddVectorScalar(c, p1, weight)
  1087.         Call vectorAddVectorScalar(c, p2, weight)
  1088.         intx2 = p1.x * p1.x + p2.x * p1.x + p2.x * p2.x
  1089.         inty2 = p1.y * p1.y + p2.y * p1.y + p2.y * p2.y
  1090.         I = I + (0.25 * k_inv3 * D) * (intx2 + inty2)
  1091.     Next ii
  1092.  
  1093.     Call vectorMultiplyScalar(c, 1.0 / area)
  1094.  
  1095.     For ii = 0 To pa(index).count
  1096.         Call vectorSubVector(p(pa(index).start + ii).vert, c)
  1097.     Next
  1098.  
  1099.     b(index).mass = density * area
  1100.     If b(index).mass <> 0.0 Then
  1101.         b(index).invMass = 1.0 / b(index).mass
  1102.     Else
  1103.         b(index).invMass = 0.0
  1104.     End If
  1105.  
  1106.     b(index).inertia = I * density
  1107.     If b(index).inertia <> 0 Then
  1108.         b(index).invInertia = 1.0 / b(index).inertia
  1109.     Else
  1110.         b(index).invInertia = 0.0
  1111.     End If
  1112.  
  1113. '**********************************************************************************************
  1114. '   Vector Math Ahead
  1115. '**********************************************************************************************
  1116.  
  1117. Sub vectorSet (v As tVECTOR2d, x As _Float, y As _Float)
  1118.     v.x = x
  1119.     v.y = y
  1120.  
  1121. Sub vectorSetVector (o As tVECTOR2d, v As tVECTOR2d)
  1122.     o.x = v.x
  1123.     o.y = v.y
  1124.  
  1125. Sub vectorNeg (v As tVECTOR2d)
  1126.     v.x = -v.x
  1127.     v.y = -v.y
  1128.  
  1129. Sub vectorNegND (o As tVECTOR2d, v As tVECTOR2d)
  1130.     o.x = -v.x
  1131.     o.y = -v.y
  1132.  
  1133. Sub vectorMultiplyScalar (v As tVECTOR2d, s As _Float)
  1134.     v.x = v.x * s
  1135.     v.y = v.y * s
  1136.  
  1137. Sub vectorMultiplyScalarND (o As tVECTOR2d, v As tVECTOR2d, s As _Float)
  1138.     o.x = v.x * s
  1139.     o.y = v.y * s
  1140.  
  1141. Sub vectorDivideScalar (v As tVECTOR2d, s As _Float)
  1142.     v.x = v.x / s
  1143.     v.y = v.y / s
  1144.  
  1145. Sub vectorDivideScalarND (o As tVECTOR2d, v As tVECTOR2d, s As _Float)
  1146.     o.x = v.x / s
  1147.     o.y = v.y / s
  1148.  
  1149. Sub vectorAddScalar (v As tVECTOR2d, s As _Float)
  1150.     v.x = v.x + s
  1151.     v.y = v.y + s
  1152.  
  1153. Sub vectorAddScalarND (o As tVECTOR2d, v As tVECTOR2d, s As _Float)
  1154.     o.x = v.x + s
  1155.     o.y = v.y + s
  1156.  
  1157. Sub vectorMultiplyVector (v As tVECTOR2d, m As tVECTOR2d)
  1158.     v.x = v.x * m.x
  1159.     v.y = v.y * m.y
  1160.  
  1161. Sub vectorMultiplyVectorND (o As tVECTOR2d, v As tVECTOR2d, m As tVECTOR2d)
  1162.     o.x = v.x * m.x
  1163.     o.y = v.y * m.y
  1164.  
  1165. Sub vectorDivideVector (v As tVECTOR2d, m As tVECTOR2d)
  1166.     v.x = v.x / m.x
  1167.     v.y = v.y / m.y
  1168.  
  1169. Sub vectorAddVector (v As tVECTOR2d, m As tVECTOR2d)
  1170.     v.x = v.x + m.x
  1171.     v.y = v.y + m.y
  1172.  
  1173. Sub vectorAddVectorND (o As tVECTOR2d, v As tVECTOR2d, m As tVECTOR2d)
  1174.     o.x = v.x + m.x
  1175.     o.y = v.y + m.y
  1176.  
  1177. Sub vectorAddVectorScalar (v As tVECTOR2d, m As tVECTOR2d, s As _Float)
  1178.     v.x = v.x + m.x * s
  1179.     v.y = v.y + m.y * s
  1180.  
  1181. Sub vectorAddVectorScalarND (o As tVECTOR2d, v As tVECTOR2d, m As tVECTOR2d, s As _Float)
  1182.     o.x = v.x + m.x * s
  1183.     o.y = v.y + m.y * s
  1184.  
  1185. Sub vectorSubVector (v As tVECTOR2d, m As tVECTOR2d)
  1186.     v.x = v.x - m.x
  1187.     v.y = v.y - m.y
  1188.  
  1189. Sub vectorSubVectorND (o As tVECTOR2d, v As tVECTOR2d, m As tVECTOR2d)
  1190.     o.x = v.x - m.x
  1191.     o.y = v.y - m.y
  1192.  
  1193. Function vectorLengthSq (v As tVECTOR2d)
  1194.     vectorLengthSq = v.x * v.x + v.y * v.y
  1195.  
  1196. Function vectorLength (v As tVECTOR2d)
  1197.     vectorLength = Sqr(vectorLengthSq(v))
  1198.  
  1199. Sub vectorRotate (v As tVECTOR2d, radians As _Float)
  1200.     Dim c, s, xp, yp As _Float
  1201.     c = Cos(radians)
  1202.     s = Sin(radians)
  1203.     xp = v.x * c - v.y * s
  1204.     yp = v.x * s + v.y * c
  1205.     v.x = xp
  1206.     v.y = yp
  1207.  
  1208. Sub vectorNormalize (v As tVECTOR2d)
  1209.     Dim lenSQ, invLen As _Float
  1210.     lenSQ = vectorLengthSq(v)
  1211.     If lenSQ > cEPSILON_SQ Then
  1212.         invLen = 1.0 / Sqr(lenSQ)
  1213.         v.x = v.x * invLen
  1214.         v.y = v.y * invLen
  1215.     End If
  1216.  
  1217. Sub vectorMin (a As tVECTOR2d, b As tVECTOR2d, o As tVECTOR2d)
  1218.     o.x = scalarMin(a.x, b.x)
  1219.     o.y = scalarMin(a.y, b.y)
  1220.  
  1221. Sub vectorMax (a As tVECTOR2d, b As tVECTOR2d, o As tVECTOR2d)
  1222.     o.x = scalarMax(a.x, b.x)
  1223.     o.y = scalarMax(a.y, b.y)
  1224.  
  1225. Function vectorDot (a As tVECTOR2d, b As tVECTOR2d)
  1226.     vectorDot = a.x * b.x + a.y * b.y
  1227.  
  1228. Function vectorSqDist (a As tVECTOR2d, b As tVECTOR2d)
  1229.     Dim dx, dy As _Float
  1230.     dx = b.x - a.x
  1231.     dy = b.y - a.y
  1232.     vectorSqDist = dx * dx + dy * dy
  1233.  
  1234. Function vectorDistance (a As tVECTOR2d, b As tVECTOR2d)
  1235.     vectorDistance = Sqr(vectorSqDist(a, b))
  1236.  
  1237. Function vectorCross (a As tVECTOR2d, b As tVECTOR2d)
  1238.     vectorCross = a.x * b.y - a.y * b.x
  1239.  
  1240. Sub vectorCrossScalar (o As tVECTOR2d, v As tVECTOR2d, a As _Float)
  1241.     o.x = v.y * -a
  1242.     o.y = v.x * a
  1243.  
  1244. Function vectorArea (a As tVECTOR2d, b As tVECTOR2d, c As tVECTOR2d)
  1245.     vectorArea = (((b.x - a.x) * (c.y - a.y)) - ((c.x - a.x) * (b.y - a.y)))
  1246.  
  1247. Function vectorLeft (a As tVECTOR2d, b As tVECTOR2d, c As tVECTOR2d)
  1248.     vectorLeft = vectorArea(a, b, c) > 0
  1249.  
  1250. Function vectorLeftOn (a As tVECTOR2d, b As tVECTOR2d, c As tVECTOR2d)
  1251.     vectorLeftOn = vectorArea(a, b, c) >= 0
  1252.  
  1253. Function vectorRight (a As tVECTOR2d, b As tVECTOR2d, c As tVECTOR2d)
  1254.     vectorRight = vectorArea(a, b, c) < 0
  1255.  
  1256. Function vectorRightOn (a As tVECTOR2d, b As tVECTOR2d, c As tVECTOR2d)
  1257.     vectorRightOn = vectorArea(a, b, c) <= 0
  1258.  
  1259. Function vectorCollinear (a As tVECTOR2d, b As tVECTOR2d, c As tVECTOR2d, thresholdAngle As _Float)
  1260.     If (thresholdAngle = 0) Then
  1261.         vectorCollinear = (vectorArea(a, b, c) = 0)
  1262.     Else
  1263.         Dim ab As tVECTOR2d
  1264.         Dim bc As tVECTOR2d
  1265.         Dim dot As _Float
  1266.         Dim magA As _Float
  1267.         Dim magB As _Float
  1268.         Dim angle As _Float
  1269.  
  1270.         ab.x = b.x - a.x
  1271.         ab.y = b.y - a.y
  1272.         bc.x = c.x - b.x
  1273.         bc.y = c.y - b.y
  1274.  
  1275.         dot = ab.x * bc.x + ab.y * bc.y
  1276.         magA = Sqr(ab.x * ab.x + ab.y * ab.y)
  1277.         magB = Sqr(bc.x * bc.x + bc.y * bc.y)
  1278.         angle = _Acos(dot / (magA * magB))
  1279.         vectorCollinear = angle < thresholdAngle
  1280.     End If
  1281.  
  1282. Sub vectorGetSupport (p() As tPOLY, pa() As tPOLYATTRIB, index As Integer, dir As tVECTOR2d, bestVertex As tVECTOR2d)
  1283.     Dim bestProjection As _Float
  1284.     Dim v As tVECTOR2d
  1285.     Dim projection As _Float
  1286.     Dim i As Integer
  1287.     bestVertex.x = -9999999
  1288.     bestVertex.y = -9999999
  1289.     bestProjection = -9999999
  1290.  
  1291.     For i = 0 To pa(index).count
  1292.         v = p(i + pa(index).start).vert
  1293.         projection = vectorDot(v, dir)
  1294.         If projection > bestProjection Then
  1295.             bestVertex = v
  1296.             bestProjection = projection
  1297.         End If
  1298.     Next
  1299.  
  1300. '**********************************************************************************************
  1301. '   Matrix Stuff Ahead
  1302. '**********************************************************************************************
  1303.  
  1304. Sub matrixSetRadians (m As tMATRIX2d, radians As _Float)
  1305.     Dim c As _Float
  1306.     Dim s As _Float
  1307.     c = Cos(radians)
  1308.     s = Sin(radians)
  1309.     m.m00 = c
  1310.     m.m01 = -s
  1311.     m.m10 = s
  1312.     m.m11 = c
  1313.  
  1314. Sub matrixSetScalar (m As tMATRIX2d, a As _Float, b As _Float, c As _Float, d As _Float)
  1315.     m.m00 = a
  1316.     m.m01 = b
  1317.     m.m10 = c
  1318.     m.m11 = d
  1319.  
  1320. Sub matrixAbs (m As tMATRIX2d, o As tMATRIX2d)
  1321.     o.m00 = Abs(m.m00)
  1322.     o.m01 = Abs(m.m01)
  1323.     o.m10 = Abs(m.m10)
  1324.     o.m11 = Abs(m.m11)
  1325.  
  1326. Sub matrixGetAxisX (m As tMATRIX2d, o As tVECTOR2d)
  1327.     o.x = m.m00
  1328.     o.y = m.m10
  1329.  
  1330. Sub matrixGetAxisY (m As tMATRIX2d, o As tVECTOR2d)
  1331.     o.x = m.m01
  1332.     o.y = m.m11
  1333.  
  1334. Sub matrixTransposeI (m As tMATRIX2d)
  1335.     Swap m.m01, m.m10
  1336.  
  1337. Sub matrixTranspose (m As tMATRIX2d, o As tMATRIX2d)
  1338.     o.m00 = m.m00
  1339.     o.m01 = m.m10
  1340.     o.m10 = m.m01
  1341.     o.m11 = m.m11
  1342.  
  1343. Sub matrixMultiplyVector (m As tMATRIX2d, v As tVECTOR2d, o As tVECTOR2d)
  1344.     Dim t As tVECTOR2d
  1345.     t.x = m.m00 * v.x + m.m01 * v.y
  1346.     t.y = m.m10 * v.x + m.m11 * v.y
  1347.     o = t
  1348.  
  1349. Sub matrixMultiplyMatrix (m As tMATRIX2d, x As tMATRIX2d, o As tMATRIX2d)
  1350.     o.m00 = m.m00 * x.m00 + m.m01 * x.m10
  1351.     o.m01 = m.m00 * x.m01 + m.m01 * x.m11
  1352.     o.m10 = m.m10 * x.m00 + m.m11 * x.m10
  1353.     o.m11 = m.m10 * x.m01 + m.m11 * x.m11
  1354.  
  1355. '**********************************************************************************************
  1356. '   Mostly Unused Stuff Ahead
  1357. '**********************************************************************************************
  1358.  
  1359. Sub polygonMakeCCW (obj As tTRIANGLE)
  1360.     If vectorLeft(obj.a, obj.b, obj.c) = 0 Then
  1361.         Swap obj.a, obj.c
  1362.     End If
  1363.  
  1364. Function polygonIsReflex (t As tTRIANGLE)
  1365.     polygonIsReflex = vectorRight(t.a, t.b, t.c)
  1366.  
  1367. Function scalarMin (a As _Float, b As _Float)
  1368.     If a < b Then
  1369.         scalarMin = a
  1370.     Else
  1371.         scalarMin = b
  1372.     End If
  1373.  
  1374. Function scalarMax (a As _Float, b As _Float)
  1375.     If a > b Then
  1376.         scalarMax = a
  1377.     Else
  1378.         scalarMax = b
  1379.     End If
  1380.  
  1381. Sub lineIntersection (l1 As tLINE2d, l2 As tLINE2d, o As tVECTOR2d)
  1382.     Dim a1, b1, c1, a2, b2, c2, det As _Float
  1383.     o.x = 0
  1384.     o.y = 0
  1385.     a1 = l1.b.y - l1.a.y
  1386.     b1 = l1.a.x - l1.b.x
  1387.     c1 = a1 * l1.a.x + b1 * l1.a.y
  1388.     a2 = l2.b.y - l2.a.y
  1389.     b2 = l2.a.x - l2.b.x
  1390.     c2 = a2 * l2.a.x + b2 * l2.a.y
  1391.     det = a1 * b2 - a2 * b1
  1392.  
  1393.     If Int(det * cPRECISION) <> 0 Then
  1394.         o.x = (b2 * c1 - b1 * c2) / det
  1395.         o.y = (a1 * c2 - a2 * c1) / det
  1396.     End If
  1397.  
  1398. Function lineSegmentsIntersect (l1 As tLINE2d, l2 As tLINE2d)
  1399.     Dim dx, dy, da, db, s, t As _Float
  1400.     dx = l1.b.x - l1.a.x
  1401.     dy = l1.b.y - l1.a.y
  1402.     da = l2.b.x - l2.a.x
  1403.     db = l2.b.y - l2.a.y
  1404.     If da * dy - db * dx = 0 Then
  1405.         lineSegmentsIntersect = 0
  1406.     Else
  1407.         s = (dx * (l2.a.y - l1.a.y) + dy * (l1.a.x - l2.a.x)) / (da * dy - db * dx)
  1408.         t = (da * (l1.a.y - l2.a.y) + db * (l2.a.x - l1.a.x)) / (db * dx - da * dy)
  1409.         lineSegmentsIntersect = (s >= 0 And s <= 1 And t >= 0 And t <= 1)
  1410.     End If
  1411.  
  1412. '**********************************************************************************************
  1413. '   Impulse Specific Math Ahead
  1414. '**********************************************************************************************
  1415.  
  1416. Function impulseEqual (a As _Float, b As _Float)
  1417.     impulseEqual = Abs(a - b) <= cEPSILON
  1418.  
  1419. Function impulseClamp (min As _Float, max As _Float, a As _Float)
  1420.     If a < min Then
  1421.         impulseClamp = min
  1422.     Else If a > max Then
  1423.             impulseClamp = max
  1424.         Else
  1425.             impulseClamp = a
  1426.         End If
  1427.     End If
  1428.  
  1429. Function impulseRound (a As _Float)
  1430.     impulseRound = Int(a + 0.5)
  1431.  
  1432. Function impulseRandom_float (min As _Float, max As _Float)
  1433.     impulseRandom_float = ((max - min) * Rnd + min)
  1434.  
  1435. Function impulseRandomInteger (min As Integer, max As Integer)
  1436.     impulseRandomInteger = Int((max - min) * Rnd + min)
  1437.  
  1438. Function impulseGT (a As _Float, b As _Float)
  1439.     impulseGT = (a >= b * cBIAS_RELATIVE + a * cBIAS_ABSOLUTE)
  1440.  
  1441. '**********************************************************************************************
  1442. '   Troubleshooting Tools
  1443. '**********************************************************************************************
  1444.  
  1445.  
  1446. Sub printMatrix (u As tMATRIX2d, n As Integer)
  1447.     Print "---------------------------"
  1448.     Print n; " u:"; u.m00; "|"; u.m10
  1449.     Print "       "; u.m10; "|"; u.m11
  1450.  
  1451.  

Offline NOVARSEG

  • Seasoned Forum Regular
  • Posts: 391
Re: 2D Physics Engine
« Reply #1 on: April 21, 2021, 03:51:25 AM »
looking at code
« Last Edit: April 21, 2021, 03:53:29 AM by NOVARSEG »

Offline bplus

  • Forum Resident
  • Posts: 6700
  • b = b + ...
Re: 2D Physics Engine
« Reply #2 on: April 21, 2021, 01:48:15 PM »
Well it is nice demo!

Welcome @justsomeguy

Offline justsomeguy

  • Newbie
  • Posts: 38
Re: 2D Physics Engine
« Reply #3 on: April 21, 2021, 01:58:08 PM »
Thank you for the feedback!

Offline Dimster

  • Seasoned Forum Regular
  • Posts: 486
Re: 2D Physics Engine
« Reply #4 on: April 21, 2021, 02:34:48 PM »
That's pretty neat - maybe a new engine for a lottery machine. Seems to be a possible recursive pattern to the moving objects but getting hypnotized trying to see it.

Offline justsomeguy

  • Newbie
  • Posts: 38
Re: 2D Physics Engine
« Reply #5 on: April 21, 2021, 04:29:15 PM »
Hello

I have added textures to the objects and and added a FPS counter. I would be interested in what kind of performance that everyone is getting. I will appreciate any feedback good or bad.

Thanks

Code: QB64: [Select]
  1. '/*
  2. '    Copyright (c) 2013 Randy Gaul http://RandyGaul.net
  3.  
  4. '    This software is provided 'as-is', without any express or implied
  5. '    warranty. In no event will the authors be held liable for any damages
  6. '    arising from the use of this software.
  7.  
  8. '    Permission is granted to anyone to use this software for any purpose,
  9. '    including commercial applications, and to alter it and redistribute it
  10. '    freely, subject to the following restrictions:
  11. '      1. The origin of this software must not be misrepresented; you must not
  12. '         claim that you wrote the original software. If you use this software
  13. '         in a product, an acknowledgment in the product documentation would be
  14. '         appreciated but is not required.
  15. '      2. Altered source versions must be plainly marked as such, and must not be
  16. '         misrepresented as being the original software.
  17. '      3. This notice may not be removed or altered from any source distribution.
  18.  
  19. '    Port to Java by Philip Diffenderfer http://magnos.org
  20. '    Port to QB64 by justsomeguy
  21. '*/
  22. '
  23.  
  24. '**********************************************************************************************
  25. '   Setup Types and Variables
  26. '**********************************************************************************************
  27. Type tVECTOR2d
  28.     x As _Float
  29.     y As _Float
  30.  
  31. Type tLINE2d ' Not used
  32.     a As tVECTOR2d
  33.     b As tVECTOR2d
  34.  
  35. Type tFACE2d ' Not used
  36.     f0 As tVECTOR2d
  37.     f1 As tVECTOR2d
  38.  
  39. Type tTRIANGLE ' Not used
  40.     a As tVECTOR2d
  41.     b As tVECTOR2d
  42.     c As tVECTOR2d
  43.  
  44. Type tMATRIX2d
  45.     m00 As _Float
  46.     m01 As _Float
  47.     m10 As _Float
  48.     m11 As _Float
  49.  
  50. Type tSHAPE
  51.     ty As Integer ' cSHAPE_CIRCLE = 1, cSHAPE_POLYGON = 2
  52.     radius As _Float ' Only necessary for circle shapes
  53.     u As tMATRIX2d ' Only neceassary for polygons
  54.     texture As Long
  55.  
  56. Type tPOLY 'list of vertices for all objects in simulation
  57.     vert As tVECTOR2d
  58.     norm As tVECTOR2d
  59.  
  60. Type tPOLYATTRIB 'keep track of polys in monlithic list of vertices
  61.     start As Integer ' starting vertex of the polygon
  62.     count As Integer ' number of vertices in polygon
  63.  
  64. Type tBODY
  65.     position As tVECTOR2d
  66.     velocity As tVECTOR2d
  67.     force As tVECTOR2d
  68.     angularVelocity As _Float
  69.     torque As _Float
  70.     orient As _Float
  71.     mass As _Float
  72.     invMass As _Float
  73.     inertia As _Float
  74.     invInertia As _Float
  75.     staticFriction As _Float
  76.     dynamicFriction As _Float
  77.     restitution As _Float
  78.     shape As tSHAPE
  79.     c As Long ' color
  80.     visible As Integer 'Hide a body ;)
  81.  
  82. Type tMANIFOLD
  83.     A As Integer
  84.     B As Integer
  85.     penetration As _Float
  86.     normal As tVECTOR2d
  87.     contactCount As Integer
  88.     e As _Float
  89.     df As _Float
  90.     sf As _Float
  91.  
  92. Const cSHAPE_CIRCLE = 1
  93. Const cSHAPE_POLYGON = 2
  94. Const cPRECISION = 100
  95. Const cMAXNUMOFTRIANGLES = 100
  96. Const cMAXNUMBEROFOBJECTS = 1000 ' Max number of objects at one time
  97. Const cMAXNUMBEROFPOLYGONS = 10000 ' Max number of total vertices included in all objects
  98. Const cPI = 3.14159
  99. Const cEPSILON = 0.0001
  100. Const cEPSILON_SQ = cEPSILON * cEPSILON
  101. Const cBIAS_RELATIVE = 0.95
  102. Const cBIAS_ABSOLUTE = 0.01
  103. Const cDT = 1.0 / 60.0
  104. Const cITERATIONS = 10
  105. Const cPENETRATION_ALLOWANCE = 0.05
  106. Const cPENETRATION_CORRECTION = 0.4 ' misspelled in original code
  107. Const cPARAMETER_POSITION = 1
  108. Const cPARAMETER_VELOCITY = 2
  109. Const cPARAMETER_FORCE = 3
  110. Const cPARAMETER_ANGULARVELOCITY = 4
  111. Const cPARAMETER_TORQUE = 5
  112. Const cPARAMETER_ORIENT = 6
  113. Const cPARAMETER_STATICFRICTION = 7
  114. Const cPARAMETER_DYNAMICFRICTION = 8
  115. Const cPARAMETER_COLOR = 9
  116. Const cPARAMETER_VISIBLE = 10
  117. Const cPARAMETER_STATIC = 11
  118. Const cPARAMETER_TEXTURE = 12
  119.  
  120. Dim Shared sGRAVITY As tVECTOR2d: Call vectorSet(sGRAVITY, 0.0, 100.0)
  121. Dim Shared sRESTING As _Float: Dim o As tVECTOR2d: Call vectorMultiplyScalarND(o, sGRAVITY, cDT): sRESTING = vectorLengthSq(o) + cEPSILON
  122. Dim Shared sNUMBEROFBODIES As Integer: sNUMBEROFBODIES = 100 ' 0 is included - Don't go under 10 for the current scene
  123.  
  124. Dim poly(cMAXNUMBEROFPOLYGONS) As tPOLY
  125. Dim polyattributes(cMAXNUMBEROFPOLYGONS) As tPOLYATTRIB
  126. Dim body(cMAXNUMBEROFOBJECTS) As tBODY
  127.  
  128. Const cSCENE_USER = 0
  129. Const cSCENE_FLOOR = 1
  130. Const cSCENE_SPINNEROFDEATH = 2
  131. Const cSCENE_RIGHTRAMP = 3
  132. Const cSCENE_LEFTRAMP = 4
  133. Const cSCENE_DOZEROFDOOM = 5
  134. Const cSCENE_ELEVATOROFTERROR = 6
  135. Const cSCENE_RIGHTWALL = 7
  136. Const cSCENE_ELEVATORKICKER = 8
  137.  
  138. '**********************************************************************************************
  139. _Title "phyNGN"
  140. Screen _NewImage(1024, 768, 32)
  141. 'Randomize Timer
  142. Call buildSimpleScene(poly(), polyattributes(), body())
  143. '**********************************************************************************************
  144. Do: fps = Timer(.001)
  145.     Cls , _RGB32(28, 28, 22)
  146.     Call animateScene(body())
  147.     Call impulseStep(poly(), polyattributes(), body(), cDT, cITERATIONS)
  148.     Call renderBodies(poly(), polyattributes(), body())
  149.     Locate 1, 60: Print "FPS:"; Int(1 / (Timer(.001) - fps))
  150.     _Display
  151.     _Limit 240
  152. '**********************************************************************************************
  153. '   End of Main loop
  154. '**********************************************************************************************
  155.  
  156. '**********************************************************************************************
  157. '   Scene Creation and Handling Ahead
  158. '**********************************************************************************************
  159.  
  160. Sub animateScene (body() As tBODY)
  161.     If _MouseInput Then ' Terrible user interaction
  162.         Call vectorSet(body(cSCENE_USER).velocity, _MouseMovementX * 100, _MouseMovementY * 100)
  163.     End If
  164.  
  165.     Call setBody(body(), cPARAMETER_ORIENT, cSCENE_SPINNEROFDEATH, body(cSCENE_SPINNEROFDEATH).orient + (cPI / 90), 0)
  166.     If body(cSCENE_SPINNEROFDEATH).orient > 2 * cPI Then body(cSCENE_SPINNEROFDEATH).orient = 0
  167.  
  168.     Call setBody(body(), cPARAMETER_POSITION, cSCENE_DOZEROFDOOM, body(cSCENE_DOZEROFDOOM).position.x - 1, _Height - 75)
  169.     If body(cSCENE_DOZEROFDOOM).position.x < 120 Then body(cSCENE_DOZEROFDOOM).position.x = _Width
  170.  
  171.     Call setBody(body(), cPARAMETER_POSITION, cSCENE_ELEVATOROFTERROR, 55, body(cSCENE_ELEVATOROFTERROR).position.y - 1)
  172.     If body(cSCENE_ELEVATOROFTERROR).position.y < 50 Then body(cSCENE_ELEVATOROFTERROR).position.y = _Height - 25
  173.  
  174. Sub buildSimpleScene (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY)
  175.     Dim i, ty As Integer
  176.     Dim bm(5) As Long
  177.     Call generateBitmap(bm())
  178.  
  179.     Call createBoxBodies(p(), pa(), body(), cSCENE_USER, 10, 10)
  180.     Call setBody(body(), cPARAMETER_POSITION, cSCENE_USER, (_Width / 2), (_Height / 2))
  181.     Call setBody(body(), cPARAMETER_VELOCITY, cSCENE_USER, 0, 0)
  182.  
  183.     Call createBoxBodies(p(), pa(), body(), cSCENE_FLOOR, 1000, 5)
  184.     Call setBody(body(), cPARAMETER_POSITION, cSCENE_FLOOR, (_Width / 2), (_Height - 20))
  185.     Call setBody(body(), cPARAMETER_STATIC, cSCENE_FLOOR, 0, 0)
  186.  
  187.     Call createBoxBodies(p(), pa(), body(), cSCENE_SPINNEROFDEATH, 75, 5)
  188.     Call setBody(body(), cPARAMETER_POSITION, cSCENE_SPINNEROFDEATH, (_Width / 2), (_Height - 100))
  189.     Call setBody(body(), cPARAMETER_STATIC, cSCENE_SPINNEROFDEATH, 0, 0)
  190.  
  191.     Call createBoxBodies(p(), pa(), body(), cSCENE_RIGHTRAMP, 350, 5)
  192.     Call setBody(body(), cPARAMETER_POSITION, cSCENE_RIGHTRAMP, (_Width * .75) + 35, (_Height / 2) + 105)
  193.     Call setBody(body(), cPARAMETER_ORIENT, cSCENE_RIGHTRAMP, (cPI * .75), 0)
  194.     Call setBody(body(), cPARAMETER_STATIC, cSCENE_RIGHTRAMP, 0, 0)
  195.  
  196.     Call createBoxBodies(p(), pa(), body(), cSCENE_LEFTRAMP, 225, 5)
  197.     Call setBody(body(), cPARAMETER_POSITION, cSCENE_LEFTRAMP, (_Width * .25) + 55, (_Height / 2))
  198.     Call setBody(body(), cPARAMETER_ORIENT, cSCENE_LEFTRAMP, (cPI * .1), 0)
  199.     Call setBody(body(), cPARAMETER_STATIC, cSCENE_LEFTRAMP, 0, 0)
  200.  
  201.     Call createBoxBodies(p(), pa(), body(), cSCENE_DOZEROFDOOM, 5, 50)
  202.     Call setBody(body(), cPARAMETER_POSITION, cSCENE_DOZEROFDOOM, 0, _Height - 75)
  203.     Call setBody(body(), cPARAMETER_STATIC, cSCENE_DOZEROFDOOM, 0, 0)
  204.  
  205.     Call createBoxBodies(p(), pa(), body(), cSCENE_ELEVATOROFTERROR, 50, 5)
  206.     Call setBody(body(), cPARAMETER_POSITION, cSCENE_ELEVATOROFTERROR, 55, _Height - 25)
  207.     Call setBody(body(), cPARAMETER_ORIENT, cSCENE_ELEVATOROFTERROR, (cPI * .98), 0)
  208.     Call setBody(body(), cPARAMETER_STATIC, cSCENE_ELEVATOROFTERROR, 0, 0)
  209.  
  210.     Call createBoxBodies(p(), pa(), body(), cSCENE_RIGHTWALL, 5, 375)
  211.     Call setBody(body(), cPARAMETER_POSITION, cSCENE_RIGHTWALL, 5, _Height / 2 - 20)
  212.     Call setBody(body(), cPARAMETER_STATIC, cSCENE_RIGHTWALL, 0, 0)
  213.  
  214.     Call createBoxBodies(p(), pa(), body(), cSCENE_ELEVATORKICKER, 100, 5)
  215.     Call setBody(body(), cPARAMETER_POSITION, cSCENE_ELEVATORKICKER, 55, 120)
  216.     Call setBody(body(), cPARAMETER_ORIENT, cSCENE_ELEVATORKICKER, (cPI * .70), 0)
  217.     Call setBody(body(), cPARAMETER_STATIC, cSCENE_ELEVATORKICKER, 0, 0)
  218.  
  219.     For i = cSCENE_ELEVATORKICKER + 1 To sNUMBEROFBODIES
  220.         If Rnd > .25 Then ' Circles are must faster than polygons
  221.             ty = cSHAPE_CIRCLE
  222.         Else
  223.             ty = cSHAPE_POLYGON
  224.         End If
  225.         Call createBodies(p(), pa(), body(), i, ty)
  226.         Call setBody(body(), cPARAMETER_POSITION, i, (Rnd * _Width / 2) + (_Width / 4), ((Rnd * _Height / 2) + (_Height / 4)) - 200)
  227.         Call setBody(body(), cPARAMETER_VELOCITY, i, Rnd * 100 - 50, Rnd * 100 - 50)
  228.     Next
  229.     For i = cSCENE_ELEVATORKICKER + 1 To sNUMBEROFBODIES
  230.         If body(i).shape.ty = cSHAPE_CIRCLE Then
  231.             Call setBody(body(), cPARAMETER_TEXTURE, i, bm(0), 0)
  232.         Else
  233.             Call setBody(body(), cPARAMETER_TEXTURE, i, bm(1), 0)
  234.         End If
  235.     Next
  236.  
  237. Sub setBody (body() As tBODY, Parameter As Integer, Index As Integer, arg1 As _Float, arg2 As _Float)
  238.     Select Case Parameter
  239.         Case cPARAMETER_POSITION:
  240.             Call vectorSet(body(Index).position, arg1, arg2)
  241.         Case cPARAMETER_VELOCITY:
  242.             Call vectorSet(body(Index).velocity, arg1, arg2)
  243.         Case cPARAMETER_FORCE:
  244.             Call vectorSet(body(Index).force, arg1, arg2)
  245.         Case cPARAMETER_ANGULARVELOCITY:
  246.             body(Index).angularVelocity = arg1
  247.         Case cPARAMETER_TORQUE:
  248.             body(Index).torque = arg1
  249.         Case cPARAMETER_ORIENT:
  250.             body(Index).orient = arg1
  251.             Call matrixSetRadians(body(Index).shape.u, body(Index).orient)
  252.         Case cPARAMETER_STATICFRICTION:
  253.             body(Index).staticFriction = arg1
  254.         Case cPARAMETER_DYNAMICFRICTION:
  255.             body(Index).dynamicFriction = arg1
  256.         Case cPARAMETER_COLOR:
  257.             body(Index).c = arg1
  258.         Case cPARAMETER_VISIBLE:
  259.             body(Index).visible = arg1
  260.         Case cPARAMETER_STATIC:
  261.             Call bodySetStatic(body(Index))
  262.         Case cPARAMETER_TEXTURE:
  263.             body(Index).shape.texture = arg1
  264.     End Select
  265.  
  266. Sub createCircleBody (pa() As tPOLYATTRIB, body() As tBODY, index As Integer, radius As _Float)
  267.     Dim shape As tSHAPE
  268.     Call shapeCreate(shape, cSHAPE_CIRCLE, radius)
  269.     Call bodyCreate(body(), index, shape)
  270.     Call circleInitialize(body(), index)
  271.     ' Even though circles do not have Polygons, they still must be included in the Vertices
  272.     If index = 0 Then
  273.         pa(index).start = 0
  274.     Else
  275.         pa(index).start = pa(index - 1).start + pa(index - 1).count + 1
  276.     End If
  277.     pa(index).count = 1
  278.     body(index).c = _RGB32(255, 255, 255)
  279.  
  280. Sub createBoxBodies (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY, index As Integer, xs As _Float, ys As _Float)
  281.     Dim shape As tSHAPE
  282.     Call shapeCreate(shape, cSHAPE_POLYGON, 0)
  283.     Call bodyCreate(body(), index, shape)
  284.     Call boxCreate(p(), pa(), index, xs, ys)
  285.     Call polygonInitialize(body(), p(), pa(), index)
  286.     body(index).c = _RGB32(255, 255, 255)
  287.  
  288. Sub createBodies (p() As tPOLY, pa() As tPOLYATTRIB, body() As tBODY, index As Integer, ty As Integer)
  289.     If ty = cSHAPE_CIRCLE Then
  290.         Call createCircleBody(pa(), body(), index, 10 + Rnd * 20)
  291.     Else
  292.         If ty = cSHAPE_POLYGON Then
  293.             Call createBoxBodies(p(), pa(), body(), index, 10 + Rnd * 10, 10 + Rnd * 10)
  294.         End If
  295.     End If
  296.     body(index).c = _RGB32(55 + Rnd * 200, 55 + Rnd * 200, 55 + Rnd * 200)
  297.  
  298. '**********************************************************************************************
  299. '   Physics and Collision Stuff Ahead
  300. '**********************************************************************************************
  301.  
  302. Sub impulseIntegrateForces (b As tBODY, dt As _Float)
  303.     If b.invMass = 0.0 Then Exit Sub
  304.     Dim dts As _Float
  305.     dts = dt * .5
  306.     Call vectorAddVectorScalar(b.velocity, b.force, b.invMass * dts)
  307.     Call vectorAddVectorScalar(b.velocity, sGRAVITY, dts)
  308.     b.angularVelocity = b.angularVelocity + (b.torque * b.invInertia * dts)
  309.  
  310. Sub impulseIntegrateVelocity (body