ekku@197: #include "PhysicsObject.hh"
ekku@197: #include "Engine.hh"
ekku@197: 
ekku@197: #include <cmath>
saiam@273: #include <utility>
saiam@273: #include <queue>
ekku@197: 
terom@279: PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, Vector position, Vector velocity, ObjectType type, 
terom@279:             float collision_elasticity, bool enabled) :
terom@282:     world(world), 
terom@282:     position(position), 
ekku@285:     previousPosition(position),
terom@282:     velocity(velocity), 
terom@282:     mass(mass), 
terom@282:     inAir(true), 
terom@282:     collision_elasticity(collision_elasticity),
terom@282:     aim(0), 
terom@282:     facing(FACING_RIGHT), 
terom@282:     alive(false),
terom@282:     shouldDelete(false), 
terom@282:     type(type), 
terom@282:     pivot(NULL)
ekku@225: {  
ekku@225:     if (enabled)
ekku@225:         enable();  
ekku@222: }
ekku@222: 
ekku@222: PhysicsObject::~PhysicsObject (void) {
ekku@225: //    Engine::log(DEBUG, "PhysicsObject.destructor") << this /* << ": objects.size=" << ((int) world.objects.size()) */;
ekku@197: }
ekku@197: 
ekku@197: /**
ekku@197:  * Player walks on floor.
ekku@197:  */
ekku@197: Vector PhysicsObject::walk_one_step (float partial, bool right) {
ekku@197:     // which way we are walking
ekku@197:     float deltaX = right ? partial : -partial;
ekku@197:     Vector reached = this->position;
ekku@197:     if(reached.roundToInt() == (reached+Vector(deltaX, 0)).roundToInt()) {
ekku@197:         return reached+Vector(deltaX, 0);
ekku@197:     }
ekku@197:     // Is there upward ramp
ekku@197:     if(!possibleLocation(position+Vector(deltaX, 0))) {
ekku@197:         // Yes. Then we check n pixels up
ekku@197:         for(int i = 1; i < 3; i++) {
ekku@197:             if(possibleLocation(position+Vector(deltaX, -i))) {
ekku@197:                 // and when there is finally EMPTY, we can walk
ekku@197:                 reached = position+Vector(deltaX, -i);
ekku@197:                 break;
ekku@197:             }
ekku@197:         }
ekku@197:     } else {
ekku@197:         // Or downward ramp or flat
ekku@197:         for(int i = 0; 1; i++) {
ekku@197: 
ekku@197:             // And when there is finally ground we can step on
ekku@197:             // it. If there is no gound we still step there,
ekku@197:             // but will fall one pixel down
ekku@197:             if(possibleLocation(position+Vector(deltaX, i))) {
ekku@197:                 reached = position+Vector(deltaX, i);
ekku@197:             } else {
ekku@197:                 break;
ekku@197:             }
ekku@197:             
ekku@197:             // If the fall is big enough, set the worm in the air
ekku@197:             if (i >= 2) {
ekku@197: //                Vector back = walk(dt, !right);
ekku@197:                 this->inAir = true;
ekku@197: //                this->velocity.x = right ? velocity : -velocity;
ekku@197:                 // Avoid stepping two pixels down when it starts to free fall
ekku@197:                 reached.y -= 2;
ekku@197: //                this->velocity = (reached-back)*1000/dt;
ekku@197:                 break;
ekku@197:             }
ekku@197:         }
ekku@197:     }
ekku@197:     // And we return where we got
ekku@197:     return reached;
ekku@197: }
ekku@197: void PhysicsObject::walk (TimeMS dt, bool right) {
ekku@197:     float velocity = PLAYER_WALK_SPEED;
ekku@197:     float walkAmount = (velocity*dt)/1000;
ekku@285:     while (walkAmount > 0){// && !this->inAir) {
ekku@285:         setPosition (walk_one_step((1 < walkAmount ? 1 : walkAmount), right));
ekku@197:         walkAmount--;
ekku@197:     }
ekku@197:     // TODO: Should the remaining walkAmount be handled somehow?
ekku@197: }
ekku@197: 
ekku@197: /**
ekku@197:  * Makes the player jump in the air.
ekku@197:  * @param direction -1: jump left, 0: jump up, 1: jump right
ekku@197:  */
ekku@197: void PhysicsObject::jump (int direction) {
ekku@197:     // Jump only if player is "on the ground"
ekku@197:     if (!this->inAir) {
ekku@197:  	    velocity.y = -100;
ekku@197:         switch (direction) {
ekku@197:             case 1:
ekku@197:                 velocity.x += 20;
ekku@197:                 break;
ekku@197:             case -1:
ekku@197:                 velocity.x -= 20;
ekku@197:                 break;
ekku@197:             case 0:
ekku@197:                 break;
ekku@197:             default:
ekku@197:                 throw std::logic_error("Invalid jump direction");
ekku@197:         }
ekku@197: 	    inAir = true;
ekku@197:     }
ekku@197: }
ekku@197: 
ekku@197: bool PhysicsObject::possibleLocation (Vector loc) {
ekku@197:     for(unsigned int i = 0; i < this->shape.size(); i++) {
ekku@197:         if(world.collides(loc+shape[i]))
ekku@197:             return false;
ekku@197:     }
ekku@197:     return true;
ekku@197: }
ekku@197: 
ekku@197: /**
ekku@197:  * Updates object speed and position. This function organises force
ekku@197:  * integration and collision detection.
ekku@197:  */   
terom@205: void PhysicsObject::updatePosition (TimeMS dt) {
ekku@197:     // Add gravity to the force queue
saiam@273:     applyForce(world.gravity);
ekku@197:     
ekku@228:     // If the object (practically player) has a pivot point add
ekku@228:     // a force towards that
ekku@228:     if (pivot != NULL) {
ekku@228:         Vector direction(pivot->getPosition()-position);
ekku@228:         float magnitude = pivot->getPivotForce(this);
ekku@228:         float length = direction.length();
ekku@228:         if (length > 0) {
ekku@228:             direction = direction / length * magnitude;
saiam@273:             applyForce(direction);
ekku@228:         }
ekku@228:     }
saiam@273: 
saiam@273:     std::pair<Force, TimeMS> force;
saiam@273:     std::queue<std::pair<Force, TimeMS> > newfq;
ekku@197:     Force total;
ekku@197:     while (!forceq.empty()) {
saiam@273:         force = forceq.front();
saiam@275:         if (force.second > dt) {
saiam@275:             force.second -= dt;
saiam@273:             newfq.push(force);
saiam@273:         }
saiam@273:         total += force.first;
ekku@197:         forceq.pop();
ekku@197:     }
saiam@273:     forceq = newfq;
ekku@197: 
ekku@197:     // If the player has stopped and there's some ground under some of the 3 some of the 3t
ekku@197:     // set inAir false
ekku@197:     if (this->velocity == Vector(0,0)) {
ekku@197:         this->inAir = !world.collides(this->position+shape[1]+Vector(0, 1))
ekku@197:                       && !world.collides(this->position+shape[2]+Vector(0, 1))
ekku@197:                       && !world.collides(this->position+shape[3]+Vector(0, 1));
ekku@197:         // If, however, there's a force caused by a bomb, e.g., set it in air.
ekku@197:         // Still, we have to be able to separate forces caused by walking attempts
ekku@197:         // and bombs etc (+0.1 because float comparison can be dangerous)
terom@282:         if (total.y < 0.01 || fabs(total.x) > PLAYER_MOVE_FORCE + 0.1)
ekku@197:             this->inAir = true;
ekku@197:     }
ekku@197: 
ekku@197:     if(!possibleLocation(position)) {
ekku@197:         //if we are trapped in ground form dirtball or something
ekku@197:         //we might want to just return and set velocity to some value
ekku@197:         //return;
ekku@197:     }
ekku@197: 
ekku@197:     // If the worm is not in the air make it walk,
ekku@197:     // otherwise integrate the new position and velocity
ekku@197:     if (!this->inAir) {
ekku@197:         // It walks only if there's some vertical force
ekku@197:         if (total.x != 0) {
terom@205:             walk(dt, total.x > 0);
ekku@197:             this->velocity = Vector(0,0);
ekku@247:         }   
ekku@247:         // Now the possible walking has been done so we can return from this function.
ekku@247:         // In walk inAir could have been set true, but that will be handled in the next tick.
ekku@247:         return;
ekku@197:     }
ekku@197: 
ekku@222:     if (!possibleLocation(position))
ekku@222:         Engine::log(DEBUG, "PhysicsObject.updatePosition") << "impossible location: " << position;
ekku@222: 
ekku@197:     Vector newPosition;
ekku@197:     Vector velAfterTick;
ekku@197:     // Calculate new position and velocity to the given references
terom@205:     integrate(total, dt, newPosition, velAfterTick);
ekku@197:     this->velocity = velAfterTick;
ekku@197: 
ekku@197:     // Collision detection
ekku@197:     bool collided = false;
ekku@252:     Vector collisionPoint;
ekku@197:    
terom@279:     const Vector diffVec = newPosition - position;
ekku@197:     const Vector unitVector = diffVec / diffVec.length();
nireco@297:     if(unitVector == Vector(0, 0)) {
nireco@297:         return;
nireco@297:     }
ekku@197:     Vector reached = position;
ekku@247:     
terom@279:     while ((position - reached).sqrLength() < diffVec.sqrLength()) {
ekku@197:         reached += unitVector;
ekku@197:         // Check if any of the shapes points collide
ekku@197:         for (uint64_t i = 0; i < shape.size(); i++) {
ekku@197:             if (world.collides(reached+shape[i])) {  // Collision
ekku@252: 
ekku@252:                 collisionPoint = reached+shape[i];
ekku@252: 
terom@279:                 if (inAir)
terom@279:                     this->bounce(world.getNormal(reached+shape[i], reached-unitVector+shape[i]));
terom@279: 
ekku@197:                 reached = reached - unitVector; // Return to last point
ekku@197:                 collided = true;
terom@279: 
terom@279:                 if (this->velocity.sqrLength() < PLAYER_MIN_SPEED * PLAYER_MIN_SPEED)
ekku@197:                     this->velocity = Vector(0,0);
terom@279: 
ekku@197:                 break;
ekku@197:             }
ekku@197:         }
terom@279: 
ekku@197:         if (collided)
ekku@197:             break;
ekku@197:     }
ekku@197:    
ekku@197:     
ekku@222:     if (!possibleLocation(reached))
ekku@222:         Engine::log(DEBUG, "PhysicsObject.updatePosition") << "impossible location: " << position << ", diffVec=" << diffVec;
ekku@197: 
ekku@197:     // In case of some float error check the final coordinate
ekku@222:     if (!collided) {
ekku@222:         if (!possibleLocation(newPosition)) {
ekku@197:             newPosition = reached;
ekku@197:         } else {
ekku@197:             // This means everything was ok, so no need to do anything
ekku@197:         }
ekku@222: 
ekku@285:         setPosition (newPosition);
ekku@222: 
ekku@197:     } else {
ekku@197:         newPosition = reached;
ekku@285:         setPosition (newPosition);
ekku@222: 
ekku@222:         // the following may delete this object, so it must be the last thing called
nireco@288:         onCollision(collisionPoint);
ekku@222: 
ekku@222:         return;
ekku@197:     }
ekku@197: }
ekku@197: 
ekku@197: /**
ekku@197:  * Bounces from straight wall in any direction.
ekku@197:  * Direction given as normal of that wall
ekku@197:  */
ekku@197: void PhysicsObject::bounce (Vector normal) {
ekku@197:     // normal.sqrLength can't be 0 when got from getNormal()
ekku@197:     if (normal.sqrLength() != 0) {
ekku@197:         Vector nvel = velocity;
ekku@197:         // We project the velocity on normal and remove twice that much from velocity
ekku@197:         nvel = nvel - ((2)*((nvel*normal)/(normal*normal))*normal);
ekku@197:         velocity = nvel;
ekku@197:         // We lose some of our speed on collision
ekku@197:         this->velocity *= this->collision_elasticity;
ekku@197:     }
ekku@197: }
ekku@197: 
ekku@197: /**
ekku@197:  * Integrates given force over time and stores new position to
ekku@197:  * posAfterTick and new velocity to velAfterTick.
ekku@197:  * @param force Force vector.
ekku@197:  * @param dt The time the force is applied (<=PHYSICS_TICK_MS)
ekku@197:  */
ekku@197: void PhysicsObject::integrate(Force force, TimeMS dt, Vector &posAfterTick, Vector &velAfterTick) {
ekku@197:     posAfterTick = position;
ekku@197:     velAfterTick = velocity;
ekku@197:     Derivative tmpd;
ekku@197:     Derivative k1 = evaluate(force, 0, tmpd, posAfterTick, velAfterTick);
terom@282:     Derivative k2 = evaluate(force, dt / 2, k1, posAfterTick, velAfterTick);
terom@282:     Derivative k3 = evaluate(force, dt / 2, k2, posAfterTick, velAfterTick);
ekku@197:     Derivative k4 = evaluate(force, dt, k3, posAfterTick, velAfterTick);
ekku@197:     
ekku@197: 
ekku@197:     const Vector dxdt = (k1.dx + (k2.dx + k3.dx) * 2.0f + k4.dx) * 1.0f/6.0f;
ekku@197:     const Vector dvdt = (k1.dv + (k2.dv + k3.dv) * 2.0f + k4.dv) * 1.0f/6.0f;
ekku@197:     
ekku@197:     //    Engine::log(DEBUG, "PhysicsObject.integrate") << "Changes: "<< dxdt << " " << dvdt << " Time: " <<dt;
ekku@197:     posAfterTick = posAfterTick + (dxdt * dt)/1000;
ekku@197:     velAfterTick = velAfterTick + (dvdt * dt)/1000;
ekku@197:     //Engine::log(DEBUG, "PhysicsObject.integrate") << "velAfterTick: " << velAfterTick;
ekku@197: }
ekku@197: 
ekku@197: Derivative PhysicsObject::evaluate(Force force, TimeMS dt, Derivative &d, const Vector &posAfterTick, const Vector &velAfterTick) {
ekku@197:     Vector curPos = posAfterTick + (d.dx*dt)/1000;
ekku@197:     Vector curVel = velAfterTick + (d.dv*dt)/1000;
ekku@197: 
ekku@197:     Derivative out;
ekku@197:     out.dx = curVel;
ekku@197:     out.dv = acceleration(force);
ekku@197:     //Engine::log(DEBUG, "PhysicsObject.evaluate") << "Out.dx: " << out.dx;
ekku@197:     return out;
ekku@197: }
ekku@197: 
ekku@197: Vector PhysicsObject::acceleration(const Force &force) {
ekku@197:     return (force/mass);
ekku@197: }
ekku@197: 
saiam@273: void PhysicsObject::applyForce (Force force, TimeMS dt) {
ekku@197:     // Add applied force to the queue
saiam@273:     forceq.push(std::make_pair(force, dt));
ekku@197: }
ekku@197: 
ekku@197: void PhysicsObject::changeAim(float da) {
ekku@197:     this->aim += da;
ekku@197: 
ekku@197:     if (this->aim > PLAYER_AIM_MAX) this->aim = PLAYER_AIM_MAX;
ekku@197:     if (this->aim < PLAYER_AIM_MIN) this->aim = PLAYER_AIM_MIN;
ekku@197:     //Engine::log(DEBUG, "PhysicsObject.changeAim") << "Player aim: " << this->aim;
ekku@197: }
ekku@197: 
ekku@287: ObjectType PhysicsObject::getType (void) const {
ekku@287:     return this->type;
ekku@287: }
ekku@287: 
terom@264: void PhysicsObject::setFacing (FacingDirection facing) {
terom@264:     this->facing = facing;
ekku@197: }
ekku@197: 
terom@264: void PhysicsObject::updatePhysics (Vector position, Vector velocity, bool inAir, FacingDirection facing, float aim) {
ekku@285:     setPosition (position);
ekku@197:     this->velocity = velocity;
ekku@197:     this->inAir = inAir;
terom@264:     this->facing = facing;
terom@200:     this->aim = aim;
ekku@197: }
ekku@225: 
terom@257: Vector PhysicsObject::getPosition (void) const {
terom@257:     return position;
ekku@197: }
terom@257:     
ekku@285: Vector PhysicsObject::getPreviousPosition (void) const {
ekku@285:     return previousPosition;
ekku@285: }
ekku@285:     
ekku@285: void PhysicsObject::setPosition (Vector pos) {
ekku@285:     this->previousPosition = this->position;
ekku@285:     this->position = pos;
ekku@285: }
ekku@285:     
terom@257: PixelCoordinate PhysicsObject::getCoordinate (void) const {
terom@257:     return world.getPixelCoordinate(position);
ekku@222: }
ekku@222: 
terom@257: Vector PhysicsObject::getVelocity (void) const {
terom@257:     return velocity;
ekku@197: }
ekku@197: 
terom@264: FacingDirection PhysicsObject::getFacing (void) const {
terom@264:     return facing;
ekku@197: }
ekku@197: 
terom@257: float PhysicsObject::getAim (void) const {
terom@257:     return aim;
terom@257: }
terom@257: 
terom@257: Vector PhysicsObject::getDirection (void) const {
terom@264:     return facing == FACING_RIGHT ? Vector(cos(aim), -sin(aim)) : Vector(-cos(aim), -sin(aim));
terom@235: }
terom@235: 
terom@257: const std::vector<Vector>& PhysicsObject::getShape () const {
terom@257:     return shape;
ekku@197: }
ekku@197: 
ekku@197: void PhysicsObject::setShape (std::vector<Vector> shape) {
ekku@197:     this->shape = shape;
ekku@197: }
ekku@197: 
ekku@228: void PhysicsObject::setPivot (PhysicsObject *pivot) {
ekku@228:     this->pivot = pivot;
ekku@228: }
ekku@228: 
terom@205: void PhysicsObject::tick (TimeMS tick_length) {
terom@205:     this->updatePosition(tick_length);
ekku@197: }
ekku@222:     
ekku@225: void PhysicsObject::enable (void) {
terom@241:     // only enable once until disabled
terom@241:     if (alive)
terom@241:         return;
terom@241:     
terom@241:     // mark as alive
ekku@225:     alive = true;
terom@241:     
terom@241:     // add the world objects list
ekku@225:     world.addPhysicsObject(this);
ekku@225: }
ekku@225: 
ekku@225: void PhysicsObject::disable (void) {
terom@241:     // mark as disabled
ekku@225:     alive = false;
ekku@225: }
ekku@225: 
ekku@222: void PhysicsObject::destroy (void) {
terom@241:     // mark as disabled and for deletion
ekku@222:     alive = false;
ekku@225:     shouldDelete = true;
ekku@222: }
ekku@222:     
ekku@222: bool PhysicsObject::isDestroyed (void) {
ekku@222:     return !alive;
ekku@222: }
ekku@222:     
ekku@222: bool PhysicsObject::removeIfDestroyed (void) {
ekku@222:     if (!alive) {
ekku@225:         if (shouldDelete)
ekku@225:             delete this;
ekku@225: 
ekku@222:         return true;
ekku@225: 
ekku@222:     } else {
ekku@222:         return false;
ekku@222:     }
ekku@222: }
ekku@197: 
terom@264: float PhysicsObject::getPivotForce (PhysicsObject *bob) { 
terom@282:     (void) bob;
terom@282: 
terom@264:     return 0.0; 
ekku@197: }
ekku@197: 
saiam@268: bool PhysicsObject::collides (const PhysicsObject &obj) {
saiam@268:     const std::vector<Vector> oShape = obj.getShape();
saiam@268:     Vector p1, p2, p3;
saiam@268:     int8_t sign, nsign;
saiam@268:     for (std::vector<Vector>::const_iterator i = oShape.begin(); i != oShape.end(); i++) { // For every point in other shape
saiam@268:         p3 = *i + obj.getPosition();
saiam@268:         sign = 0;
saiam@268:         for (std::vector<Vector>::const_iterator j = shape.begin(); j != shape.end(); j++) {
saiam@268:             p1 = *j + position;
saiam@268:             if ( (j+1) == shape.end() ) p2 = *shape.begin() + position;
saiam@268:             else p2 = *(j+1) + position;
saiam@268:             nsign = crossProduct(p1, p2, p3);
saiam@268:             if ( sign == 0 ) sign = nsign;
saiam@268:             else if ( ((sign < 0) && (nsign < 0)) || ((sign > 0) && (nsign > 0)) ) continue;
saiam@268:             else return false;
saiam@268:         }
saiam@268:     }
saiam@268:     return true;
saiam@268: }
terom@282:     
terom@282: void  PhysicsObject::onCollision (Vector collisionPoint, PhysicsObject *other) {
terom@282:     (void) collisionPoint;
terom@282:     (void) other;
terom@282: }
saiam@268: 
saiam@268: int8_t crossProduct (const Vector &p1, const Vector &p2, const Vector &p3) {
saiam@268:     float p = (p2.x - p1.x)*(p3.y - p1.y) - (p2.y - p1.y)*(p3.x - p1.x);
saiam@268:     if (p < 0)
saiam@268:         return -1;
saiam@268:     else
saiam@268:         return 1;
saiam@268: }