terom@58: 
terom@58: #include "Physics.hh"
terom@58: #include "Engine.hh"
saiam@115: #include "GameState.hh"
saiam@150: #include "Terrain.hh"
terom@58: 
saiam@45: #include <algorithm>
terom@50: #include <functional>
nireco@77: #include <cmath>
ekku@104: #include <assert.h>
saiam@45: 
terom@60: PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions)
saiam@161:     : Terrain(1337), tick_timer(PHYSICS_TICK_MS), tick_counter(0), dimensions(dimensions),
saiam@150:       gravity(gravity) {
terom@54:     slots.connect(tick_timer.sig_timer(), this, &PhysicsWorld::tick);
terom@50:     tick_timer.enable();
terom@50: }
ekku@44: 
ekku@44: void PhysicsWorld::addObject (PhysicsObject *object) {
saiam@47:     objects.push_back(object);
ekku@44: }
ekku@44: 
ekku@44: void PhysicsWorld::tick () {
saiam@123:     //    Engine::log(DEBUG, "physics.apply_force") << "*tick*";
terom@58: 
nireco@76:     for (std::vector<PhysicsObject*>::iterator i = objects.begin(); i != objects.end(); i++) {
nireco@76:         (*i)->tick(); 
nireco@76:     }
terom@105: 
terom@105:     tick_counter++;
terom@105: }
terom@105: 
terom@105: uint32_t PhysicsWorld::getTick (void) {
terom@105:     return tick_counter;
saiam@48: }
saiam@48: 
saiam@128: PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, 
saiam@128:                               Vector position, Vector velocity)
saiam@135:     : world(world), position(position), velocity(velocity),
saiam@135:       mass(mass), inAir(true), aim(0), facingRight(true) {
saiam@135:     // TODO: Is thir the right way to do this?
terom@60:     world.addObject(this);
terom@60: }
ekku@44: 
terom@96: Vector PhysicsObject::walk (bool right) {
ekku@163:     Vector cursor = this->position + shape[2] + (right ? Vector(1,0) : Vector(-1,0));
nireco@112:     Vector reached = this->position;
saiam@115:     
nireco@112:     //for(int steps = 0; steps < 3; steps++) {
nireco@112:     
saiam@115:     // Go up but not if the wall is over two pixels
ekku@164:     if(world.collides(cursor)) {
ekku@164:    
ekku@163:         for(int height = 0, max = 2; height < max+42; height++) {
saiam@115:             
saiam@115:             if(height >= max)
saiam@115:                 return reached;
saiam@115:             
saiam@115:             cursor.y--;
ekku@164:             if(!world.collides(cursor)) {
saiam@115:                 // Check that the other parts of the worm don't collide with anything
saiam@115:                 if(possibleLocation(cursor)) {
ekku@163:                     reached = cursor - shape[2];
saiam@115:                     continue;
saiam@123:                 } else {
saiam@115:                     // Can't get any further
nireco@112:                     return reached;
nireco@112:                 }
nireco@112:             }
saiam@115:         }
saiam@115:     } else {
saiam@115:         if(possibleLocation(cursor)) {
ekku@163:             reached = cursor - shape[2];
saiam@115:         }
saiam@115:         // Start checking if the lower squares are empty
saiam@115:         
saiam@115:         for(int depth = 0, max = 3; depth < max+42; depth++) {
saiam@115:             
saiam@115:             if(depth >= max) {
saiam@115:                 // We can start a free fall now
saiam@123:                 //
nireco@112:                 // TODO it should be set inAir if it falls from a cliff
nireco@116:                 this->inAir = true;
saiam@115:                 
saiam@115:                 // Put some speed there to make loke smoother
saiam@115:                 //this->velocity.y = -5;
saiam@115:                 return reached;
saiam@115:             }
saiam@115:             
saiam@115:             cursor.y++;
ekku@164:             if(!world.collides(cursor)) {
saiam@115:                 // Check that the other parts of the worm don't collide with anything
saiam@115:                 if(possibleLocation(cursor)) {
ekku@163:                     reached = cursor - shape[2];
saiam@115:                     continue;
saiam@115:                 } else {
saiam@115:                     // Can't get any further
nireco@112:                     return reached;
nireco@112:                 }
nireco@112:             }
saiam@115:         }
saiam@115:     }      
saiam@115:     
nireco@112:     //    cursor.x += right ? 1 : -1;
nireco@112:     //}
nireco@112:     return reached;
ekku@94: }
ekku@94: 
ekku@94: void PhysicsObject::jump () {
saiam@114:     // Jump only if player is "on the ground"
saiam@114:     if (!this->inAir) {
ekku@159:  	    velocity.y = -100;
ekku@159:         velocity.x += facingRight ? 20 : -20;
ekku@159: 	    inAir = true;
saiam@114:     }
ekku@94: }
ekku@94: 
ekku@94: bool PhysicsObject::possibleLocation (Vector loc) {
nireco@112:     for(unsigned int i = 0; i < this->shape.size(); i++) {
nireco@112:         if(world.getType(loc+shape[i]) != EMPTY)
nireco@112:             return false;
nireco@112:     }
nireco@112:     return true;
ekku@94: }
ekku@94: 
ekku@94: /**
saiam@83:  * Updates object speed and position. This function organises force
saiam@83:  * integration and collision detection.
saiam@83:  */   
saiam@83: void PhysicsObject::updatePosition () {
ekku@94: 
saiam@83:     // Add gravity to the force queue
saiam@85:     forceq.push(world.gravity);
saiam@83:     
saiam@83:     // Go trough every force in the queue
saiam@84:     Force total;
saiam@83:     posAfterTick = position;
saiam@83:     velAfterTick = velocity;
saiam@83:     while (!forceq.empty()) {
saiam@85:         total += forceq.front();
saiam@83:         forceq.pop();
saiam@83:     }
saiam@114: 
saiam@114:     // TODO: This is _ugly_ (but not so ugly as the last one I think)
saiam@114:     // hack. I think we should handle walking from the collision
saiam@114:     // detection code.
saiam@114: 
saiam@114:     if (!this->inAir) {
saiam@114:         if (total.x != 0)
saiam@114:             this->position = walk(total.x > 0);
saiam@154:         return; 
saiam@154:         //total.x = 0;
saiam@114:     }
saiam@114: 
saiam@85:     integrate(total, PHYSICS_TICK_MS);
saiam@83: 
saiam@84:     Vector newPosition = posAfterTick /*+ (velAfterTick * PHYSICS_TICK_MS)/1000*/;
saiam@83:     this->velocity = velAfterTick;
saiam@83: 
saiam@123:    
saiam@114:     // Collision detection
saiam@115:     bool collided = false;
saiam@115:    
saiam@115:     const Vector diffVec = newPosition-position;
saiam@115:     const Vector unitVector = diffVec / diffVec.length();
saiam@115:     Vector reached = position;
saiam@115: 
saiam@115:     while ((position-reached).length() < diffVec.length()) {
saiam@115:         // Check if any of the shapes points collide
saiam@154:         for (uint64_t i = 0; i < shape.size(); i++) {
saiam@115:             if (world.getType(reached+shape[i]) != EMPTY) {  // Collision
saiam@154:                 if (inAir) {
saiam@154:                     //                    Engine::log(DEBUG, "Here");
saiam@154:                     this->bounce(world.getNormal(reached+shape[i], 
saiam@154:                                                  reached-unitVector+shape[i]));
saiam@154:                     //this->velocity *= COLLISION_ELASTICITY;
saiam@154:                 }
saiam@115:                 reached = reached - unitVector; // Return to last point
saiam@115:                 collided = true;
saiam@123:                 if (this->velocity.length() < PLAYER_MIN_SPEED) {
saiam@115:                     this->inAir = false;
saiam@115:                     this->velocity = Vector(0,0);
saiam@115:                 }
saiam@115:                 break;
saiam@115:             }
saiam@115:         }
saiam@115:         if (collided)
saiam@115:             break;
saiam@115:         reached += unitVector;
saiam@115:     }
saiam@123:    
ekku@95:     
saiam@123:     /*
saiam@123:       bool collided = false;
ekku@94: 
saiam@123:       //goes 1 unit forward every step and check if has hit anything
saiam@123:       Vector unitVector = (newPosition-position) / (newPosition-position).length();
saiam@114:     
saiam@123:       Vector tmpVector = position;
saiam@123:       Vector reached = position;
saiam@123: 
saiam@123:       int steps = (int) (newPosition-position).length() + 2;
saiam@123:         
saiam@123:       //Engine::log(DEBUG, "physics.update_position") << unitVector-newPosition;
saiam@123:       //Vector foo = position+unitVector*steps-newPosition;
saiam@123:       //Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Virhe: "<< foo;
saiam@123:     
saiam@123:       for(int i = 0; i < steps; i++) {
saiam@123:       tmpVector += unitVector;
saiam@123: 
saiam@123:       float minVelocity = 10;
saiam@123:       // Check if any of the four corners of the worm collide
saiam@123:       for(int sh = 0; sh < 4; sh++) {
saiam@123:       if(world.getType(tmpVector+shape[sh]) != EMPTY) {
saiam@123:       reached = position + unitVector*(i-1);
saiam@123:       collided = true;
saiam@123:       this->bounce(world.getNormal(tmpVector+shape[sh], tmpVector-unitVector+shape[sh]));
saiam@123:       this->velocity *= 0.4;
saiam@123:       if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {
saiam@123:       this->inAir = false;
saiam@123:       this->velocity = Vector(0,0);
saiam@123:       }
saiam@123:       break;
saiam@123:       }
saiam@123:       }
saiam@123:       if(collided)
saiam@123:       break;
saiam@123:       }
saiam@123:     */
saiam@123: 
ekku@92:     // In case of some float error check the final coordinate
nireco@77:     if(!collided) {
ekku@92:         if(world.getType(newPosition+shape[0]) != EMPTY || (world.getType(newPosition+shape[1]) != EMPTY) 
saiam@114:            || (world.getType(newPosition+shape[2]) != EMPTY) || (world.getType(newPosition+shape[3]) != EMPTY)) {
ekku@92:             
saiam@150:             //  Engine::log(DEBUG, "physics.update_position") << "didnt hit";
nireco@112:             // There was error, and there is ground
ekku@92:             //newPosition = tmpVector;
nireco@77:         } else {
nireco@77:             // This means everything was ok, so no need to do anything
nireco@77:         }
nireco@77:     } else {
nireco@77:         newPosition = reached;
nireco@116:         onCollision();
ekku@92:         //this->velocity = Vector(0, 0);
nireco@77:         //TODO: it shouldn't just stop on collision
nireco@77:     }
nireco@77:     this->position = newPosition;
nireco@158: //    Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Pos: " << this->position;
ekku@44: }
ekku@97: 
ekku@97: /**
ekku@97:  * Bounces from straight wall in any direction.
ekku@97:  * Direction given as normal of that wall
ekku@97:  */
ekku@97: void PhysicsObject::bounce (Vector normal) {
saiam@150:     if (normal.length() != 0) {
saiam@154:         // Engine::log(DEBUG, "PhysicsObject.bounce") << "Velocity: " << velocity;
saiam@153:         Vector nvel = velocity;
saiam@153:         //        Engine::log(DEBUG, "PhysicsObject.bounce") << "New Velocity: " << nvel;
nireco@160: //        nvel = nvel - ((1+COLLISION_ELASTICITY)*((nvel*normal)/(normal*normal))*normal);
nireco@160:         
nireco@160:         nvel = nvel - ((2)*((nvel*normal)/(normal*normal))*normal);
nireco@160: //        Engine::log(DEBUG, "PhysicsObject.bounce") << "Projection: " << nvel;
saiam@153:         velocity = nvel;
saiam@154:         this->velocity *= COLLISION_ELASTICITY;
saiam@154:     }
ekku@97: }
ekku@44: 
saiam@83: /**
saiam@83:  * Integrates given force over time and stores new position to
saiam@83:  * posAfterTick and new velocity to velAfterTick.
saiam@83:  * @param force Force vector.
saiam@83:  * @param dt The time the force is applied (<=PHYSICS_TICK_MS)
saiam@83:  */
saiam@85: void PhysicsObject::integrate(Force force, TimeMS dt) {
saiam@83:     Derivative tmpd;
saiam@83:     Derivative k1 = evaluate(force, 0, tmpd);
saiam@83:     Derivative k2 = evaluate(force, 0.5f*dt, k1);
saiam@83:     Derivative k3 = evaluate(force, 0.5f*dt, k2);
saiam@83:     Derivative k4 = evaluate(force, dt, k3);
saiam@83:     
saiam@83: 
saiam@83:     const Vector dxdt = (k1.dx + (k2.dx + k3.dx) * 2.0f + k4.dx) * 1.0f/6.0f;
saiam@83:     const Vector dvdt = (k1.dv + (k2.dv + k3.dv) * 2.0f + k4.dv) * 1.0f/6.0f;
saiam@83:     
saiam@83:     //    Engine::log(DEBUG, "PhysicsObject.integrate") << "Changes: "<< dxdt << " " << dvdt << " Time: " <<dt;
saiam@83:     posAfterTick = posAfterTick + (dxdt * dt)/1000;
saiam@83:     velAfterTick = velAfterTick + (dvdt * dt)/1000;
saiam@83:     //Engine::log(DEBUG, "PhysicsObject.integrate") << "velAfterTick: " << velAfterTick;
saiam@70: }
saiam@70: 
saiam@85: Derivative PhysicsObject::evaluate(Force force, TimeMS dt, Derivative &d) {
saiam@83:     Vector curPos = posAfterTick + (d.dx*dt)/1000;
saiam@83:     Vector curVel = velAfterTick + (d.dv*dt)/1000;
terom@58: 
saiam@83:     Derivative out;
saiam@83:     out.dx = curVel;
saiam@83:     out.dv = acceleration(force);
saiam@83:     //Engine::log(DEBUG, "PhysicsObject.evaluate") << "Out.dx: " << out.dx;
saiam@83:     return out;
saiam@83: }
saiam@83: 
saiam@85: Vector PhysicsObject::acceleration(const Force &force) {
saiam@83:     return (force/mass);
saiam@83: }
saiam@83: 
saiam@122: void PhysicsObject::applyForce (Force force) {
saiam@123:     // Add applied force to the queue
saiam@123:     forceq.push(force);
ekku@44: }
ekku@44: 
saiam@108: void PhysicsObject::changeAim(float da) {
saiam@108:     this->aim += da;
saiam@108: 
saiam@108:     if (this->aim > PLAYER_AIM_MAX) this->aim = PLAYER_AIM_MAX;
saiam@108:     if (this->aim < PLAYER_AIM_MIN) this->aim = PLAYER_AIM_MIN;
ekku@109:     //Engine::log(DEBUG, "PhysicsObject.changeAim") << "Player aim: " << this->aim;
saiam@108: }
saiam@108: 
saiam@128: void PhysicsObject::setFacing(bool facingRight) {
ekku@109:     //Engine::log(DEBUG, "PhysicsObject.setFacing") << "Facing: " << right;
saiam@128:     this->facingRight = facingRight;
saiam@108: }
saiam@108: 
terom@107: void PhysicsObject::updatePhysics (Vector position, Vector velocity, bool inAir) {
saiam@47:     this->position = position;
saiam@47:     this->velocity = velocity;
terom@107:     this->inAir = inAir;
ekku@44: }
ekku@44:     
ekku@44: Vector PhysicsObject::getPosition () {
saiam@47:     return this->position;
ekku@44: }
ekku@44: 
saiam@108: bool PhysicsObject::getFacing() {
saiam@128:     return this->facingRight;
saiam@108: }
saiam@108: 
saiam@108: float PhysicsObject::getAim() {
saiam@108:     return this->aim;
saiam@108: }
saiam@108: 
ekku@91: std::vector<Vector>& PhysicsObject::getShape () {
ekku@91:     return this->shape;
ekku@91: }
ekku@91: 
ekku@91: void PhysicsObject::setShape (std::vector<Vector> shape) {
saiam@123:     this->shape = shape;
ekku@91: }
ekku@91: 
ekku@44: void PhysicsObject::tick () {
saiam@47:     this->updatePosition();
ekku@44: }
ekku@44: 
saiam@153: void PhysicsObject::draw(CL_GraphicContext *gc) {
saiam@153:     CL_Quad player(
saiam@153:                    (position+shape[0]).x, (position+shape[0]).y,
saiam@153:                    (position+shape[1]).x, (position+shape[1]).y,
saiam@153:                    (position+shape[2]).x, (position+shape[2]).y,
saiam@153:                    (position+shape[3]).x, (position+shape[3]).y
saiam@153:                    );
saiam@153:     
saiam@153:     gc->fill_quad(player, CL_Color::green);
saiam@153:     
saiam@153:     const uint16_t chlen = 10;
saiam@153:     uint16_t x = player.center().x;
saiam@153:     uint16_t y = player.center().y;
saiam@153:     if (facingRight) {
saiam@153:         gc->draw_line(x, y,
saiam@153:                       x + std::cos(aim)*chlen,
saiam@153:                       y - std::sin(aim)*chlen,
saiam@153:                       CL_Color::black);
saiam@153:     } else {
saiam@153:         gc->draw_line(x, y,
saiam@153:                       x - std::cos(aim)*chlen,
saiam@153:                       y - std::sin(aim)*chlen,
saiam@153:                       CL_Color::black);
saiam@153:     }
saiam@153: }
saiam@153: