Graphics3D 800,600,16,2
SetBuffer BackBuffer()

Include "Draw3D.bb"

Global Camera = CreateCamera()

DrawInit3D(Camera)

Global Nat = LoadImage3D("Nat.png")
Global Font= LoadFont3D("Font.png")

Global grav_y# = -0.4;0.981
Global grav_x# = 0
Global Looptime#=0
Global L_COL_X#,L_COL_Y#

v1.point = AddPoint(-100,-25)
v2.point = AddPoint(+100,-25)
v3.point = AddPoint(-100,+25)
v4.point = AddPoint(+100,+25)
AddCons(v1,v2)
AddCons(v1,v3)
AddCons(v2,v4)
AddCons(v3,v4)

PointStat(v1)
PointStat(v2)
PointStat(v3)
PointStat(v4)

Global del = 0


Repeat
   If MouseHit(1)
      AddPoint(MouseX3D,MouseY3D)
   EndIf
   If MouseHit(2)
      If Mode = 0
         near.point = GetNearestPoint(MouseX3D,MouseY3D)
         Mode = 1
      ElseIf Mode = 1
         near2.point = GetNearestPoint(MouseX3D,MouseY3D)
         If near2 <> near
            AddCons(near,near2)
            Mode = 0
         EndIf
      EndIf
   EndIf
   If KeyHit(17) And MoveIt = 0
      MoveIt = 1
      MoveP.point = GetNearestPoint(MouseX3D,MouseY3D)
      ;If MoveP\typ = 0 MoveP = Null : MoveIT = 0
   EndIf
   If MoveIt = 1
      If KeyDown(17) = 0 MoveIt = 0
      PositionPoint(MoveP,MouseX3D,MouseY3D)
   EndIf
   If Mode = 1
      Line3D(Nat,near\x,near\y,MouseX3D,MouseY3D)
   EndIf
   If MouseHit(3) Then Start = 1- Start
   UpdatePhysik(10,1,Start)
   DrawPoints(Nat)
   DrawCons(Nat)
   RenderWorld()
   Clear3D()
   Flip
Until KeyHit(1)
End

Type point
   Field x#,y#
   Field ox#,oy#
   Field sx#,sy#
   Field coltyp
   Field typ
End Type

Type cons
   Field p1.point,p2.point
   Field lenght#
End Type

