9     : tick_timer(PHYSICS_TICK_MS), gravity(gravity), dimensions(dimensions) {

    10     : tick_timer(PHYSICS_TICK_MS), gravity(gravity), dimensions(dimensions), terrain(dimensions.x, std::vector<TerrainType>(dimensions.y, EMPTY)) {

    11 

    12 	generateTerrain(1337);

    22 	for (std::vector<PhysicsObject*>::iterator i = objects.begin(); i != objects.end(); i++) {

    25     for (std::vector<PhysicsObject*>::iterator i = objects.begin(); i != objects.end(); i++) {

    23        	(*i)->tick(); 

    26         (*i)->tick(); 

    24   	}

    27     }

    32     

    35 

    36 /**

    37  * Updates object speed and position. This function organises force

    38  * integration and collision detection.

    39  */   

    34 

    41     // Add gravity to the force queue

    35 	// Check if the player is moving on the ground

    42     forceq.push(world.gravity);

    36 	/*if (this->velocity.y == 0 && (position.y >= world.dimensions.y - 3)) {

    43     

    37     	position.x += 50 * velocity.x * (PHYSICS_TICK_MS / 1000.0);

    44     // Go trough every force in the queue

    38 		velocity.x = 0;

    45     // TODO: It might be possible to optimize by adding forces together

    39 		return;

    46     Force total;

    40 	}*/

    47     posAfterTick = position;

    41 

    48     velAfterTick = velocity;

    42 	// If not moving on the ground, apply normal physics

    49     while (!forceq.empty()) {

    43 

    50         total += forceq.front();

    44     // Calculate gravity's influence on the velocity vector

    51         forceq.pop();

    52         //        Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Current position: " << posAfterTick;

    53     }

    54     integrate(total, PHYSICS_TICK_MS);

    55 

    56     Vector newPosition = posAfterTick /*+ (velAfterTick * PHYSICS_TICK_MS)/1000*/;

    57     this->velocity = velAfterTick;

    58     //Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Nopeus: "<<this->velocity;

    59     /*

    46         

    61 

    54      

    68 

    55     if (newPosition.x < 0 || (newPosition.x > world.dimensions.x)) {

    69     //goes 1 unit forward every step and check if has hit anything

    56         // CRASH!

    70     Vector unitVector = (newPosition-position) / (newPosition-position).length();

    57         this->velocity.x *= -0.5;

    71     

    58 		

    72 	Vector tmpVector = position;

    59 		// If the velocity drops under some fixed constant we decide it is zero.

    73     Vector reached = position;

    60 		// This is to prevent the object from bouncing eternally.

    74 	int steps = (int) (newPosition-position).length();

    61 		if (abs(this->velocity.x) < 0.1)

    75     for(int i = 0; i < steps; i++) {

    62 			this->velocity.x = 0;

    76         tmpVector += unitVector;

    63 

    77         if(world.getType(tmpVector) != EMPTY) {

    64         collided = true;

    78 			//Engine::log(DEBUG, "physics.update_position") << "hit something";

    79             // Then we have hit something

    80             reached = position + unitVector*(i-1);

    81             collided = true;

    82             break;

    83         } else {

    84 			//Engine::log(DEBUG, "physics.update_position") << "didnt hit";

    85 		}

    86     }

    87 

    88     // In case of some float error

    89     if(!collided) {

    90         if(world.getType(newPosition)) {

    91             // There was error, and there is ground

    92             newPosition = tmpVector;

    93         } else {

    94             // This means everything was ok, so no need to do anything

    95         }

    66         this->position.x = newPosition.x;

    97         newPosition = reached;

    67     }

    98         this->velocity = Vector(0, 0);

    68     

    99         //TODO: it shouldn't just stop on collision

    69     if (newPosition.y <= 0 || (newPosition.y >= world.dimensions.y)) {

   100     }

    70 		this->velocity.y *= -0.3;

   101     this->position = newPosition;

    71 

   102 

    72 		

   103 }

    73  

   104 

    74 		if (abs(this->velocity.y) < 0.1) {

   105 bool PhysicsWorld::collided (Vector oldPos, Vector newPos) {

    75 			this->velocity.y = 0;

   106     int deltaX = oldPos.x - newPos.x; 

    76 			// Friction

   107     int deltaY = oldPos.y - newPos.y; 

    77 			this->velocity.x *= 0.95;

   108     double distance = sqrt(deltaX * deltaX + deltaY * deltaY);

    78 		} else {

   109     double xInc = deltaX / distance;

    79         	// Bigger friction

   110     double yInc = deltaY / distance;

    80 			this->velocity.x *= 0.75;

   111     double currentX = oldPos.x;

    81 		}

   112     double currentY = oldPos.y;

    82 

   113 

    83         collided = true;

   114     // This implementation is bit slow since it checks some squares twice.

    84 	} else {

   115     for(unsigned int i = 1; i < distance; i++) {

    85         this->position.y = newPosition.y;

   116         currentX += xInc;

    86 	}

   117         currentY += yInc;

    87     

   118         if(terrain[(int)currentX][(int)currentY] != EMPTY)

    88     if(!collided) {

   119             return true;

    89         this->position = newPosition;

   120     }

    90     }

   121     return false;

    91 }

   122 }

    92 

   123 

    93 void PhysicsObject::applyForce (Vector force, uint16_t dt) {

   124 /**

    94     Vector oldVelocity = velocity;

   125  * Integrates given force over time and stores new position to

    95 

   126  * posAfterTick and new velocity to velAfterTick.

    96     this->velocity += force * dt / 1000 / mass;  // The last factor denotes the time.

   127  * @param force Force vector.

    97     // It should be scaled somehow.

   128  * @param dt The time the force is applied (<=PHYSICS_TICK_MS)

    98     

   129  */

    99 //    Engine::log(DEBUG, "physics.apply_force") << "force=" << force << ", velocity " << oldVelocity << " -> " << velocity;

   130 void PhysicsObject::integrate(Force force, TimeMS dt) {

   131     Derivative tmpd;

   132     Derivative k1 = evaluate(force, 0, tmpd);

   133     Derivative k2 = evaluate(force, 0.5f*dt, k1);

   134     Derivative k3 = evaluate(force, 0.5f*dt, k2);

   135     Derivative k4 = evaluate(force, dt, k3);

   136     

   137 

   138     const Vector dxdt = (k1.dx + (k2.dx + k3.dx) * 2.0f + k4.dx) * 1.0f/6.0f;

   139     const Vector dvdt = (k1.dv + (k2.dv + k3.dv) * 2.0f + k4.dv) * 1.0f/6.0f;

   140     

   141     //    Engine::log(DEBUG, "PhysicsObject.integrate") << "Changes: "<< dxdt << " " << dvdt << " Time: " <<dt;

   142     posAfterTick = posAfterTick + (dxdt * dt)/1000;

   143     velAfterTick = velAfterTick + (dvdt * dt)/1000;

   144     //Engine::log(DEBUG, "PhysicsObject.integrate") << "velAfterTick: " << velAfterTick;

   145 }

   146 

   147 Derivative PhysicsObject::evaluate(Force force, TimeMS dt, Derivative &d) {

   148     Vector curPos = posAfterTick + (d.dx*dt)/1000;

   149     Vector curVel = velAfterTick + (d.dv*dt)/1000;

   150 

   151     Derivative out;

   152     out.dx = curVel;

   153     out.dv = acceleration(force);

   154     //Engine::log(DEBUG, "PhysicsObject.evaluate") << "Out.dx: " << out.dx;

   155     return out;

   156 }

   157 

   158 Vector PhysicsObject::acceleration(const Force &force) {

   159     return (force/mass);

   160 }

   161 

   162 /**

   163  * Adds force to the force queue. Force queue is emptied on each

   164  * tick. Forces that last over one tick are also handled.

   165  * @param force Force vector.

   166  * @param dt The time the force is applied.

   167  */

   168 void PhysicsObject::applyForce (Force force, TimeMS dt) {

   169     // Add applied force to the queue

   170     forceq.push(force);