terom@35: 
terom@35: #include "Physics.hh"
terom@35: #include "Engine.hh"
terom@35: 
terom@35: #include <algorithm>
terom@35: #include <functional>
terom@41: #include <cmath>
terom@35: 
terom@35: PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions)
terom@41:     : tick_timer(PHYSICS_TICK_MS), gravity(gravity), dimensions(dimensions), terrain(dimensions.x, std::vector<TerrainType>(dimensions.y, EMPTY)) {
terom@41: 
terom@41: 	generateTerrain(1337);
terom@35: 
terom@35:     slots.connect(tick_timer.sig_timer(), this, &PhysicsWorld::tick);
terom@35:     tick_timer.enable();
terom@35: }
terom@35: 
terom@35: void PhysicsWorld::addObject (PhysicsObject *object) {
terom@35:     objects.push_back(object);
terom@35: }
terom@35: 
terom@35: void PhysicsWorld::tick () {
terom@35: //    Engine::log(DEBUG, "physics.apply_force") << "*tick*";
terom@35: 
terom@41:     for (std::vector<PhysicsObject*>::iterator i = objects.begin(); i != objects.end(); i++) {
terom@41:         (*i)->tick(); 
terom@41:     }
terom@35: }
terom@35: 
terom@35: PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, Vector position, Vector velocity)
terom@35:     : world(world), mass(mass), position(position), velocity(velocity) {
terom@35: 
terom@35:     world.addObject(this);
terom@35: }
terom@35: 
terom@41: /**
terom@41:  * Updates object speed and position. This function organises force
terom@41:  * integration and collision detection.
terom@41:  */   
terom@41: void PhysicsObject::updatePosition () {
terom@41:     // Add gravity to the force queue
terom@41:     forceq.push(world.gravity);
terom@41:     
terom@41:     // Go trough every force in the queue
terom@41:     // TODO: It might be possible to optimize by adding forces together
terom@41:     Force total;
terom@41:     posAfterTick = position;
terom@41:     velAfterTick = velocity;
terom@41:     while (!forceq.empty()) {
terom@41:         total += forceq.front();
terom@41:         forceq.pop();
terom@41:         //        Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Current position: " << posAfterTick;
terom@41:     }
terom@41:     integrate(total, PHYSICS_TICK_MS);
terom@35: 
terom@41:     Vector newPosition = posAfterTick /*+ (velAfterTick * PHYSICS_TICK_MS)/1000*/;
terom@41:     this->velocity = velAfterTick;
terom@41:     //Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Nopeus: "<<this->velocity;
terom@41:     /*
terom@41:     this->velocity += world.gravity * (PHYSICS_TICK_MS / 1000.0);
terom@35: 
terom@35:     Vector newPosition = position + velocity * (PHYSICS_TICK_MS / 1000.0);
terom@41:     */
terom@35: 
terom@35:     //TODO Handle the object as a square or a polygon
terom@35: 
terom@35:     bool collided = false;
terom@35: 
terom@41:     //goes 1 unit forward every step and check if has hit anything
terom@41:     Vector unitVector = (newPosition-position) / (newPosition-position).length();
terom@41:     
terom@41: 	Vector tmpVector = position;
terom@41:     Vector reached = position;
terom@41: 	int steps = (int) (newPosition-position).length();
terom@41:     for(int i = 0; i < steps; i++) {
terom@41:         tmpVector += unitVector;
terom@41:         if(world.getType(tmpVector) != EMPTY) {
terom@41: 			//Engine::log(DEBUG, "physics.update_position") << "hit something";
terom@41:             // Then we have hit something
terom@41:             reached = position + unitVector*(i-1);
terom@41:             collided = true;
terom@41:             break;
terom@41:         } else {
terom@41: 			//Engine::log(DEBUG, "physics.update_position") << "didnt hit";
terom@35: 		}
terom@41:     }
terom@35: 
terom@41:     // In case of some float error
terom@35:     if(!collided) {
terom@41:         if(world.getType(newPosition)) {
terom@41:             // There was error, and there is ground
terom@41:             newPosition = tmpVector;
terom@41:         } else {
terom@41:             // This means everything was ok, so no need to do anything
terom@41:         }
terom@41:     } else {
terom@41:         newPosition = reached;
terom@41:         this->velocity = Vector(0, 0);
terom@41:         //TODO: it shouldn't just stop on collision
terom@35:     }
terom@41:     this->position = newPosition;
terom@41: 
terom@35: }
terom@35: 
terom@41: bool PhysicsWorld::collided (Vector oldPos, Vector newPos) {
terom@41:     int deltaX = oldPos.x - newPos.x; 
terom@41:     int deltaY = oldPos.y - newPos.y; 
terom@41:     double distance = sqrt(deltaX * deltaX + deltaY * deltaY);
terom@41:     double xInc = deltaX / distance;
terom@41:     double yInc = deltaY / distance;
terom@41:     double currentX = oldPos.x;
terom@41:     double currentY = oldPos.y;
terom@35: 
terom@41:     // This implementation is bit slow since it checks some squares twice.
terom@41:     for(unsigned int i = 1; i < distance; i++) {
terom@41:         currentX += xInc;
terom@41:         currentY += yInc;
terom@41:         if(terrain[(int)currentX][(int)currentY] != EMPTY)
terom@41:             return true;
terom@41:     }
terom@41:     return false;
terom@41: }
terom@41: 
terom@41: /**
terom@41:  * Integrates given force over time and stores new position to
terom@41:  * posAfterTick and new velocity to velAfterTick.
terom@41:  * @param force Force vector.
terom@41:  * @param dt The time the force is applied (<=PHYSICS_TICK_MS)
terom@41:  */
terom@41: void PhysicsObject::integrate(Force force, TimeMS dt) {
terom@41:     Derivative tmpd;
terom@41:     Derivative k1 = evaluate(force, 0, tmpd);
terom@41:     Derivative k2 = evaluate(force, 0.5f*dt, k1);
terom@41:     Derivative k3 = evaluate(force, 0.5f*dt, k2);
terom@41:     Derivative k4 = evaluate(force, dt, k3);
terom@35:     
terom@41: 
terom@41:     const Vector dxdt = (k1.dx + (k2.dx + k3.dx) * 2.0f + k4.dx) * 1.0f/6.0f;
terom@41:     const Vector dvdt = (k1.dv + (k2.dv + k3.dv) * 2.0f + k4.dv) * 1.0f/6.0f;
terom@41:     
terom@41:     //    Engine::log(DEBUG, "PhysicsObject.integrate") << "Changes: "<< dxdt << " " << dvdt << " Time: " <<dt;
terom@41:     posAfterTick = posAfterTick + (dxdt * dt)/1000;
terom@41:     velAfterTick = velAfterTick + (dvdt * dt)/1000;
terom@41:     //Engine::log(DEBUG, "PhysicsObject.integrate") << "velAfterTick: " << velAfterTick;
terom@41: }
terom@41: 
terom@41: Derivative PhysicsObject::evaluate(Force force, TimeMS dt, Derivative &d) {
terom@41:     Vector curPos = posAfterTick + (d.dx*dt)/1000;
terom@41:     Vector curVel = velAfterTick + (d.dv*dt)/1000;
terom@41: 
terom@41:     Derivative out;
terom@41:     out.dx = curVel;
terom@41:     out.dv = acceleration(force);
terom@41:     //Engine::log(DEBUG, "PhysicsObject.evaluate") << "Out.dx: " << out.dx;
terom@41:     return out;
terom@41: }
terom@41: 
terom@41: Vector PhysicsObject::acceleration(const Force &force) {
terom@41:     return (force/mass);
terom@41: }
terom@41: 
terom@41: /**
terom@41:  * Adds force to the force queue. Force queue is emptied on each
terom@41:  * tick. Forces that last over one tick are also handled.
terom@41:  * @param force Force vector.
terom@41:  * @param dt The time the force is applied.
terom@41:  */
terom@41: void PhysicsObject::applyForce (Force force, TimeMS dt) {
terom@41:     // Add applied force to the queue
terom@41:     forceq.push(force);
terom@35: }
terom@35: 
terom@35: void PhysicsObject::updatePhysics (Vector position, Vector velocity) {
terom@35:     this->position = position;
terom@35:     this->velocity = velocity;
terom@35: }
terom@35:     
terom@35: Vector PhysicsObject::getPosition () {
terom@35:     return this->position;
terom@35: }
terom@35: 
terom@35: void PhysicsObject::tick () {
terom@35:     this->updatePosition();
terom@35: }
terom@35: 
terom@41: /**
terom@41:  * simple random map generation
terom@41:  * first fills whole level with dirt
terom@41:  * then randomizes circles of empty or rock
terom@41:  * @param seed - seed number for random number generator
terom@41:  */
terom@41: void PhysicsWorld::generateTerrain(int seed) {
terom@41:     // generating should use own random number generator, but didn't find easily how that is done
terom@41:     srand(seed);
terom@41:     
terom@41:     // some constants to control random generation
terom@41:     const int min_range = 10;
terom@41:     const int max_range = 40;
terom@41:     const int num = 1;
terom@41:     const int rock_rarity = 4; // 1 / rock_rarity will be rock circle
terom@41: 
terom@41:     // loops for amount of circles
terom@41:     for(int i = 0; i < num; i++) {
terom@41:         // information of new circle
terom@41:         int midx = rand()%(int)dimensions.x;
terom@41:         int midy = rand()%(int)dimensions.y;
terom@41:         int range = rand()%(max_range-min_range)+min_range;
terom@41: 
terom@41:         // put first circle in the middle of the cave
terom@41: 		// so that we have some area we can certainly spawn into 
terom@41: 		if(i == 0) {
terom@41: 			midx = 60;
terom@41: 			midy = 60;
terom@41: 			range = 50;
terom@41: 		}
terom@41: 
terom@41:         TerrainType type = DIRT;
terom@41:         if(rand()%rock_rarity == 0) {
terom@41:             type = ROCK;
terom@41:         }
terom@41:         // loops for every pixel of circle
terom@41:         for(int x = std::max(0, midx-range); x < std::min((int)dimensions.x, midx+range); x++) {
terom@41:             for(int y = std::max(0, midy-range); y < std::min((int)dimensions.y, midy+range); y++) {
terom@41:                 if(x*x+y*y < range*range) {
terom@41:                     // and sets it to type
terom@41:                     terrain[x][y] = type;
terom@41:                 }
terom@41:             }
terom@41:         }
terom@41:     }
terom@41: }
terom@41: 
terom@41: /**
terom@41:  * Returns terrainType in given tile. ROCK if tile is out of area
terom@41:  * @param pos - coordinate of tile
terom@41:  */
terom@41: TerrainType PhysicsWorld::getType(Vector pos) const {
terom@41:     int x = (int)(pos.x);
terom@41:     int y = (int)(pos.y);
terom@41:     if(x < 0 || y < 0 || x >= dimensions.x || y >= dimensions.y) {
terom@41:         return ROCK;
terom@41:     }
terom@41:     return terrain[x][y];
terom@41: }