Function AddPoint.point(x#,y#,coltyp=1)
   p.point = New point
   p\x = x
   p\y = y
   p\ox = x
   p\oy = y
   p\sx = grav_x
   p\sy = grav_y
   p\coltyp = coltyp
   p\typ = 1
   Return p
End Function

Function PositionPoint(p.point,x#,y#)
   p\x = x
   p\y = y
   p\ox = x
   p\oy = y
End Function

Function PointStat(p.point,typ=0)
   If typ < 0 typ = 0
   If typ > 1 typ = 1
   p\typ = typ
End Function

Function AddCons.cons(p1.point,p2.point)
   c.cons = New cons
   c\p1 = p1
   c\p2 = p2
   c\lenght = Sqr((c\p1\x-c\p2\x)*(c\p1\x-c\p2\x)+(c\p1\y-c\p2\y)*(c\p1\y-c\p2\y))
   If p1 = p2 RuntimeError("Can't connect the same Points !")
   If p1\coltyp <> p2\coltyp RuntimeError("This points can't connect (Different ColTypes) !")
   Return c
End Function


Function UpdatePhysik(genauigkeit=1,LoopTime#=1,start=1)
   If start < 0 start = 0
   If start > 1 start = 1
   For p.point = Each point
      If p\typ = 1
         If p\colon = 0
            OldX# = p\x
            OldY# = p\y
            p\x = p\x +( (p\x - p\ox) + p\sx * LoopTime*LoopTime ) * start
            p\y = p\y +( (p\y - p\oy) + p\sy * LoopTime*LoopTime ) * start
            p\ox = OldX
            p\oy = OldY
         EndIf
         Dx# = (p\x-p\ox)
         Dy# = (p\y-p\oy)
         VR# = Sqr(Dx*Dx + Dy*Dy)
         V# = ATan2(dy,dx)
         Line3D(Nat,p\x,p\y,p\x+Cos(V)*VR,p\y+Sin(V)*VR,0.5)
         For c.cons = Each cons
            If c\p1 <> p And c\p2 <> p
               If p\colon = 0            
                  Inter1 = Line_Intersect(c\p1\x,c\p1\y,c\p2\x,c\p2\y,p\x,p\y,p\x+Cos(V)*VR,p\y+Sin(V)*VR)
                  If Inter1 = 0
                     Inter2 = Line_Intersect(c\p2\x,c\p2\y,c\p1\x,c\p1\y,p\x,p\y,p\x+Cos(V)*VR,p\y+Sin(V)*VR)      
                  EndIf
                  If Inter1 Or Inter2
                     pdcx# = (L_COL_X-p\x)
                     pdcy# = (L_COL_Y-p\y)
                     p\x = p\x + pdcx
                     p\y = p\y + pdcy
                     p\ox = p\x
                     p\oy = p\y                     
                  EndIf
               Else
                  Inter1 = Line_Intersect(c\p1\x,c\p1\y,c\p2\x,c\p2\y,p\x,p\y,p\x+Cos(V)*VR,p\y+Sin(V)*VR)
                  If Inter1 = 0
                     Inter2 = Line_Intersect(c\p2\x,c\p2\y,c\p1\x,c\p1\y,p\x,p\y,p\x+Cos(V)*VR,p\y+Sin(V)*VR)      
                  EndIf
                  If Inter1 = 0 And Inter2 = 0
                     p\colon = 0
                  EndIf
               EndIf
            EndIf
         Next          
         If p\y < -300 p\y = -300
         If p\x > 400 p\x = 400
         If p\x < -400 p\x = -400
      EndIf
   Next
   For i = 1 To genauigkeit
      For c.cons = Each cons
         DeltaX# = (c\p2\x-c\p1\x)
         DeltaY# = (c\p2\y-c\p1\y)
         Delta# = Sqr(DeltaX*DeltaX+DeltaY*DeltaY)
         Diff# = (Delta - c\lenght) / Delta
         If c\p1\typ = 1
            c\p1\x = c\p1\x + DeltaX * Diff * 0.5         
            c\p1\y = c\p1\y + DeltaY * Diff * 0.5
         EndIf
         If c\p2\typ = 1
            c\p2\x = c\p2\x - DeltaX * Diff * 0.5
            c\p2\y = c\p2\y - DeltaY * Diff * 0.5
         EndIf
      Next
   Next
End Function

Function GetNearestPoint.point(x#,y#)
   Local MaxP# = 1000000
   Local near.point
   For p.point = Each point
      lo# =  Sqr((p\x-x)*(p\x-x)+(p\y-y)*(p\y-y))
      If lo < MaxP Then MaxP = lo : near = p
   Next
   Return near
End Function


Function DrawPoints(Texture=0)
   If Texture = 0 Return
   For p.point = Each point
      Plot3D(Texture,p\x,p\y)
   Next
End Function

Function DrawCons(Texture=0)
   If Texture = 0 Return
   For c.cons = Each cons
      Line3D(Texture,c\p1\x,c\p1\y,c\p2\x,c\p2\y)
   Next
End Function



Function Line_intersect(x1#,y1#,x2#,y2#, x3#,y3#,x4#,y4#)
   ;Steigung ausrechnen
   m1# = (y1#-y2#)/(x1#-x2#)
   m2# = (y3#-y4#)/(x3#-x4#)
   
   ;Y-Achsen Abschnitt ausrechnen
   b1# = y1# - m1#*x1#
   b2# = y3# - m2#*x3#
   
   
   ;Term umformen
   x# = (b2# - b1#) / (m1# - m2#)
   y# = x# * m1# + b1#
   
   ;schauen, ob der Punkt auf einer Linie liegt
   abstand# = Abs (x1#-x2#)
   If Abs (x#-x1#) <= abstand # Then
      If Abs (x#-x2#) <= abstand # Then
         
         abstand# = Abs (y1#-y2#)
         If Abs (y#-y1#) <= abstand # Then
            If Abs (y#-y2#) <= abstand # Then
         
               abstand# = Abs (x3#-x4#)
               If Abs (x#-x3#) <= abstand # Then
                  If Abs (x#-x4#) <= abstand # Then
                     
                     abstand# = Abs (y3#-y4#)
                     If Abs (y#-y3#) <= abstand # Then
                        If Abs (y#-y4#) <= abstand # Then
                           L_COL_X# = x#
                     L_COL_Y# = y#
                           Return 1
                           
                           
                        EndIf
                     EndIf
                  EndIf
               EndIf
            EndIf
         EndIf
      EndIf
   EndIf
End Function

Include "Matrix.bb"

Const GWIDTH = 800
Const GHEIGHT = 600

Graphics GWIDTH, GHEIGHT, 0, 2
SetBuffer BackBuffer()

Const MAX_VERTICES   = 64
Const MAX_EDGES      = 64

Const STEPSIZE = 6

Global ProjectedMin#, ProjectedMax#
Global ResultX#, ResultY#
Global CollisionVX#, CollisionVY#, CollisionPX#, CollisionPY#

Type TParticle
   Field X#
   Field Y#
   
   Field OldX#
   Field OldY#
   
   Field A_X#
   Field A_Y#
   
   Field Mass#
   Field ID
   
   Field Parent.TEntity
End Type

Type TConstraint
   Field ID
   Field P1.TParticle
   Field P2.TParticle
   
   Field Length#
   
   Field Force#
   
   Field Parent.TEntity
End Type

Type TEntity
   Field VertexCount
   Field Vertex.TParticle[ MAX_VERTICES - 1 ]
   
   Field EdgeCount
   Field Edge.TConstraint[ MAX_EDGES - 1 ]
   
   Field CenterX#
   Field CenterY#
   
   Field MinX#
   Field MinY#
   Field MaxX#
   Field MaxY#
End Type

Const TIMESTEP# = 1
Global GRAVITY_X# = 0
Global GRAVITY_Y# = 1

Global Iterations = 10
Global GameRunning = True
Global FollowParticle.TParticle
Global CurrentEntity.TEntity
Global EntityCount, VertexCount

Timer = CreateTimer( 60 )

While Not KeyHit( 1 )
   Cls
   
   Counter = MilliSecs()
   If GameRunning Then
      AccumulateForces()
      Verlet()
      SatisfyConstraints()
   EndIf
   LagString$ = "Physik: " + ( MilliSecs() - Counter ) + "ms "
   
   Counter = MilliSecs()
   Render()
   LagString$ = LagString$ + "Zeichnen: " + ( MilliSecs() - Counter ) + "ms"
   
   UserInput()
   
   Color 128, 128, 128
   Text 0, 0, LagString$ + " | " + EntityCount + " Objekte, " + VertexCount + " Vertices"
   
   Flip 0
   WaitTimer Timer
Wend

End


Function UserInput()
   If MouseHit( 1 ) Then
      Entity.TEntity = New TEntity
      
      Particle.TParticle = CreateParticle( Entity, MouseX() - 25*Rnd( 0.5, 1 ), MouseY() - 25*Rnd( 0.5, 1 ) )
      CreateParticle( Entity, MouseX() - 25*Rnd( 0.5, 1 ), MouseY() + 25*Rnd( 0.5, 1 ) )
      CreateParticle( Entity, MouseX() + 25*Rnd( 0.5, 1 ), MouseY() + 25*Rnd( 0.5, 1 ) )
      
      CreateConstraint( Entity, Particle, After Particle )
      CreateConstraint( Entity, After Particle, After After Particle )
      CreateConstraint( Entity, After After Particle, Particle )
      
      EntityCount = EntityCount + 1
      VertexCount = VertexCount + 3
   EndIf
   
   If MouseHit( 2 ) Then
      CreateBox( MouseX(), MouseY(), 40, 40 )
      EntityCount = EntityCount + 1
      VertexCount = VertexCount + 4
   EndIf
   
   If KeyHit( 28 ) Then
      If FollowParticle = Null Then
         MX = MouseX()
         MY = MouseY()
         
         NearestDistance# = 100000
         For Particle.TParticle = Each TParticle
            Dist# = ( Particle\X# - MX )*( Particle\X# - MX ) + ( Particle\Y# - MY )*( Particle\Y# - MY )
            
            If Dist# < NearestDistance# Then
               NearestDistance# = Dist#
               NearestParticle.TParticle = Particle
            EndIf
         Next
         
         FollowParticle = NearestParticle
      Else
         FollowParticle = Null
      EndIf
   EndIf
   
   If KeyHit( 57 ) Then GameRunning = Not GameRunning
End Function

Function AccumulateForces()
   For Particle.TParticle = Each TParticle
      Particle\A_X# = GRAVITY_X#
      Particle\A_Y# = GRAVITY_Y#
   Next
End Function

Function Verlet()
   For Particle.TParticle = Each TParticle
      Temp_X# = Particle\X#
      Temp_Y# = Particle\Y#
      Particle\X# = Particle\X# + Particle\X# - Particle\OldX# + Particle\A_X#*TIMESTEP#*TIMESTEP#
      Particle\Y# = Particle\Y# + Particle\Y# - Particle\OldY# + Particle\A_Y#*TIMESTEP#*TIMESTEP#
      Particle\OldX# = Temp_X#
      Particle\OldY# = Temp_Y#
   Next
End Function

Function SatisfyConstraints()
   For i = 1 To Iterations
      For Entity.TEntity = Each TEntity
         Entity\CenterX# = 0
         Entity\CenterY# = 0
         MaxX# = -1000
         MaxY# = -1000
         MinX# = 1000
         MinY# = 1000
         For t = 0 To Entity\VertexCount - 1
            Entity\CenterX# = Entity\CenterX# + Entity\Vertex[ t ]\X#/Entity\VertexCount
            Entity\CenterY# = Entity\CenterY# + Entity\Vertex[ t ]\Y#/Entity\VertexCount
            
            If Entity\Vertex[ t ]\X# > MaxX# Then MaxX# = Entity\Vertex[ t ]\X#
            If Entity\Vertex[ t ]\X# < MinX# Then MinX# = Entity\Vertex[ t ]\X#
            If Entity\Vertex[ t ]\Y# > MaxY# Then MaxY# = Entity\Vertex[ t ]\Y#
            If Entity\Vertex[ t ]\Y# < MinY# Then MinY# = Entity\Vertex[ t ]\Y#
         Next
         
         Entity\MaxX# = MaxX#
         Entity\MinX# = MinX#
         Entity\MaxY# = MaxY#
         Entity\MinY# = MinY#
      Next
      
      For E1.TEntity = Each TEntity
         For E2.TEntity = Each TEntity
            If E1 <> E2 Then
               If RectsOverlap( E1\MinX#, E1\MinY#, E1\MaxX# - E1\MinX#, E1\MaxY# - E1\MinY#, E2\MinX#, E2\MinY#, E2\MaxX# - E2\MinX#, E2\MaxY# - E2\MinY# ) Then
                  CollisionVector( E1, E2 )
               EndIf
            EndIf
         Next
      Next
      
      For Constraint.TConstraint = Each TConstraint
         For Particle.TParticle = Each TParticle
            Particle\X# = Min( Max( Particle\X#, 0 ), GWIDTH - 1 )
            Particle\Y# = Min( Max( Particle\Y#, 0 ), GHEIGHT - 1 )
         Next
         
         If Constraint\P1\Mass + Constraint\P2\Mass > 0 Then
            DeltaX# = Constraint\P2\X# - Constraint\P1\X#
            DeltaY# = Constraint\P2\Y# - Constraint\P1\Y#
            
            DeltaLength# = Sqr( DeltaX#*DeltaX# + DeltaY#*DeltaY# )
            Diff# = ( DeltaLength# - Constraint\Length# )/DeltaLength#
            
            AccWeight# = Constraint\P1\Mass + Constraint\P2\Mass
            Ratio1# = Constraint\P2\Mass/AccWeight#
            Ratio2# = Constraint\P1\Mass/AccWeight#
                     
            Constraint\P1\X# = Constraint\P1\X# + DeltaX#*Diff#*Constraint\Force#*Ratio1#
            Constraint\P1\Y# = Constraint\P1\Y# + DeltaY#*Diff#*Constraint\Force#*Ratio1#
            Constraint\P2\X# = Constraint\P2\X# - DeltaX#*Diff#*Constraint\Force#*Ratio2#
            Constraint\P2\Y# = Constraint\P2\Y# - DeltaY#*Diff#*Constraint\Force#*Ratio2#
         EndIf
      Next
      
      If FollowParticle <> Null Then
         FollowParticle\X# = MouseX()
         FollowParticle\Y# = MouseY()
      EndIf
   Next
End Function

Function Render()
   LockBuffer BackBuffer()
   
   For Entity.TEntity = Each TEntity
      If Entity\CenterX# >= 0 And Entity\CenterX# < GWIDTH Then
         If Entity\CenterY# >= 0 And Entity\CenterY# < GHEIGHT Then
            WritePixelFast Floor( Entity\CenterX# ), Floor( Entity\CenterY# ), $00FFFF00
         EndIf
      EndIf
   Next
   
   Color 255, 0, 0
   For Constraint.TConstraint = Each TConstraint
      Line Constraint\P1\X#, Constraint\P1\Y#, Constraint\P2\X#, Constraint\P2\Y#
   Next
   
   For Particle.TParticle = Each TParticle
      If Particle\X# >= 0 And Particle\X# < GWIDTH Then
         If Particle\Y# >= 0 And Particle\Y# < GHEIGHT Then
            WritePixelFast Floor( Particle\X# ), Floor( Particle\Y# ), $00FFFFFF
         EndIf
      EndIf
   Next
   
   UnlockBuffer BackBuffer()
End Function

Function Min#( A#, B# )
   If A# < B# Then Return A# Else Return B#
End Function

Function Max#( A#, B# )
   If A# > B# Then Return A# Else Return B#
End Function

Function CreateParticle.TParticle( Parent.TEntity, X#, Y#, Mass# = 1 )
   If Parent <> Null Then
      If Parent\VertexCount < MAX_VERTICES - 1 Then
         Particle.TParticle = New TParticle
            Particle\X# = X#
            Particle\Y# = Y#
            Particle\OldX# = X#
            Particle\OldY# = Y#
            Particle\Parent = Parent
            Particle\Mass# = Mass#
         
         Parent\Vertex[ Parent\VertexCount ] = Particle
         Parent\VertexCount = Parent\VertexCount + 1
         
         Return Particle
      EndIf
   EndIf
End Function

Function CreateConstraint.TConstraint( Parent.TEntity, P1.TParticle, P2.TParticle, Force# = 1 )
   If Parent <> Null Then
      If Parent\EdgeCount < MAX_EDGES - 1 Then
         If P1 <> P2 Then
            Constraint.TConstraint = New TConstraint
               Constraint\P1 = P1
               Constraint\P2 = P2
               ConstraintLengthSq# = ( Constraint\P1\X# - Constraint\P2\X# )*( Constraint\P1\X# - Constraint\P2\X# ) + ( Constraint\P1\Y# - Constraint\P2\Y# )*( Constraint\P1\Y# - Constraint\P2\Y# )
               Constraint\Length# = Sqr( ConstraintLengthSq# )
               Constraint\Parent = Parent
               Constraint\Force# = Force#
            
            Parent\Edge[ Parent\EdgeCount ] = Constraint
            Parent\EdgeCount = Parent\EdgeCount + 1
            
            Return Constraint
         EndIf
      EndIf
   EndIf
End Function

Function CreateBox( X#, Y#, Width#, Height# )
   Entity.TEntity = New TEntity
   
   P1.TParticle = CreateParticle( Entity, X# - Width#/2, Y# - Height#/2 )
   P2.TParticle = CreateParticle( Entity, X# + Width#/2, Y# - Height#/2 )
   P3.TParticle = CreateParticle( Entity, X# + Width#/2, Y# + Height#/2 )
   P4.TParticle = CreateParticle( Entity, X# - Width#/2, Y# + Height#/2 )
   
   CreateConstraint( Entity, P1, P2 )
   CreateConstraint( Entity, P2, P3 )
   CreateConstraint( Entity, P3, P4 )
   CreateConstraint( Entity, P4, P1 )
   CreateConstraint( Entity, P1, P3 )
   CreateConstraint( Entity, P2, P4 )
End Function

Function CreateCircle( X#, Y#, RadiusX#, RadiusY# )
   Entity.TEntity = New TEntity
   
   For i = STEPSIZE To 360 Step STEPSIZE
      P.TParticle = CreateParticle( Entity, X# + Cos( i )*RadiusX#, Y# + Sin( i )*RadiusY# )
      If i <> STEPSIZE Then CreateConstraint( Entity, P, Before P )
   Next
   
   CreateConstraint( Entity, P, Entity\Vertex[ 0 ] )
   
   Center.TParticle = CreateParticle( Entity, X#, Y# )
   
   For i = 0 To Entity\VertexCount - 2
      CreateConstraint( Entity, Entity\Vertex[ i ], Center )
   Next
End Function

Function Dotproduct#( X1#, Y1#, X2#, Y2# )
   Return X1#*X2# + Y1#*Y2#
End Function

Function Normalize( X#, Y# )
   Length# = Sqr( X#*X# + Y#*Y# )
   ResultX# = X#/Length#
   ResultY# = Y#/Length#
End Function

Function ProjectToAxis( AxisX#, AxisY#, Entity.TEntity )
   Local DotP# = Dotproduct( AxisX#, AxisY#, Entity\Vertex[ 0 ]\X#, Entity\Vertex[ 0 ]\Y# )
   ProjectedMin# = DotP#
   ProjectedMax# = DotP#
   
   For i = 1 To Entity\VertexCount - 1
      DotP# = Dotproduct( AxisX#, AxisY#, Entity\Vertex[ i ]\X#, Entity\Vertex[ i ]\Y# )
      
      If DotP# < ProjectedMin# Then ProjectedMin# = DotP# ElseIf DotP# > ProjectedMax# Then ProjectedMax# = DotP#
   Next
End Function

Function IntervalDistance#( A1#, B1#, A2#, B2# )
   If A1# < A2# Then
      Return A2# - B1#
   Else
      Return A1# - B2#
   EndIf
End Function

Function CollisionVector( E1.TEntity, E2.TEntity )
   CollisionPX# = 0
   CollisionPY# = 0
   CollisionVX# = 0
   CollisionVY# = 0
   
   MinDistance# = 10000000
   For i = 0 To E1\EdgeCount + E2\EdgeCount - 1
      If i < E1\EdgeCount Then
         Edge.TConstraint = E1\Edge[ i ]
      Else
         Edge.TConstraint = E2\Edge[ i - E1\EdgeCount ]
      EndIf
      
      AxisX# = -( Edge\P2\Y# - Edge\P1\Y# )
      AxisY# = Edge\P2\X# - Edge\P1\X#
      Normalize( AxisX#, AxisY# )
      AxisX# = ResultX#
      AxisY# = ResultY#
      
      ProjectToAxis( AxisX#, AxisY#, E1 )
      Min# = ProjectedMin#
      Max# = ProjectedMax#
      ProjectToAxis( AxisX#, AxisY#, E2 )
      
      Distance# = IntervalDistance( Min#, Max#, ProjectedMin#, ProjectedMax# )
      If Distance# > 0 Then
         Return False
      ElseIf Abs( Distance# ) < MinDistance# Then
         MinDistance# = Abs( Distance# )
         CollAxisX# = AxisX#
         CollAxisY# = AxisY#
         
         DiffX# = E1\CenterX# - E2\CenterX#
         DiffY# = E1\CenterY# - E2\CenterY#
         
         If Dotproduct( CollAxisX#, CollAxisY#, DiffX#, DiffY# ) < 0 Then
            DiffX# = -DiffX#
            DiffY# = -DiffY#
         EndIf
         
         CollEdge.TConstraint = Edge
      EndIf
   Next
   
   CollisionVX# = CollAxisX#*MinDistance#
   CollisionVY# = CollAxisY#*MinDistance#
   
   If CollEdge\Parent <> E2 Then
      Temp.TEntity = E2
      E2 = E1
      E1 = Temp
   EndIf
   
   Angle# = ATan2( CollisionVY#, CollisionVX# )
   RotMat.TMatrix3 = TransposeMatrix( RotationMatrix( Angle# ) )
   TranMat.TMatrix3 = TranslationMatrix( -E1\CenterX#, -E1\CenterY# )
   VertV.TVector3 = New TVector3
      VertV\A[ 2 ] = 1
      VertV\A[ 0 ] = E2\CenterX#
      VertV\A[ 1 ] = E2\CenterY#
   
   ResultV.TVector3 = MatrixVectorMultiply( RotMat, MatrixVectorMultiply( TranMat, VertV ) )
   If ResultV\A[ 0 ] > 0 Then
      CollisionVX# = -CollisionVX#
      CollisionVY# = -CollisionVY#
      
      Angle# = ATan2( CollisionVY#, CollisionVX# )
      RotMat.TMatrix3 = TransposeMatrix( RotationMatrix( Angle# ) )
   EndIf
   
   SmallestY# = 10000
   For i = 0 To E1\VertexCount - 1
      Vertex.TParticle = E1\Vertex[ i ]
      
      VertV\A[ 0 ] = Vertex\X#
      VertV\A[ 1 ] = Vertex\Y#
      
      ResultV.TVector3 = MatrixVectorMultiply( RotMat, MatrixVectorMultiply( TranMat, VertV ) )
      If ResultV\A[ 0 ] < SmallestY# Then
         SmallestY# = ResultV\A[ 0 ]
         LowestVertex.TParticle = Vertex
      EndIf
      
      Delete ResultV
   Next
   
   Treshold# = 0.001
   For i = 0 To E2\EdgeCount - 1
      V1.TParticle = E2\Edge[ i ]\P1
      V2.TParticle = E2\Edge[ i ]\P2
      
      VertV\A[ 0 ] = V1\X# - CollisionVX#
      VertV\A[ 1 ] = V1\Y# - CollisionVY#
      
      ResultV.TVector3 = MatrixVectorMultiply( RotMat, MatrixVectorMultiply( TranMat, VertV ) )
      V1Y# = ResultV\A[ 0 ]
      
      Delete ResultV
      
      VertV\A[ 0 ] = V2\X# - CollisionVX#
      VertV\A[ 1 ] = V2\Y# - CollisionVY#
      
      ResultV.TVector3 = MatrixVectorMultiply( RotMat, MatrixVectorMultiply( TranMat, VertV ) )
      V2Y# = ResultV\A[ 0 ]
      
      If Abs( V1Y# - V2Y# ) < Treshold# Then
         If Abs( ( V1Y# + V2Y# )/2. - SmallestY# ) < Treshold# Then Exit
      EndIf
      
      Delete ResultV
   Next
   
   Delete Each TMatrix3
   Delete Each TVector3
   
   CollisionPX# = LowestVertex\X#
   CollisionPY# = LowestVertex\Y#
   
   If Abs( V2\X# - V1\X# ) > Abs( V2\Y# - V1\Y# ) Then
      Ratio# = ( CollisionPX# + CollisionVX# - V1\X# )/( V2\X# - V1\X# )
   Else
      Ratio# = ( CollisionPY# + CollisionVY# - V1\Y# )/( V2\Y# - V1\Y# )
   EndIf
   
   Lambda# = Ratio#*Ratio# + ( 1 - Ratio# )*( 1 - Ratio# )
   
   V1\X# = V1\X# - CollisionVX#*( 1 - Ratio# )*0.5/Lambda#
   V1\Y# = V1\Y# - CollisionVY#*( 1 - Ratio# )*0.5/Lambda#
   V2\X# = V2\X# - CollisionVX#*Ratio#*0.5/Lambda#
   V2\Y# = V2\Y# - CollisionVY#*Ratio#*0.5/Lambda#
   LowestVertex\X# = LowestVertex\X# + CollisionVX#*0.5
   LowestVertex\Y# = LowestVertex\Y# + CollisionVY#*0.5
   
   Return True
End Function

Type TMatrix3
   Field A#[ 8 ]
End Type

Type TVector3
   Field A#[ 2 ]
End Type

Function MatrixMatrixMultiply.TMatrix3( A.TMatrix3, B.TMatrix3 )
   M.TMatrix3 = New TMatrix3
      M\A[ 0 ] = A\A[ 0 ]*B\A[ 0 ] + A\A[ 1 ]*B\A[ 3 ] + A\A[ 2 ]*B\A[ 6 ]
      M\A[ 1 ] = A\A[ 0 ]*B\A[ 1 ] + A\A[ 1 ]*B\A[ 4 ] + A\A[ 2 ]*B\A[ 7 ]
      M\A[ 2 ] = A\A[ 0 ]*B\A[ 2 ] + A\A[ 1 ]*B\A[ 5 ] + A\A[ 2 ]*B\A[ 8 ]
      
      M\A[ 3 ] = A\A[ 3 ]*B\A[ 0 ] + A\A[ 4 ]*B\A[ 3 ] + A\A[ 5 ]*B\A[ 6 ]
      M\A[ 4 ] = A\A[ 3 ]*B\A[ 1 ] + A\A[ 4 ]*B\A[ 4 ] + A\A[ 5 ]*B\A[ 7 ]
      M\A[ 5 ] = A\A[ 3 ]*B\A[ 2 ] + A\A[ 4 ]*B\A[ 5 ] + A\A[ 5 ]*B\A[ 8 ]
   
      M\A[ 6 ] = A\A[ 6 ]*B\A[ 0 ] + A\A[ 7 ]*B\A[ 3 ] + A\A[ 8 ]*B\A[ 6 ]
      M\A[ 7 ] = A\A[ 6 ]*B\A[ 1 ] + A\A[ 7 ]*B\A[ 4 ] + A\A[ 8 ]*B\A[ 7 ]
      M\A[ 8 ] = A\A[ 6 ]*B\A[ 2 ] + A\A[ 7 ]*B\A[ 5 ] + A\A[ 8 ]*B\A[ 8 ]
   
   Return M
End Function

Function MatrixVectorMultiply.TVector3( A.TMatrix3, B.TVector3 )
   V.TVector3 = New TVector3
      V\A[ 0 ] = A\A[ 0 ]*B\A[ 0 ] + A\A[ 1 ]*B\A[ 1 ] + A\A[ 2 ]*B\A[ 2 ]
      V\A[ 1 ] = A\A[ 3 ]*B\A[ 0 ] + A\A[ 4 ]*B\A[ 1 ] + A\A[ 5 ]*B\A[ 2 ]
      V\A[ 2 ] = A\A[ 6 ]*B\A[ 0 ] + A\A[ 7 ]*B\A[ 1 ] + A\A[ 8 ]*B\A[ 2 ]
   
   If V\A[ 2 ] <> 1 Then Stop
   
   Return V
End Function

Function RotationMatrix.TMatrix3( Scalar# )
   M.TMatrix3 = New TMatrix3
      M\A[ 0 ] = Cos( Scalar# ) : M\A[ 1 ] = -Sin( Scalar# )
      M\A[ 3 ] = Sin( Scalar# ) : M\A[ 4 ] = Cos( Scalar# )
                                                M\A[ 8 ] = 1
   
   Return M
End Function

Function TranslationMatrix.TMatrix3( X#, Y# )
   M.TMatrix3 = IdentityMatrix()
                  M\A[ 2 ] = X#
                  M\A[ 5 ] = Y#
   
   Return M
End Function

Function IdentityMatrix.TMatrix3()
   M.TMatrix3 = New TMatrix3
      M\A[ 0 ] = 1
               M\A[ 4 ] = 1
                        M\A[ 8 ] = 1
   
   Return M
End Function

Function TransposeMatrix.TMatrix3( M.TMatrix3 )
   Temp# = M\A[ 1 ]
   M\A[ 1 ] = M\A[ 3 ]
   M\A[ 3 ] = Temp#
   
   Temp# = M\A[ 2 ]
   M\A[ 2 ] = M\A[ 6 ]
   M\A[ 6 ] = Temp#
   
   Temp# = M\A[ 5 ]
   M\A[ 5 ] = M\A[ 7 ]
   M\A[ 7 ] = Temp#
   
   Return M
End Function