Michael "h4tt3n" Nissen's FreeBasic spring simulation tutorial
part 3 - angular springs, also appearing in QBExpress # 30...
1. INTRODUCTION
Hello everyone, and welcome to part three of my little spring tutorial. In part one we looked at all the basics needed to create a physics simulation, and we looked at a simple one dimensional code example of a mass-spring simulation. In part two we had a good look at the vector math needed in 2d spring physics, and finally we looked at a method for doing velocity dependent damping.
In this part we will stay in 2d and learn how to implement angular springs. In the previous two parts of this tutorial we've been working with what I prefer to call linear springs, which act on two point masses and tries to keep them at a fixed distance by applying a ”linear force” along the spring axis. Similarly, the type of angular spring we will be looking at acts on two linear springs and tries to keep them at a fixed angle by applying an ”angular force” - also called torque - perpendicular to the spring axis.
Angular springs are a very useful tool for building many different types of deformable or flexible ”soft bodies” out of linear springs and point masses, like for instance hair, trees, water balloons, cloth, steel girders, and jelly. I'll cover this subject in greater detail in a later tutorial. Put very shortly, in this part we'll learn:
1.how to find the angle between two connected linear springs
2.how to apply an angular force to the springs in order to keep them at a fixed angle
Let's get started by looking at...
2. HOW TO FIND THE ANGLE BETWEEN TWO SPRINGS
In the previous part of this tutorial we learned a great deal of vector math, especially in the part about velocity dependent damping. One of the very important equations were about finding the scalar product or ”dot product” of two vectors:
A · B = |A| * |B| * Cos(v)
where A and B are 2d vectors, |A| and |B| is the magnitude of A and B, and v is the angle between the two vectors. If we rearrange the equation a bit we get
Cos(v) = A · B / |A| * |B|
Which can be boiled further down to just taking the dot product of the normalized vectors:
Cos(v) = (A / |A|) · (B / |B|) = A normalized · B normalized
And voila! Now we already know how to get cosine to the angle between the springs, even without calling any CPU-expensive trig functions. We've already found the magnitude of the springs and normalised them in order to calculate the linear spring force, so we just recycle that information here. This helps making the angular spring implementation computationally very cheap. But what about sine to the angle? Lucky for us there's a very similar function just for that. Remember the definition of the dot product:
A · B = A.x * B.x + A.y * B.y
There is a similar function called the perpendicular dot product, also called ”perproduct” or ”perpdot” for short, which looks like this:
A ”perpdot” B = A.x * B.y – A.y * B.x
If we use perpdot instead of dot in the above equation we get:
Sin(v) = A ”perpdot” B / |A| * |B|
Or boiled down like the cosine equation:
Sin(v) = A normalised ”perpdot” B normalised
There we go. Now we know how to find sine and cosine of the angle between two springs. If we wanted to know the actual angle in radians we could use the ATan2() function, but as we shall see later there's really no need to. Here you can see what all the above equations look like when they're implemented in a working code sample:
Code: Select all
'******************************************************************************'
'
' Michael "h4tt3n" Nissen's mass-spring-damper tutorial, November 2008
'
' Example # 3.1
'
' Simple angle test program. In this example we use vector math to find sine
' and cosine of the angle between the two springs. (Press esc to quit)
'
'******************************************************************************'
'' define constants
const pi = 4*atn(1) '' pi
Const rad2deg = 180/pi '' radians to degrees
Const num_masses = 3 '' number of point masses
Const Rope_Length = 350 '' total length of all springs
Const Scrn_Wid = 600 '' screen width
Const Scrn_Hgt = 600 '' screen height
'' define types
Type Vector_Type
As Double X, Y
End Type
Type Mass_Type
As Double Mass, dens, radius
As Vector_Type Frc, Acc, Vel, Psn
End Type
Type Spring_Type
As Integer a, b
As Double Length
As Vector_Type Nor_Lng, Lng
End Type
Type Angular_Spring_Type
As Integer a, b
As Double sin_angle, cos_angle
End Type
'' dim arrays & variables
Dim As mass_type mass(1 to num_masses)
dim as spring_type spring(1 to num_masses-1)
dim as angular_spring_type angular_spring(1 to num_masses-2)
Dim As Integer mouse_x, mouse_y
'' initiate point masses
for i As Integer = lbound(mass) to Ubound(mass)
with mass(i)
.radius = 5
.Psn.y = scrn_hgt*0.5-Rope_Length*0.5+(Rope_Length/(num_masses-1))*(i-1)
.Psn.x = scrn_wid*0.5
end with
Next
'' initiate linear springs
'' (a and b stores the number of the masses it's attached to)
for i As Integer = lbound(spring) to ubound(spring)
with spring(i)
.a = i
.b = i+1
end with
next
'' initiate angular springs
'' (a and b stores the number of the linear springs it's attached to)
for i As Integer = lbound(angular_spring) to ubound(angular_spring)
with angular_spring(i)
.a = i
.b = i+1
end with
Next
'' initiate screen
ScreenRes Scrn_Wid, Scrn_hgt, 16
Color RGB(0, 0, 0), RGB(255, 255, 255)
Do
'' use mouse to change spring angle
GetMouse Mouse_x, Mouse_Y
Mass(1).Psn.X = Mouse_X
Mass(1).Psn.Y = Mouse_Y
'' get linear spring data
For i As Single = Lbound(Spring) to Ubound(Spring)
With Spring(i)
'' spring length vector
.Lng.X = Mass(.b).Psn.X-Mass(.a).Psn.X
.Lng.Y = Mass(.b).Psn.Y-Mass(.a).Psn.Y
'' spring length scalar
.Length = Sqr(.Lng.X*.Lng.X+.Lng.Y*.Lng.Y)
'' normalised spring length vector
.Nor_Lng.X = .Lng.X/.Length
.Nor_Lng.Y = .Lng.Y/.Length
End With
Next
'' get angular spring data
For i As Integer = Lbound(Angular_Spring) To Ubound(Angular_Spring)
With Angular_Spring(i)
'' cosine of the angle - dot product of normalized spring vectors
.cos_angle = Spring(.a).Nor_Lng.x*Spring(.b).Nor_Lng.x+Spring(.a).Nor_Lng.y*Spring(.b).Nor_Lng.y
'' sine of the angle - perpendicular dot product of normalized spring vectors
.sin_angle = Spring(.a).Nor_Lng.x*Spring(.b).Nor_Lng.y-Spring(.a).Nor_Lng.y*Spring(.b).Nor_Lng.x
End With
Next
'' draw to screen
ScreenLock
'' clear screen
Cls
'' print data
With Angular_Spring(1)
Locate 2, 2: Print Using " Sine of spring angle: ####.### (-1, 1)"; .sin_Angle
Locate 4, 2: Print Using " Cosine of spring angle: ####.### (-1, 1)"; .cos_Angle
Locate 6, 2: Print Using "Spring angle in radians: ####.### (-pi, pi)"; ATan2(.Sin_Angle, .Cos_Angle)
Locate 8, 2: Print Using "Spring angle in degrees: ####.### (-180, 180)"; ATan2(.Sin_Angle, .Cos_Angle)*rad2Deg
End With
'' draw springs
For i As Integer = Lbound(Spring) To Ubound(Spring)
With Spring(i)
Line (Mass(.a).Psn.X, Mass(.a).Psn.Y)-(Mass(.b).Psn.X, Mass(.b).Psn.Y), Rgb(64, 160, 32)
End With
Next
'' draw particles
For i As Integer = Lbound(mass) To Ubound(mass)
With mass(i)
Circle(.Psn.X, .Psn.Y), .radius, rgb(0, 0, 0),,, 1, f
Circle(.Psn.X, .Psn.Y), .radius-1, rgb(255, 32, 32),,, 1, f
End With
Next
ScreenUnLock
Sleep 1, 1
Loop Until MultiKey(1)
End
3. A LITTLE MATH AND PHYSICS ABOUT ROTATION
In the previous parts of this tutorial we learned how to apply a linear force along the spring axis. When dealing with angular springs we don't want to apply forces parallel to the spring axis but rather perpendicular to it. We also need to find a good angular spring equation that applies the right amount of ”angular force” in the right situation. To do this we first need to get familiar with two new concepts: normal vectors and torque.
A normal vector - not to be confused with a normalised vector! - is simply a different name for a perpendicular vector. Finding the normal of a vector is very easy - the normal of vector (x, y) is simply (y, -x). This vector always points to the right of the vector it is normal to and is hence called a right-hand normal vector. There also is a left-hand normal (-y, x), but the right-hand normal is most commonly used, so we'll stick to that. If we find the normal of a spring vector and then normalise it – in other words the normalised normal vector - and use this to define the angular spring force direction, it will always be perpendicular to the spring's axis, which is just what we want.
So much for the direction of the angular force vector, but what about its magnitude – how much force do we apply? This is where torque comes into the picture. When dealing with physics, torque is simply the rotational equivalent of force. When you apply force to an object it moves along a straight line, and when you apply torque to an object it rotates. Within the boundaries of our simple 2d simulation, the definition of torque is
T = r * F
Which also means that
F = T / r
Where T is torque, r is distance to center of rotation, and F is force. Please note that this definition is only valid if the force vector is at a right angle to the distance vector, but since this is always the case in our simulation we'll just keep it at that and skip the more complex definitions of torque for now.
So, why do we need to know about torque? Let's go back and have a look at Newton's 3rd law, stating that for every force (action) there is an equal and opposite force (reaction). For linear springs this means that when applying spring force F to one of the endpoint particles, we have to apply exactly -F to the other particle, otherwise the simulation won't behave physically correct. We say that the net force on the system must be zero. This law is also valid for torque, which means that the torque we apply on one of the linear springs must be ”canceled out” by the torque we apply on the other - the net torque on the system must be zero. Please note that in this case force doesn't have to zero out, but force times distance to rotational center – or torque - does! And this means trouble when the springs are of different length, which they are more or less all the time. We somehow need to scale the forces, so they result in equal, opposite torques.
Besides all this there is another problem with the equation F = T / r, namely that force increases towards infinity as spring length decreases towards zero and vice versa. Practically speaking this means that two identical angular springs behave differently depending on wether they're applied to long or short linear springs. If one or both of the linear springs has near-zero length, then the applied force will be near-infinite, which usually results in simulation instability or even program crash. So we need to scale the forces, so they're always proportional to the length of the springs they're applied to.
Long story short, I've found a very simple and stable solution to both of the above mentioned problems. The applied angular force on linear spring 1 and 2 is simply
Angular Force 1 = Spring Force * Angular Spring 2 Length
Angular Force 2 = -Spring Force * Angular Spring 1 Length
Ok, now we've been around normals and torque. Next thing is to find a suitable angular spring force equation, and then we're ready to rock. Let's look at a practical example: We'll use the damped rope simulation (example 2.3) from the previous part of this tutorial and apply an angular spring between each pair of linear springs in the rope. Let's say we want to make all the springs stay parallel and form a sort of ”soft rod”, essentially behaving like a hair or a flag pole. In order to do this we first need an angular spring force equation. Remember the linear spring force equation, Hooke's law of elasticity?
Linear Force = -Stiffnes * (Length – Rest length)
This equation always produces a scalar spring force value of the right magnitude and with the right sign (+ or -). What we need to do is write a similar equation for angular springs. It's really tempting and intuitive to assume that we'll be all right by just translating the linear spring equation into
Angular Force = -Stiffnes * (Angle – Rest angle)
I've tested a few implementations of this method and discarded them for several reasons. One of the main reasons is that you need to call the cpu-expensive ATan2() function in order to get the angle between the springs. Besides, when bending the angular spring past the ”180 degree mark” away from it's rest angle, the spring force – which is at maximum - suddenly changes direction, often resulting in chaotic movement and simulation instability. Of course we could implement it if we really wanted to, but I just don't think it's worth the effort.
Instead, let me introduce you to a more stable and computationally much cheaper method. In our case with the ”soft rod” simulation all we need is sine of the angle between the springs in order to calculate the angular force: If the angle between two lines is 0 or 180 degrees – in other words they're parallel – then sine of the angle is zero. If for instance the angle is a little bigger than – say - 0 degrees, then sine of the angle is also slightly higher than zero and vice versa. This is a really good foundation for a spring equation. In our case the angular spring force equation is simply
Force = -Stiffnes * Sine angle
This equation will produce a force value that increases as the angle diverges more and more from 0 and which is signed either positive or negative depending on what direction the spring is bent. In other words we have an angular spring with a rest angle of 0 degrees.
If the angular spring is bent +/-90 degrees away from it's rest angle – which means sine to the angle is 1 – the spring force reaches its maximum. If the spring is bent even further the spring force decreases and reaches 0 when the spring is bent +/-180 degrees, where sine becomes 0 again. This sounds like a drawback, since the angular spring force will be 0 when the spring is bent furthest away from its rest angle, but practically it works quite well, and it avoids the above mentioned ”force-suddently-changes-direction” problem in the angle based equation.
If we used cosine instead of sine in our equation we'd get an angular spring that would be at rest at +/- 90 degrees. Combining sine and cosine in one spring equation allows us to pick several different rest angles. Here's an equation template ranging from -180 to +180 degrees at 45 degree intervals:
+/-180 degrees: Force = -Angular Stiffnes*(-Sin Angle)
+135 degrees: Force = -Angular Stiffnes*(-Sin Angle-Cos Angle)
+90 degrees: Force = -Angular Stiffnes*(-Cos Angle)
+45 degrees: Force = -Angular Stiffnes*(Sin Angle-Cos Angle)
0 degrees: Force = -Angular Stiffnes*(Sin Angle)
-45 degrees: Force = -Angular Stiffnes*(Sin Angle+Cos Angle)
-90 degrees: Force = -Angular Stiffnes*(Cos Angle)
-135 degrees: Force = -Angular Stiffnes*(-Sin Angle+Cos Angle)
If we want to simulate a soft body composed of simple geometric shapes like rods, boxes, triangles, octagons and the like, these equations will be adequate. Let's look at a code sample and see if all this works, shall we?
Code: Select all
'******************************************************************************'
'
' Michael "h4tt3n" Nissen's mass-spring-damper tutorial, November 2008
'
' Example # 3.2
'
' Simple angular spring simulation. This code sample is showing how linear
' and angular springs can be connected into a "soft rod" of different shapes.
' One of the point masses has been glued stuck and acts as a pivot for the
' rest of the body. Scroll down a bit and test the different angular spring
' force equations.
'
' (Press esc to quit)
'
'******************************************************************************'
'' define constants. Play around with these and see what happens!
const pi = 4*atn(1) '' pi (better not change ^^)
Const rad2deg = 180/pi '' radians to degrees
Const deg2rad = Pi/180 '' degrees to radians
Const Grav_acc = 2000 '' gravitational acceleration
const num_masses = 8 '' number of masses in rod
Const Rod_Length = 300 '' total spring body length
Const Linear_Stiffnes = 10000 '' linear spring stiffnes
Const Linear_Damping = 40 '' linear spring damping
Const Angular_Stiffnes = 10000 '' angular spring stiffnes
Const Scrn_Wid = 600 '' screen width
Const Scrn_Hgt = 600 '' screen height
Const dt = 0.0025 '' timestep, delta time
Const air_friction = 0.002 '' air friction (to calm springs down)
'' define types
Type Vector_Type
As Double X, Y
End Type
Type Mass_Type
As Double Mass, radius
As Vector_Type Frc, Acc, Vel, Psn
End Type
Type Linear_Spring_Type
As Integer a, b
As Double Length, Rest_Length
As Vector_Type Nor_Lng, Vel, Lng
End Type
Type Angular_Spring_Type
As Integer a, b
As Double sin_angle, cos_angle
End Type
'' dim variables
Dim As mass_type mass(1 to num_masses)
dim as Linear_spring_type Linear_spring(1 to num_masses-1)
dim as angular_spring_type angular_spring(1 to num_masses-2)
Dim As Integer mouse_x, mouse_y
Randomize timer
'' initiate point masses
for i As Integer = lbound(mass) to Ubound(mass)
with mass(i)
.mass = 1
.radius = 4
.Psn.x = scrn_wid*0.5+(Rod_Length/(num_masses-1))*(i-2)
.Psn.y = scrn_hgt*0.5
end with
Next
'' initiate linear springs
for i As Integer = lbound(Linear_spring) to ubound(Linear_spring)
with Linear_spring(i)
.a = i
.b = i+1
.Lng.X = Mass(.b).Psn.X-Mass(.a).Psn.X
.Lng.Y = Mass(.b).Psn.Y-Mass(.a).Psn.Y
.Length = Sqr(.Lng.X*.Lng.X+.Lng.Y*.Lng.Y)
.rest_length = .length
end With
next
'' initiate angular springs
for i As Integer = lbound(angular_spring) to ubound(angular_spring)
with angular_spring(i)
.a = i
.b = i+1
end with
Next
'' initiate screen
ScreenRes Scrn_Wid, Scrn_hgt, 16
Color RGB(0, 0, 0), RGB(255, 255, 255)
Mouse_X = Mass(1).Psn.X
Mouse_Y = Mass(1).Psn.Y
SetMouse Mouse_x, Mouse_Y
Do
'' use mouse to change spring angle
GetMouse Mouse_x, Mouse_Y
Mass(1).Psn.X = Mouse_X
Mass(1).Psn.Y = Mouse_Y
'' get linear spring data
For i As Integer = Lbound(Linear_Spring) to Ubound(Linear_Spring)
With Linear_Spring(i)
'' spring length vector
.Lng.X = Mass(.b).Psn.X-Mass(.a).Psn.X
.Lng.Y = Mass(.b).Psn.Y-Mass(.a).Psn.Y
'' spring length scalar
.Length = Sqr(.Lng.X*.Lng.X+.Lng.Y*.Lng.Y)
'' normalised spring length vector
.Nor_Lng.X = .Lng.X/.Length
.Nor_Lng.Y = .Lng.Y/.Length
'' velocity (used for damping)
.Vel.X = Mass(.b).Vel.X-Mass(.a).Vel.X
.Vel.Y = Mass(.b).Vel.Y-Mass(.a).Vel.Y
End With
Next
'' get angular spring data
For i As Integer = Lbound(Angular_Spring) To Ubound(Angular_Spring)
With Angular_Spring(i)
'' sine of the angle = perpendicular dot product of normalized spring vectors
.sin_angle = Linear_Spring(.a).Nor_Lng.x*Linear_Spring(.b).Nor_Lng.y-Linear_Spring(.a).Nor_Lng.y*Linear_Spring(.b).Nor_Lng.x
'' cosine of the angle = dot product of normalized spring vectors
.cos_angle = Linear_Spring(.a).Nor_Lng.x*Linear_Spring(.b).Nor_Lng.x+Linear_Spring(.a).Nor_Lng.y*Linear_Spring(.b).Nor_Lng.y
End With
Next
'' apply linear spring force
For i As Integer = Lbound(Linear_Spring) to Ubound(Linear_Spring)
With Linear_Spring(i)
Dim As Single Force = -Linear_Stiffnes*(.Length-.Rest_Length)-Linear_Damping*(.Nor_Lng.X*.Vel.X+.Nor_Lng.Y*.Vel.Y)
Dim As Vector_Type Frc
'' force vector = scalar force * normalised spring vector
frc.x = Force*.Nor_Lng.X
frc.y = Force*.Nor_Lng.Y
Mass(.a).Frc.X -= Frc.X
Mass(.a).Frc.Y -= Frc.Y
Mass(.b).Frc.X += Frc.X
Mass(.b).Frc.Y += Frc.Y
End With
Next
'' apply angular spring force
For i As Integer = Lbound(Angular_Spring) To Ubound(Angular_Spring)
With Angular_Spring(i)
Dim As Vector_Type Frc
'' angular spring force templates ranging from -180 to 180 degrees in 45 degree intervals
'' try these out and see how the "soft rod" stabilizes in different shapes!
'Dim As Single Force = -Angular_Stiffnes*(-.Sin_Angle) '' +/-180 deg
'Dim As Single Force = -Angular_Stiffnes*(-.Sin_Angle-.Cos_Angle) '' +135 deg
'Dim As Single Force = -Angular_Stiffnes*(-.Cos_Angle) '' +90 deg
'Dim As Single Force = -Angular_Stiffnes*(.Sin_Angle-.Cos_Angle) '' +45 deg
Dim As Single Force = -Angular_Stiffnes*(.Sin_Angle) '' 0 deg
'Dim As Single Force = -Angular_Stiffnes*(.Sin_Angle+.Cos_Angle) '' -45 deg
'Dim As Single Force = -Angular_Stiffnes*(.Cos_Angle) '' -90 deg
'Dim As Single Force = -Angular_Stiffnes*(-.Sin_Angle+.Cos_Angle) '' -135 deg
'' weighted average force keeps net torque zero and scales
'' torque so it is always proportional to spring length.
Dim As Single Force_a = Force*Linear_spring(.b).Length
Dim As Single Force_b = -Force*Linear_spring(.a).Length
With Linear_spring(.a)
'' force vector
frc.x = Force_a* .Nor_Lng.y
frc.y = Force_a*-.Nor_Lng.x
'' apply force to point masses
Mass(.a).Frc.x -= Frc.x
Mass(.a).Frc.y -= Frc.y
Mass(.b).Frc.x += Frc.x
Mass(.b).Frc.y += Frc.y
End with
With Linear_spring(.b)
'' force vector
frc.x = Force_b* .Nor_Lng.y
frc.y = Force_b*-.Nor_Lng.x
'' apply force to point masses
Mass(.a).Frc.x -= Frc.x
Mass(.a).Frc.y -= Frc.y
Mass(.b).Frc.x += Frc.x
Mass(.b).Frc.y += Frc.y
End with
End With
Next
'' apply velocity squared air friction
For i As Integer = lbound(mass) to ubound(mass)
With mass(i)
.Frc.x -= .Vel.X*Abs(.Vel.X)*Air_Friction
.Frc.y -= .Vel.y*Abs(.Vel.y)*Air_Friction
End With
Next
'' integrate (symplectic Euler 1st order)
For i As Integer = lbound(mass)+2 to ubound(mass)
With mass(i)
.acc.x = (.frc.x/.mass)
.acc.y = (.frc.y/.mass) + grav_acc
.frc.x = 0
.frc.y = 0
.vel.x += .acc.x*dt
.vel.y += .acc.y*dt
.Psn.x += .vel.x*dt
.Psn.y += .vel.y*dt
End with
Next
'' draw to screen
ScreenLock
'' clear screen
Cls
'' print data
With Angular_Spring(1)
Locate 2, 2: Print Using "Angular spring 1 angle: ####.## (-180, 180)"; ATan2(.Sin_Angle, .Cos_Angle)*rad2Deg
End With
'' draw springs
For i As Integer = Lbound(Linear_Spring) To Ubound(Linear_Spring)
With Linear_Spring(i)
Line (Mass(.a).Psn.X, Mass(.a).Psn.Y)-(Mass(.b).Psn.X, Mass(.b).Psn.Y), Rgb(64, 160, 32)
End With
Next
'' draw particles
For i As Integer = Lbound(mass) To Ubound(mass)
With mass(i)
Circle(.Psn.X, .Psn.Y), .radius, rgb(0, 0, 0),,, 1, f
Circle(.Psn.X, .Psn.Y), .radius-1, rgb(255, 32, 32),,, 1, f
End With
Next
ScreenUnLock
Sleep 1, 1
Loop Until Multikey(1)
End
But what if we want to assign a rest angle that isn't listed above? What we need is a generic all-purpose equation that will allow us to assign any rest angle and even change it during run-time. And here it is without further discussion:
Fa = -Ka * (cos (Vrest) * sin (V) – sin (Vr est)* cos (V))
Where Fa is angular force, Ka is angular spring stiffnes, V is current angle, and Vrest is rest angle. Notice how very similar this is to the perpdot equation? I don't know what it means, but it's cool! Here's a code sample showing the equation in action:
Code: Select all
'******************************************************************************'
'
' Michael "h4tt3n" Nissen's mass-spring-damper tutorial, November 2008
'
' Example # 3.3
'
' Angular spring simulation. This code sample is showing how linear
' and angular springs can form a string shaped soft body.
' Use mouse to move the soft body around and arrow keys to change
' the size and shape of the body.
'
' (Press esc to quit)
'
'******************************************************************************'
'' define constants. Play around with these and see what happens!
const pi = 4*atn(1) '' pi (better not change ^^)
Const rad2deg = 180/pi '' radians to degrees
Const deg2rad = Pi/180 '' degrees to radians
Const Grav_acc = 2000 '' gravitational acceleration
const num_masses = 12 '' number of masses in rod (3 or higher)
Const Rod_Length = 300 '' total spring body length
Const Linear_Stiffnes = 10000 '' linear spring stiffnes
Const Linear_Damping = 40 '' linear spring damping
Const Angular_Stiffnes = 10000 '' angular spring stiffnes
Const Scrn_Wid = 600 '' screen width
Const Scrn_Hgt = 600 '' screen height
Const dt = 0.0025 '' timestep, delta time
Const air_friction = 0.002 '' air friction (to calm springs down)
Const Delta_Angle = 0.1 '' spring angle step
Const Delta_Length = 1.002 '' spring length step
'' define types
Type Vector_Type
As Double X, Y
End Type
Type Mass_Type
As Double Mass, radius
As Vector_Type Frc, Acc, Vel, Psn
End Type
Type Linear_Spring_Type
As Integer a, b
As Double Length, Rest_Length, Mass
As Vector_Type Nor_Lng, Vel, Lng
End Type
Type Angular_Spring_Type
As Integer a, b
As Double sin_angle, cos_angle, sin_rest_angle, cos_rest_angle, Rest_Angle
End Type
'' dim variables
Dim As mass_type mass(1 to num_masses)
dim as Linear_spring_type Linear_spring(1 to num_masses-1)
dim as angular_spring_type angular_spring(1 to num_masses-2)
Dim As Integer mouse_x, mouse_y
Randomize timer
'' initiate point masses
for i As Integer = lbound(mass) to Ubound(mass)
with mass(i)
.mass = 1
.radius = 4
.Psn.x = (scrn_wid\2)
.Psn.y = (scrn_hgt\4) + (Rod_Length/(num_masses-1))*i-1
end with
Next
'' initiate linear springs
for i As Integer = lbound(Linear_spring) to ubound(Linear_spring)
with Linear_spring(i)
.a = i
.b = i+1
.Lng.X = Mass(.b).Psn.X-Mass(.a).Psn.X
.Lng.Y = Mass(.b).Psn.Y-Mass(.a).Psn.Y
.Length = Sqr(.Lng.X*.Lng.X+.Lng.Y*.Lng.Y)
.Nor_Lng.X = .Lng.X/.Length
.Nor_Lng.Y = .Lng.Y/.Length
.rest_length = .length
.Mass = mass(.a).mass+mass(.b).mass
end With
next
'' initiate angular springs
for i As Integer = lbound(angular_spring) to ubound(angular_spring)
with angular_spring(i)
.a = i
.b = i+1
.sin_angle = Linear_Spring(.a).Nor_Lng.x*Linear_Spring(.b).Nor_Lng.y-Linear_Spring(.a).Nor_Lng.y*Linear_Spring(.b).Nor_Lng.x
.cos_angle = Linear_Spring(.a).Nor_Lng.x*Linear_Spring(.b).Nor_Lng.x+Linear_Spring(.a).Nor_Lng.y*Linear_Spring(.b).Nor_Lng.y
.Sin_Rest_Angle = .Sin_Angle
.Cos_Rest_Angle = .Cos_Angle
.rest_angle = ATan2(.Sin_Rest_Angle, .Cos_Rest_Angle)
end with
Next
'' initiate screen
ScreenRes Scrn_Wid, Scrn_hgt, 16
Color RGB(0, 0, 0), RGB(255, 255, 255)
Mouse_X = Mass(1).Psn.X
Mouse_Y = Mass(1).Psn.Y
SetMouse Mouse_x, Mouse_Y
Do
'' use mouse to change spring angle
GetMouse Mouse_x, Mouse_Y
Mass(1).Psn.X = Mouse_X
Mass(1).Psn.Y = Mouse_Y
'' up arrow -> increase spring length
If Multikey(&h48) Then
For i As Integer = Lbound(Linear_Spring) To Ubound(Linear_Spring)
With Linear_Spring(i)
.Rest_Length *= Delta_Length
End With
Next
End If
'' down arrow -> decrease spring length
If Multikey(&h50) Then
For i As Integer = Lbound(Linear_Spring) To Ubound(Linear_Spring)
With Linear_Spring(i)
.Rest_Length /= Delta_Length
End With
Next
End If
'' Right arrow -> increase angular spring angle
If Multikey(&h4d) Then
For i As Integer = Lbound(Angular_Spring) To Ubound(Angular_Spring)
With Angular_Spring(i)
.Rest_Angle += Delta_Angle*deg2rad
If .rest_angle < -Pi Then .rest_angle += 2*Pi
If .rest_angle > Pi Then .rest_angle -= 2*Pi
.Sin_Rest_angle = Sin(.Rest_Angle)
.Cos_Rest_angle = Cos(.Rest_Angle)
End With
Next
End If
'' Left arrow -> decrease angular spring angle
If Multikey(&h4b) Then
For i As Integer = Lbound(Angular_Spring) To Ubound(Angular_Spring)
With Angular_Spring(i)
.Rest_Angle -= Delta_Angle*deg2rad
If .rest_angle < -Pi Then .rest_angle += 2*Pi
If .rest_angle > Pi Then .rest_angle -= 2*Pi
.Sin_Rest_angle = Sin(.Rest_Angle)
.Cos_Rest_angle = Cos(.Rest_Angle)
End With
Next
End If
'' get linear spring data
For i As Integer = Lbound(Linear_Spring) to Ubound(Linear_Spring)
With Linear_Spring(i)
'' spring length vector
.Lng.X = Mass(.b).Psn.X-Mass(.a).Psn.X
.Lng.Y = Mass(.b).Psn.Y-Mass(.a).Psn.Y
'' spring length scalar
.Length = Sqr(.Lng.X*.Lng.X+.Lng.Y*.Lng.Y)
'' normalised spring length vector
.Nor_Lng.X = .Lng.X/.Length
.Nor_Lng.Y = .Lng.Y/.Length
'' velocity (used for damping)
.Vel.X = Mass(.b).Vel.X-Mass(.a).Vel.X
.Vel.Y = Mass(.b).Vel.Y-Mass(.a).Vel.Y
End With
Next
'' get angular spring data
For i As Integer = Lbound(Angular_Spring) To Ubound(Angular_Spring)
With Angular_Spring(i)
'' sine of the angle = perpendicular dot product of normalized spring vectors
.sin_angle = Linear_Spring(.a).Nor_Lng.x*Linear_Spring(.b).Nor_Lng.y-Linear_Spring(.a).Nor_Lng.y*Linear_Spring(.b).Nor_Lng.x
'' cosine of the angle = dot product of normalized spring vectors
.cos_angle = Linear_Spring(.a).Nor_Lng.x*Linear_Spring(.b).Nor_Lng.x+Linear_Spring(.a).Nor_Lng.y*Linear_Spring(.b).Nor_Lng.y
End With
Next
'' apply linear spring force
For i As Integer = Lbound(Linear_Spring) to Ubound(Linear_Spring)
With Linear_Spring(i)
Dim As Single Force = -Linear_Stiffnes*(.Length-.Rest_Length)-Linear_Damping*(.Nor_Lng.X*.Vel.X+.Nor_Lng.Y*.Vel.Y)
Dim As Vector_Type Frc
'' force vector = scalar force * normalised spring vector
frc.x = Force*.Nor_Lng.X
frc.y = Force*.Nor_Lng.Y
Mass(.a).Frc.X -= Frc.X
Mass(.a).Frc.Y -= Frc.Y
Mass(.b).Frc.X += Frc.X
Mass(.b).Frc.Y += Frc.Y
End With
Next
'' apply angular spring force
For i As Integer = Lbound(Angular_Spring) To Ubound(Angular_Spring)
With Angular_Spring(i)
Dim As Vector_Type Frc
'' generic spring force equation
Dim As Single Force = -Angular_Stiffnes*(.Cos_Rest_angle*.Sin_Angle-.Sin_Rest_angle*.Cos_angle)
'' weighted average force keeps net torque zero and scales
'' torque so it is always proportional to spring length.
Dim As Single Force_a = Force*Linear_spring(.b).Length
Dim As Single Force_b = -Force*Linear_spring(.a).Length
With Linear_spring(.a)
'' force vector = scalar force * normalised normal vector
frc.x = Force_a*.Nor_Lng.y
frc.y = Force_a*-.Nor_Lng.x
'' apply force to point masses
Mass(.a).Frc.x -= Frc.x
Mass(.a).Frc.y -= Frc.y
Mass(.b).Frc.x += Frc.x
Mass(.b).Frc.y += Frc.y
End with
With Linear_spring(.b)
'' force vector as above
frc.x = Force_b*.Nor_Lng.y
frc.y = Force_b*-.Nor_Lng.x
'' apply force to point masses
Mass(.a).Frc.x -= Frc.x
Mass(.a).Frc.y -= Frc.y
Mass(.b).Frc.x += Frc.x
Mass(.b).Frc.y += Frc.y
End with
End With
Next
'' apply velocity squared air friction
For i As Integer = lbound(mass) to ubound(mass)
With mass(i)
.Frc.x -= .Vel.X*Abs(.Vel.X)*Air_Friction
.Frc.y -= .Vel.y*Abs(.Vel.y)*Air_Friction
End With
Next
'' integrate (symplectic Euler 1st order)
For i As Integer = lbound(mass)+1 to ubound(mass)
With mass(i)
.acc.x = (.frc.x/.mass)
.acc.y = (.frc.y/.mass) + grav_acc
.frc.x = 0
.frc.y = 0
.vel.x += .acc.x*dt
.vel.y += .acc.y*dt
.Psn.x += .vel.x*dt
.Psn.y += .vel.y*dt
End with
Next
'' draw to screen
ScreenLock
'' clear screen
Cls
'' print data
With Angular_Spring(1)
Locate 2, 2: Print Using " Angular spring 1 angle: ####.##"; ATan2(.Sin_Angle, .Cos_Angle)*rad2Deg
Locate 4, 2: Print Using "Angular spring 1 rest angle: ####.##"; .rest_angle*rad2Deg
End With
'' draw springs
For i As Integer = Lbound(Linear_Spring) To Ubound(Linear_Spring)
With Linear_Spring(i)
Line (Mass(.a).Psn.X, Mass(.a).Psn.Y)-(Mass(.b).Psn.X, Mass(.b).Psn.Y), Rgb(64, 160, 32)
End With
Next
'' draw particles
For i As Integer = Lbound(mass) To Ubound(mass)
With mass(i)
Circle(.Psn.X, .Psn.Y), .radius, rgb(0, 0, 0),,, 1, f
Circle(.Psn.X, .Psn.Y), .radius-1, rgb(255, 32, 32),,, 1, f
End With
Next
ScreenUnLock
Sleep 1, 1
Loop Until Multikey(1)
End
That's all for this time, hope you enjoyed it.
Cheers,
Michael