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