--- a/src/proto2/Physics.cc	Thu Nov 20 21:22:03 2008 +0000
+++ b/src/proto2/Physics.cc	Thu Nov 20 21:25:09 2008 +0000
@@ -30,16 +30,45 @@
 
     world.addObject(this);
 }
-    
-void PhysicsObject::updatePosition () {
 
-    // Calculate gravity's influence on the velocity vector
+/**
+ * Updates object speed and position. This function organises force
+ * integration and collision detection.
+ */   
+void PhysicsObject::updatePosition () {
+    // Add gravity to the force queue
+    forceq.push(Force(world.gravity, PHYSICS_TICK_MS));
+    
+    // Go trough every force in the queue
+    // TODO: It might be possible to optimize by adding forces together
+    std::queue<Force> newfq;
+    Force tmpf;
+    posAfterTick = position;
+    velAfterTick = velocity;
+    while (!forceq.empty()) {
+        tmpf = forceq.front();
+        if (tmpf.dt <= PHYSICS_TICK_MS) { // Force affects only one tick
+            integrate(tmpf.force, tmpf.dt);
+        } else { // Add remaining time to next tick
+            newfq.push(Force(tmpf.force, tmpf.dt - PHYSICS_TICK_MS));
+            integrate(tmpf.force, PHYSICS_TICK_MS);
+        }
+        forceq.pop();
+        //        Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Current position: " << posAfterTick;
+    }
+    forceq = newfq;
+
+    Vector newPosition = posAfterTick + (velAfterTick * PHYSICS_TICK_MS)/1000;
+    this->velocity = velAfterTick;
+    Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Nopeus: "<<this->velocity;
+    /*
     this->velocity += world.gravity * (PHYSICS_TICK_MS / 1000.0);
-        
+
     Vector newPosition = position + velocity * (PHYSICS_TICK_MS / 1000.0);
+    */
 
     //TODO Handle the object as a square or a polygon
-    
+
     bool collided = false;
 
     //goes 1 unit forward every step and check if has hit anything
@@ -75,6 +104,7 @@
         //TODO: it shouldn't just stop on collision
     }
     this->position = newPosition;
+
 }
 
 bool PhysicsWorld::collided (Vector oldPos, Vector newPos) {
@@ -96,17 +126,53 @@
     return false;
 }
 
+/**
+ * Integrates given force over time and stores new position to
+ * posAfterTick and new velocity to velAfterTick.
+ * @param force Force vector.
+ * @param dt The time the force is applied (<=PHYSICS_TICK_MS)
+ */
 void PhysicsObject::integrate(Vector force, TimeMS dt) {
-    // TODO
+    Derivative tmpd;
+    Derivative k1 = evaluate(force, 0, tmpd);
+    Derivative k2 = evaluate(force, 0.5f*dt, k1);
+    Derivative k3 = evaluate(force, 0.5f*dt, k2);
+    Derivative k4 = evaluate(force, dt, k3);
+    
+
+    const Vector dxdt = (k1.dx + (k2.dx + k3.dx) * 2.0f + k4.dx) * 1.0f/6.0f;
+    const Vector dvdt = (k1.dv + (k2.dv + k3.dv) * 2.0f + k4.dv) * 1.0f/6.0f;
+    
+    //    Engine::log(DEBUG, "PhysicsObject.integrate") << "Changes: "<< dxdt << " " << dvdt << " Time: " <<dt;
+    posAfterTick = posAfterTick + (dxdt * dt)/1000;
+    velAfterTick = velAfterTick + (dvdt * dt)/1000;
+    //Engine::log(DEBUG, "PhysicsObject.integrate") << "velAfterTick: " << velAfterTick;
 }
 
-void PhysicsObject::applyForce (Vector force, TimeMS dt) {
-    Vector oldVelocity = velocity;
+Derivative PhysicsObject::evaluate(Vector force, TimeMS dt, Derivative &d) {
+    Vector curPos = posAfterTick + (d.dx*dt)/1000;
+    Vector curVel = velAfterTick + (d.dv*dt)/1000;
 
-    this->velocity += force * dt / 1000 / mass;  // The last factor denotes the time.
-    // It should be scaled somehow.
-    
-//    Engine::log(DEBUG, "physics.apply_force") << "force=" << force << ", velocity " << oldVelocity << " -> " << velocity;
+    Derivative out;
+    out.dx = curVel;
+    out.dv = acceleration(force);
+    //Engine::log(DEBUG, "PhysicsObject.evaluate") << "Out.dx: " << out.dx;
+    return out;
+}
+
+Vector PhysicsObject::acceleration(const Vector &force) {
+    return (force/mass);
+}
+
+/**
+ * Adds force to the force queue. Force queue is emptied on each
+ * tick. Forces that last over one tick are also handled.
+ * @param force Force vector.
+ * @param dt The time the force is applied.
+ */
+void PhysicsObject::applyForce (Vector force, TimeMS dt) {
+    // Add applied force to the queue
+    forceq.push(Force(force, dt));
 }
 
 void PhysicsObject::updatePhysics (Vector position, Vector velocity) {

--- a/src/proto2/Physics.hh	Thu Nov 20 21:22:03 2008 +0000
+++ b/src/proto2/Physics.hh	Thu Nov 20 21:25:09 2008 +0000
@@ -4,6 +4,7 @@
 #include "Vector.hh"
 
 #include <vector>
+#include <queue>
 #include <ClanLib/core.h>
 
 typedef uint16_t TimeMS;
@@ -14,6 +15,8 @@
 
 // forward-declare
 class PhysicsObject;
+class Force;
+struct Derivative;
 
 class PhysicsWorld {
     friend class PhysicsObject;
@@ -45,13 +48,19 @@
 class PhysicsObject {
 protected:
     PhysicsWorld &world;
+    
     float mass;
     Vector position;
     Vector velocity;
     // Whether the object (worms mainly) is in the air 
     // or firmly on the ground. Affects to physics.
     bool inAir;
-    
+
+    // Force queue that is emptied on every tick
+    std::queue<Force> forceq;
+    Vector posAfterTick;
+    Vector velAfterTick;
+
     PhysicsObject (PhysicsWorld &world, float mass, Vector position, Vector velocity);
     
     virtual void applyForce (Vector force, TimeMS dt);
@@ -64,11 +73,29 @@
      * Use RK4 to integrate the effects of force over a time intervall.
      */
     void integrate(Vector force, TimeMS dt);
-    
+    Derivative evaluate(Vector force, TimeMS dt, Derivative &d);
+    Vector acceleration(const Vector &force);
+
 public:
     Vector getPosition (void);
     
     void tick (void);
 };
 
+class Force {
+public:
+    Vector force;
+    TimeMS dt;
+    
+    Force() {}
+    Force(Vector force, TimeMS dt) : force(force), dt(dt) {}
+};
+
+struct Derivative {
+    Vector dx; // Velocity
+    Vector dv; // Acceleration
+};
+
+
+
 #endif