Anyone know the formula used to control the movement of a simple tank-like vehicle?
To 'steer' it, you need to alter the force applied the left and right "wheels". Eg. 1 unit of force on both wheels makes it go forward. -1 units of force on both wheels makes it go backwards. Apply more force to one wheel than the other and it turns.
How would you calculate the amount of force needed on both wheels to turn the tank a certain number of degrees either way?
Or am I thinking about this in the wrong way?
edit:
As William Keller mentioned I missed out the speed of the tank. Assume 1 unit of force on both wheels moves the tank forward at 1 unit per second.
For anyone who's interested, I just found this thread on gamedev.net:
http://66.102.9.104/search?q=cache:wSn5t58ACJwJ:www.gamedev.net/community/forums/topic.asp%3Ftopic_id%3D407491+tank+track+radius+velocity&hl=en&ct=clnk&cd=1&gl=za&client=firefox-a
Another thread:
http://www.physicsforums.com/showthread.php?t=220317
It turns out the key to finding the formula was just knowing the correct terminology ("skid steer") :P
For a skid steered vehicle that is required to turn in radius 'r' at a given speed 'Si' of the Inner Wheel/Track, the Outer track must be driven at speed 'So' :
So = Si * ((r+d)/r)
Details:
In Skid Steering, a turn is performed by the outer wheels/track traveling further distance than the inner wheels/track.
Furthermore, the extra distance traveled is completed in the same time as the inner track, meaning that the outer wheels/track must run faster.
Circle circumference circumscribed by "Inner" track:
c1 = 2*PI*r
'r' is radius of circle origin to track/wheel
Circle circumference circumscribed by "Outer" track:
c2 = 2*PI*(r+d)
'r' is radius of circle origin to inner track/wheel
'd' is the distance between the Inner and Outer wheels/track.
Furthermore, c2 = X * c1, which says that c2 is proportionally bigger than c1
X = c2 / c1
X = 2*PI*(r+d) / 2*PI*r
X = (r+d)/r
Therefore for a skid steered vehicle that is required to turn in radius 'r' at a given speed 's' of the Inner Wheel/Track, the Outer track must be driven at :
So = Si * ((r+d)/r)
Where:
'So' = Speed of outer track
'Si' = Speed of inner track
'r' = turn radius from inner track
'd' = distance between vehicle tracks.
********* <---------------- Outer Track
**** | ****
** |<--------**----------- 'd' Distance between tracks
* *******<-------*---------- Inner Track
* *** ^ *** *
* * |<-----*------*-------- 'r' Radius of Turn
* * | * *
* * O * *
* * * *
* * * *
* *** *** *
* ******* *
** **
**** ****
*********
You're thinking about it the wrong way. The thing is, differing amounts of force on the tracks will not turn the tank a certain number of degrees. Rather, differing force will alter the RATE of turn.
The relationship between the force and the turn rate will vary depending on the mechanics of the tank. The wider the tank the slower it turns. The faster the tank the faster it turns.
P.S. Some more thoughts on this: I don't think a physics-based answer is possible without basing it off a real-world tank. Several of the answers address the physics of the turn but there is the implicit assumption in all of them that the system has infinite power. Can the tank really operate at 1, -1? And can it reach that velocity instantly--acceleration applies to turns, also.
Finally, treads have length as well as width. That means you are going to get some sideways slippage of the treads in any turning situation, the faster the turn the more such slippage will be required. That is going to burn up energy in a sharp turn, even if the engine has the power to do a 1, -1 turn it wouldn't turn as fast as that would indicate because of friction losses.
Change in angle (in radians/sec) = (l-r)/(radius between treads)
Velocity = l+r
For the dtheta, imagine you had a wooden pole between your two hands, and you want to calculate how much it rotates depending on how hard and which way your hands are pressing - you want to figure out:
how much surface distance on the pole you cover per sec -> how many rotations/sec that is -> how many radians/sec (i.e. mult by 2pi)
Well, keep in mind that you're also talking about duration here. You need to find out the forces taking in to account the speed at which the tank turns at (1, -1).
I.E., if the tank takes one second to spin 360˚ at (1, -1), and you want to spin 180˚ in one second, (.5, -.5) would do the trick. If you wanted to spin the same amount in half a second, then (1, -1) would work.
This is all further complicated if you use abs(lrate) != abs(rrate), in which case you'll probably need to break out a pencil!
Here's how I would attack the tank problem.
The center of the tank will probably be moving by the average speed of the right and left tracks. At the same time, the tank will be rotating clockwise around it's center by ([left track speed] * -[right track speed]) / [width].
This should give you speed and a direction vector.
Disclaimer: I have not tested this...
It has been a while since I did any physics but I would have thought that the apposing forces of the two tracks moving in opposite directions results in a torque about the center of mass of the tank.
It is this torque that results in the angular momentum of the tank which is just another way of saying the tank starts to rotation.
I'd say you're thinking about it in the wrong way.
Increasing the difference in speed between the two treads doesn't cause degrees of turn - they, combined with time (distance at different speed) cause degrees of turn.
The more of a difference in tread speed, the less time needed to achieve X degrees of turn.
So, in order to come up with a formula, you'll have to make a few assumptions. Either turn at a fixed rate, and use time as your variable for turning X degrees, or set a fixed amount of time to complete a turn, and use the track speed difference as your variable.
It's not a matter of force - it depends on the difference in velocity between the 2 sides, and how long that difference holds (also the tank's width, but that's just a constant parameter).
Basically, you should calculate it along these lines:
The velocity ratio between the 2 sides is the same as the radius ratio.
The tank's width is the actual difference between the 2 rasiuses (sp?).
Using those 2 numbers, find the actual values for the radius.
Multiply the velocity of one of the sides by the time it was moving to get the distance it traveled.
Calculate what part of a full circle it traveled by dividing that into that circle's perimeter.
You could look at it by saying : each track describes a circle.
In the case where one track is turning (lets say the left) and the other isn't, then the facing will be dependant on how long and how far the left tracks turn for.
This distance will be the speed of the tracks x time.
Now draw a triangle with this distance, and the wheelbase pencilled in, plus some sin and cos equations & approximations, and you might get an approximate equation like :
facing change = distance travelled by tracks / wheelbase
Then you could incorporate some accelleration to be more realistic: More physics...
The speed isn't constant - it accellerates (and decellerates).
ie over a period of time the speed increases from 0, to 0.1 ... 0.2 ... 0.3 up to a limit.
Of course, as the speed changes, the rate of change of the facing changes too - a bit more realistic than the speed (and thus rate of change of the facing) being entirely constant.
In other words, instead of controlling the speed, the player controls the change in speed. This would make the speed go from 0 ... 0.02 ... 0.06 ... 0.1 etc. as the player pushes the controller. Similarly for decelleration, but a bit more rapidly probably.
hope this helps.
I think everyone should also take note of the fact that tanks can turn on a Zero-radius circle: by applying the same speed on each track but on opposite directions, tanks can turn on a dime.
Related
I want to develop a game where the universe is a maximum 65536 x 65536 grid. I do not want the universe to be random, what I want is for it to be procedurally generated according to location. What it should generate is a number from 0 to 15.
0 means empty space. Most of the universe (probably 50-80%) is empty space.
1 - 9 a planet of that technology level
10-15 various anomalies (black hole, star, etc.)
Given an address from 0x8000-0xFFFF or 0 or 1-0x7fff for the X address, and the same range for the Y address, returns a number from 0 to 15. Presumably this would place planets nearer to 0,0 more plentiful than those at a distance of
The idea being, the function is called passing the two values and returns the planet number. I used to have a function to do this, but it has gotten lost over various moves.
While the board could be that big, considering how easy it would be to get lost, I'll probably cut the size to 1200 in both directions, -600 to +600. Even that would be huge.
I've tried a number of times, but I've come to the conclusion that I lack the sufficient math skills to do this. It's probably no more than 10 lines. As it is intended to be multiplayer, it'll probably be either a PHP application on the back end or a desktop application connecting to a server.
Any help would be appreciated. I can probably read any commonly used programming language you might use.
Paul Robinson
See How to draw sky chart? for the planetary position math. Especially pay attention to the image with equations you can use it to compute period of your planet based on its distance and mass to system central mass... for simple circular orbit just match the centripedal force with gravity like I did in here:
Is it possible to make realistic n-body solar system simulation in matter of size and mass?
So for example:
G = 6.67384e-11;
v = sqrt(G*M/a); // orbital speed
T = sqrt((4.0*M_PI*M_PI*a*a*a)/(G*(m+M))); // orbital period
pos = (a,0,0); // start position
vel = (0,sqrt(G*M/a),0); // start speed
The distribution of planets and their sizes complies specific (empirically obtained) rules (that is one of the reasons why we are still looking for 10th planet). I can't remember the name of the rule, however from quick look on Google Image from here can be used too:
Distribution of the planets in solar system according to their mass
and their distances from the Sun. The distances (X-axis) are in AU and
masses (Y-axis) in Yotta (10^24)
Jupiter mass is M=1,898e27 kg so mass units are in 10^24 kg.
So just match your PRNG generation to such curve and be done with it.
My question may seem trivial, but the more I read about it - the more confused I get... I have started a little project where I want to roughly track the movements of a rotating object. (A basketball to be precise)
I have a 3-axis accelerometer (low-pass-filtered) and a 3-axis gyroscope measuring °/s.
I know about the issues of a gyro, but as the measurements will only be several seconds and the angles tend to be huge - I don't care about drift and gimbal right now.
My Gyro gives me the rotation speed of all 3 axis. As I want to integrate the acceleration twice to get the position at each timestep, I wanted to convert the sensors coordinate-system into an earthbound system.
For the first try, I want to keep things simple, so I decided to go with the big standard rotation matrix.
But as my results are horrible I wonder if this is the right way to do so. If I understood correctly - the matrix is simply 3 matrices multiplied in a certain order. As rotation of a basketball doesn't have any "natural" order, this may not be a good idea. My sensor measures 3 angular velocitys at once. If I throw them into my system "step by step" it will not be correct since my second matrix calculates the rotation around the "new y-axis" , but my sensor actually measured an angular velocity around the "old y-axis". Is that correct so far?
So how can I correctly calculate the 3D rotation?
Do I need to go for quaternoins? but how do I get one from 3 different rotations? And don't I have the same issue here again?
I start with a unity-matrix ((1, 0, 0)(0, 1, 0)(0, 0, 1)) multiplied with the acceleration vector to give me the first movement.
Then I want use the Rotation matrix to find out, where the next acceleration is really heading so I can simply add the accelerations together.
But right now I am just too confused to find a proper way.
Any suggestions?
btw. sorry for my poor english, I am tired and (obviously) not a native speaker ;)
Thanks,
Alex
Short answer
Yes, go for quaternions and use a first order linearization of the rotation to calculate how orientation changes. This reduces to the following pseudocode:
float pose_initial[4]; // quaternion describing original orientation
float g_x, g_y, g_z; // gyro rates
float dt; // time step. The smaller the better.
// quaternion with "pose increment", calculated from the first-order
// linearization of continuous rotation formula
delta_quat = {1, 0.5*dt*g_x, 0.5*dt*g_y, 0.5*dt*g_z};
// final orientation at start time + dt
pose_final = quaternion_hamilton_product(pose_initial, delta_quat);
This solution is used in PixHawk's EKF navigation filter (it is open source, check out formulation here). It is simple, cheap, stable and accurate enough.
Unit matrix (describing a "null" rotation) is equivalent to quaternion [1 0 0 0]. You can get the quaternion describing other poses using a suitable conversion formula (for example, if you have Euler angles you can go for this one).
Notes:
Quaternions following [w, i, j, k] notation.
These equations assume angular speeds in SI units, this is, radians per second.
Long answer
A gyroscope describes the rotational speed of an object as a decomposition in three rotational speeds around the orthogonal local axes XYZ. However, you could equivalently describe the rotational speed as a single rate around a certain axis --either in reference system that is local to the rotated body or in a global one.
The three rotational speeds affect the body simultaneously, continously changing the rotation axis.
Here we have the problem of switching from the continuous-time real world to a simpler discrete-time formulation that can be easily solved using a computer. When discretizing, we are always going to introduce errors. Some approaches will lead to bigger errors, while others will be notably more accurate.
Your approach of concatenating three simultaneous rotations around orthogonal axes work reasonably well with small integration steps (let's say smaller than 1/1000 s, although it depends on the application), so that you are simulating the continuous change of rotation axis. However, this is computationally expensive, and error grows as you make time steps bigger.
As an alternative to first-order linearization, you can calculate pose increments as a small delta of angular speed gradient (also using quaternion representation):
quat_gyro = {0, g_x, g_y, g_z};
q_grad = 0.5 * quaternion_product(pose_initial, quat_gyro);
// Important to normalize result to get unit quaternion!
pose_final = quaternion_normalize(pose_initial + q_grad*dt);
This technique is used in Madgwick rotation filter (here an implementation), and works pretty fine for me.
I'm making an object which always faces mouse position with smooth transition - acceleration and deceleration.
For decelerating near mouse it's an easy equation:
rotation += deltaRotation / speed
For accelerating there's a little more code (but it wobbles around target rotation, as there's no friction or damping)
var deltaRotation = rotation - targetRotation;
if (Math.abs(deltaRotation) < EPSILON) { //Stop motion near 0
return;
}
var direction = deltaRotation != 0 ? deltaRotation / Math.abs(deltaRotation) : 0;
dv += ACCELERATION * direction * dt;
dv = FMath.clamp(dv, -MAX_SPEED, MAX_SPEED);
rotation += dv * deltaRotation;
How to merge these both algorithms, so the rotation will slowly start and slowly reach target rotation?
In the physical world, the acceleration is proportional to force applied. One way to model the force applied at a given time step is to make it proportional to distance you need to object to move, minus some damping force. The damping force (like friction or wind resistance) is in the opposite direction of and proportional to the current velocity. The damping force effectively limits the top speed and gives a nice deceleration as the direct force drops off.
Consider a linear system. Let's say xCursor is the position of the cursor, and xMonster is the position of the thing chasing the cursor. Let's use dxMonster for the monster's velocity.
First we need to know how far away we are, because we want to accelerate the monster according to that.
float delta = xCursor - xMonster;
force is the net force. It includes a direct force that accelerates the monster toward the cursor and a damping force that resists the monster's current velocity.
float force = A*delta - B*dxMonster;
You can play with the constants to see what works best. You probably want A < B. In my test program, I used A = 0.1 and B = 0.5.
Since acceleration is proportional to force, we can add it directly to the velocity. Note that we no longer need to clamp the velocity, since the damping force will keep it from growing too large. The velocity is then used to update the position. (In a better simulation, you can approximate this better using something like Runge-Kutta to do the integration, but simply adding at each time step is fine for a simple animation.)
dxMonster += force;
xMonster += dxMonster;
Repeat for each timestep. I omitted the dts because my frame rate was stable enough that it didn't matter much.
Your problem is about rotation rather than linear motion, but that's completely analogous. In physical terms, it's just angular velocity and torque rather than linear velocity and force.
Currently I'm using Math.cos and Math.sin to move objects in a circle in my game, however I suspect it's slow (didn't make proper tests yet though) after reading a bit about it.
Are there any ways to calculate this in a faster way?. Been reading that one alternative could be to have a sort of hash table with stored pre-calculated results, like old people used it in the old times before the computer age.
Any input is appreciated.
Expanding on my comment, if you don't have any angular acceleration (the angular velocity stays constant -- this is a requirement for the object to remain traveling in a circle with constant radius without changing the center-pointing force, e.g. via tension in a string), then you can use the following strategy:
1) Compute B = angular_velocity * time_step_size. This is how much angle change the object needs to go through in a single time step.
2) Compute sinb = sin(B) and cosb = cos(B).
3)
Note that we want to change the angle from A to A+B (the object is going counterclockwise). In this derivation, the center of the circle we're orbiting is given by the origin.
Since the radius of the circle is constant, we know r*sin(A+B) = y_new = r*sin(A)cos(B) + r*cos(A)sin(B) = y_old * cos(B) + x_old*sin(B) and r*cos(A+B) = x_new = r*cos(A)*cos(B) - r*sin(A)sin(B) = x_old*cos(B) - y_old*sin(B).
We've removed the cosine and sine of anything we don't already know, so the Cartesian coordinates can be written as
x_new = x_old*cosb - y_old*sinb
y_new = x_old*sinb + y_old*cosb
No more cos or sin calls except in an initialization step which is called once. Obviously, this won't save you anything if B keeps changing for whatever reason (either angular velocity or time step size changes).
You'll notice this is the same as multiplying the position vector by a fixed rotation matrix. You can translate by the circle center and translate back if you don't want to only consider circles with a center at the origin.
First Edit
As #user5428643 mentions, this method is numerically unstable over time due to drift in the radius. You can probably correct this by periodically renormalizing x and y (x_new = x_old * r_const / sqrt(x_old^2 + y_old^2) and similarly for y every few thousand steps -- if you implement this, save the factor r_const / sqrt(x_old^2 + y_old^2) since it is the same for both x and y). I'll think about it some more and edit this answer if I come up with a better fix.
Second Edit
Some more comments on the numerical drift over time:
I did a couple of tests in C++ and python. In C++ using single precision floats, there is sizable drift even after 1 million time steps when B = 0.1. I used a circle with radius 1. In double precision, I didn't notice any drift visually after 100 million steps, but checking the radius shows that it is contaminated in the lower few digits. Doing the renormalization on every step (which is unnecessary if you're just doing visualization) results in an approximately 4 times slower running time versus the drifty version. However, even this version is about 2-3 times faster than using sin and cos on every iteration. I used full optimization (-O3) in g++. In python (using the math package) I only got a speed up of 2 between the drifty and normalized versions, however the sin and cos version actually slots in between these two -- it's almost exactly halfway between these two in terms of run time. Renormalizing every once in a few thousand steps would still make this faster, but it's not nearly as big a difference as my C++ version would indicate.
I didn't do too much scientific testing to get the timings, just a few tests with 1 million to 1 billion steps in increments of 10.
Sorry, not enough rep to comment.
The answers by #neocpp and #oliveryas01 would both be perfectly correct without roundoff error.
The answer by #oliveryas01, just using sine and cosine directly, and precalculating and storing many values if necessary, will work fine.
However, #neocpp's answer, repeatedly rotating by small angles using a rotation matrix, is numerically unstable; over time, the roundoff error in the radius will tend to grow exponentially, so if you run your programme for a long time the objects will slowly move off the circle, spiralling either inwards or outwards.
You can see this mathematically with a little numerical analysis: at each stage, the squared radius is approximately multiplied by a number which is approximately constant and approximately equal to 1, but almost certainly not exactly equal to 1 due to inexactness of floating point representations.
If course, if you're using double precision numbers and only trying to achieve a simple visual effect, this error may not be large enough to matter to you.
I would stick with sine and cosine if I were you. They're the most efficient way to do what you're trying to do. If you really want maximum performance then you should generate an array of x and y values from the sine and cosine values, then plug that array's values into the circle's position. This way, you aren't running sine and cosine repeatedly, instead only for one cycle.
Another possibility completely avoiding the trig functions would be use a polar-coordinate model, where you set the distance and angle. For example, you could set the x coordinate to be the distance, and the rotation to be the angle, as in...
var gameBoardPin:Sprite = new Sprite();
var gameEntity:Sprite = new YourGameEntityHere();
gameBoardPin.addChild( gameEntity );
...and in your loop...
// move gameEntity relative to the center of gameBoardPin
gameEntity.x = circleRadius;
// rotate gameBoardPin from its center causes gameEntity to rotate at the circleRadius
gameBoardPin.rotation = desiredAngleForMovingObject
gameBoardPin's x,y coordinates would be set to the center of rotation for gameEntity. So, if you wanted the gameEntity to rotate with a 100 pixel tether around the center of the stage, you might...
gameBoardPin.x = stage.stageWidth / 2;
gameBoardPin.y = stage.stageHeight / 2;
gameEntity.x = 100;
...and then in the loop you might...
desiredAngleForMovingObject += 2;
gameBoardPin.rotation = desiredAngleForMovingObject
With this method you're using degrees instead of radians.
I try to tilt compensate a magnetometer (BMX055) reading and tried various approaches I have found online, not a single one works.
I atually tried almost any result I found on Google.
I run this on an AVR, it would be extra awesome to find something that works without complex functions (trigonometry etc) for angles up to 50 degree.
I have a fused gravity vector (int16 signed in a float) from gyro+acc (1g gravity=16k).
attitude.vect_mag.x/y/z is a float but contains a 16bit integer ranging from around -250 to +250 per axis.
Currently I try this code:
float rollRadians = attitude.roll * DEG_TO_RAD / 10;
float pitchRadians = attitude.pitch * DEG_TO_RAD / 10;
float cosRoll = cos(rollRadians);
float sinRoll = sin(rollRadians);
float cosPitch = cos(pitchRadians);
float sinPitch = sin(pitchRadians);
float Xh = attitude.vect_mag.x * cosPitch + attitude.vect_mag.z * sinPitch;
float Yh = attitude.vect_mag.x * sinRoll * sinPitch + attitude.vect_mag.y * cosRoll - attitude.vect_mag.z *sinRoll * cosPitch;
float heading = atan2(Yh, Xh);
attitude.yaw = heading*RAD_TO_DEG;
The result is meaningless, but the values without tilt compensation are correct.
The uncompensated formula:
atan2(attitude.vect_mag.y,attitude.vect_mag.x);
works fine (when not tilted)
I am sort of clueless what is going wrong, the normal atan2 returns a good result (when balanced) but using the wide spread formulas for tilt compensation completely fails.
Do I have to keep the mag vector values within a specific range for the trigonometry to work ?
Any way to do the compensation without trig functions ?
I'd be glad for some help.
Update:
I found that the BMX055 magnetometer has X and Y inverted as well as Y axis is *-1
The sin/cos functions now seem to lead to a better result.
I am trying to implement the suggested vector algorithms, struggling so far :)
Let us see.
(First, forgive me a bit of style nagging. The keyword volatile means that the variable may change even if we do not change it ourselves in our code. This may happen with a memory position that is written by another process (interrupt request in AVR context). For the compiler volatile means that the variable has to be always loaded and stored into memory when used. See:
http://en.wikipedia.org/wiki/Volatile_variable
So, most likely you do not want to have any attributes to your floats.)
Your input:
three 12-bit (11 bits + sign) integers representing accelerometer data
three approximately 9-bit (8 bits + sign) integers representing the magnetic field
Good news (well...) is that your resolution is not that big, so you can use integer arithmetics, which is much faster. Bad news is that there is no simple magical one-liner which would solve your problem.
First of all, what would you like to have as the compass bearing when the device is tilted? Should the device act as if it was not tilted, or should it actually show the correct projection of the magnetic field lines on the screen? The latter is how an ordinary compass acts (if the needle moves at all when tilted). In that case you should not compensate for anything, and the device can show the fancy vertical tilt of the magnetic lines when rolled sideways.
In any case, try to avoid trigonometry, it takes a lot of code space and time. Vector arithmetics is much simpler, and most of the time you can make do with multiplys and adds.
Let us try to define your problem in vector terms. Actually you have two space vectors to start with, m pointing to the direction of the magnetic field, g to the direction of gravity. If I have understood your intention correctly, you need to have vector d which points along some fixed direction in the device. (If I think of a mobile phone, d would be a vector parallel to the screen left or right edges.)
With vector mathematics this looks rather simple:
g is a normal to a horizontal (truly horizontal) plane
the projection of m on this plane defines the direction a horizontal compass would show
the projection of d on the plane defines the "north" on the compass face
the angle between m and d gives the compass bearing
Now that we are not interested in the magnitude of the magnetic field, we can scale everything as we want. This reduces the need to use unity vectors which are expensive to calculate.
So, the maths will be something along these lines:
# projection of m on g (. represents dot product)
mp := m - g (m.g) / (g.g)
# projection of d on g
dp := d - g (d.g) / (g.g)
# angle between mp and dp
cos2 := (mp.dp)^2 / (mp.mp * dp.dp)
sgn1 := sign(mp.dp)
# create a vector 90 rotated from d on the plane defined by g (x is cross product)
drot := dp x g
sin2 := (mp.drot)^2 / (mp.mp * drot.drot)
sgn2 := sign(mp.drot)
After this you will have a sin^2 and cos^2 of the compass directions. You need to create a resolving function for one quadrant and then determine the correct quadrant by using the signs. The resolving function may sound difficult, but actually you just need to create a table lookup function for sin2/cos2 or cos2/sin2 (whichever is smaller). It is relatively fast, and only a few points are required in the lookup (with bilinear approximation even fewer).
So, as you can see, there are no trig functions around, and even no square roots around. Vector dots and crosses are just multiplys. The only slightly challenging trick is to scale the fixed point arithmetics to the correct scale in each calculation.
You might notice that there is a lot of room for optimization, as the same values are used several times. The first step is to get the algorithm run on a PC with floating point with the correct results. The optimizations come later.
(Sorry, I am not going to write the actual code here, but if there is something that needs clarifying, I'll be glad to help.)