#include "Physics.hh"
#include "Engine.hh"
#include <algorithm>
#include <functional>
#include <cmath>
#include <assert.h>
PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions)
: tick_timer(PHYSICS_TICK_MS), tick_counter(0), gravity(gravity), dimensions(dimensions), terrain(dimensions.x, std::vector<TerrainType>(dimensions.y, DIRT)) {
generateTerrain(1337);
slots.connect(tick_timer.sig_timer(), this, &PhysicsWorld::tick);
tick_timer.enable();
}
void PhysicsWorld::addObject (PhysicsObject *object) {
objects.push_back(object);
}
void PhysicsWorld::tick () {
// Engine::log(DEBUG, "physics.apply_force") << "*tick*";
for (std::vector<PhysicsObject*>::iterator i = objects.begin(); i != objects.end(); i++) {
(*i)->tick();
}
tick_counter++;
}
uint32_t PhysicsWorld::getTick (void) {
return tick_counter;
}
PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, Vector position, Vector velocity)
: world(world), mass(mass), position(position), velocity(velocity) {
this->inAir = true;
world.addObject(this);
}
/**
* TODO This method doesnt quite work as it should but
* it makes the worm move pretty smoothly.
*
* Make the worm walk on the ground.
* @return Final position.
*/
Vector PhysicsObject::walk (bool right) {
Vector cursor = right ? this->position + Vector(1,0) : this->position + Vector(-1,0);
Vector reached = this->position;
//for(int steps = 0; steps < 3; steps++) {
// Go up but not if the wall is over two pixels
if(world.getType(cursor) != EMPTY) {
for(int height = 0, max = 3; height < max+42; height++) {
if(height >= max)
return reached;
cursor.y--;
if(world.getType(cursor) == EMPTY) {
// Check that the other parts of the worm don't collide with anything
if(possibleLocation(cursor)) {
reached = cursor;
continue;
} else {
// Can't get any further
return reached;
}
}
}
} else {
if(possibleLocation(cursor)) {
reached = cursor;
}
// Start checking if the lower squares are empty
for(int depth = 0, max = 3; depth < max+42; depth++) {
if(depth >= max) {
// We can start a free fall now
this->inAir = true;
// Put some speed there to make loke smoother
//this->velocity.y = -5;
return reached;
}
cursor.y++;
if(world.getType(cursor) == EMPTY) {
// Check that the other parts of the worm don't collide with anything
if(possibleLocation(cursor)) {
reached = cursor;
continue;
} else {
// Can't get any further
return reached;
}
}
}
}
// cursor.x += right ? 1 : -1;
//}
return reached;
}
void PhysicsObject::jump () {
velocity.y = -100;
inAir = true;
}
bool PhysicsObject::possibleLocation (Vector loc) {
for(unsigned int i = 0; i < this->shape.size(); i++) {
if(world.getType(loc+shape[i]) != EMPTY)
return false;
}
return true;
}
/**
* Updates object speed and position. This function organises force
* integration and collision detection.
*/
void PhysicsObject::updatePosition () {
if(!this->inAir) {
/*if(!forceq.empty()) {
if(forceq.front().y == 0) {
this->position = moveVertically(forceq.front().x > 0);
forceq.pop();
}
} */
return;
}
// TODO HERE
// Add gravity to the force queue
forceq.push(world.gravity);
// Go trough every force in the queue
// TODO: It might be possible to optimize by adding forces together
Force total;
posAfterTick = position;
velAfterTick = velocity;
while (!forceq.empty()) {
total += forceq.front();
forceq.pop();
// Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Current position: " << posAfterTick;
}
integrate(total, PHYSICS_TICK_MS);
Vector newPosition = posAfterTick /*+ (velAfterTick * PHYSICS_TICK_MS)/1000*/;
this->velocity = velAfterTick;
//Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Nopeus: "<<this->velocity;
/*
this->velocity += world.gravity * (PHYSICS_TICK_MS / 1000.0);
Vector newPosition = position + velocity * (PHYSICS_TICK_MS / 1000.0);
*/
//TODO Handle the object as a square or a polygon
bool collided = false;
//goes 1 unit forward every step and check if has hit anything
Vector unitVector = (newPosition-position) / (newPosition-position).length();
Vector tmpVector = position;
Vector reached = position;
int steps = (int) (newPosition-position).length() + 2;
//Engine::log(DEBUG, "physics.update_position") << unitVector-newPosition;
//Vector foo = position+unitVector*steps-newPosition;
//Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Virhe: "<< foo;
for(int i = 0; i < steps; i++) {
tmpVector += unitVector;
float minVelocity = 10;
// Check if any of the four corners of the worm collide
for(int sh = 0; sh < 4; sh++) {
if(world.getType(tmpVector+shape[sh]) != EMPTY) {
reached = position + unitVector*(i-1);
collided = true;
this->bounce(world.getNormal(tmpVector+shape[sh], tmpVector-unitVector+shape[sh]));
this->velocity *= 0.4;
if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {
this->inAir = false;
this->velocity = Vector(0,0);
}
break;
}
}
if(collided)
break;
/*
if(velocity.y > 0) {
if(world.getType(tmpVector+shape[2]) != EMPTY) {
reached = position + unitVector*(i-1);
collided = true;
this->velocity.y *= -0.3;
this->velocity.x *= 0.7;
if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {
this->inAir = false;
this->velocity = Vector(0,0);
}
break;
}
} else {
if(world.getType(tmpVector+shape[0]) != EMPTY) {
reached = position + unitVector*(i-1);
collided = true;
this->velocity.y *= -0.3;
this->velocity.x *= 0.7;
if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {
this->inAir = false;
this->velocity = Vector(0,0);
}
break;
}
}
if(velocity.x > 0) {
if(world.getType(tmpVector+shape[1]) != EMPTY) {
reached = position + unitVector*(i-1);
collided = true;
this->velocity.x *= -0.6;
this->velocity.y *= 0.7;
if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {
this->inAir = false;
this->velocity = Vector(0,0);
}
break;
}
} else {
if(world.getType(tmpVector+shape[3]) != EMPTY) {
reached = position + unitVector*(i-1);
collided = true;
this->velocity.x *= -0.6;
this->velocity.y *= 0.7;
if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {
this->inAir = false;
this->velocity = Vector(0,0);
}
break;
}
}*/
// This col. det. doesn't let worms inside the ground, but on the other hand the worms get often stuck
/*if(world.getType(tmpVector+shape[0]) != EMPTY || (world.getType(tmpVector+shape[2]) != EMPTY)) {
reached = position + unitVector*(i-1);
collided = true;
this->velocity.y *= -0.3;
if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {
this->inAir = false;
this->velocity = Vector(0,0);
}
break;
}
if(world.getType(tmpVector+shape[1]) != EMPTY || (world.getType(tmpVector+shape[3]) != EMPTY)) {
reached = position + unitVector*(i-1);
collided = true;
this->velocity.x *= -0.6;
if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {
this->inAir = false;
this->velocity = Vector(0,0);
}
break;
}*/
//Engine::log(DEBUG, "physics.update_position") << "didnt hit";
}
// In case of some float error check the final coordinate
if(!collided) {
if(world.getType(newPosition+shape[0]) != EMPTY || (world.getType(newPosition+shape[1]) != EMPTY)
|| (world.getType(newPosition+shape[2]) != EMPTY) || (world.getType(newPosition+shape[3]) != EMPTY)) {
Engine::log(DEBUG, "physics.update_position") << "didnt hit";
// There was error, and there is ground
//newPosition = tmpVector;
} else {
// This means everything was ok, so no need to do anything
}
} else {
newPosition = reached;
//this->velocity = Vector(0, 0);
//TODO: it shouldn't just stop on collision
}
this->position = newPosition;
}
/**
* Gets the index of the given coordinate direction
* referring to the DIRECTIONS table in Physics.hh
*/
int getDirectionIndex (Vector dir) {
int index = 0;
if(dir.x < -0.1 || (dir.y > 0.1 && dir.x < 0.1)) {
index += 4;
}
if((dir.x > 0.1 && dir.y > -0.1) || (dir.y > -0.1 && dir.x < 0.1)) {
index += 2;
}
if(!(dir.x > -0.1 && dir.y < 0.1) && !(dir.y < 0.1 && dir.y > -0.1)) {
index += 1;
}
return index;
}
/**
* Computes hitten wall's normal. Calculated from 3*3 grid
*/
Vector PhysicsWorld::getNormal (Vector hitPoint, Vector prevPoint) {
// Search free points with bfs and put them to vector
std::vector<Vector> frees;
Vector hit = Vector((int)hitPoint.x, (int)hitPoint.y);
Vector prev = Vector((int)prevPoint.x, (int)prevPoint.y);
assert(hit != prev);
int dirIdx = getDirectionIndex(hit-prev);
Engine::log(DEBUG, "physics.getNormal ") << dirIdx;
for(int i = 1; i <= 2; i++) {
if(getType(hit+DIRECTIONS[(dirIdx+i) % 8]) == EMPTY)
frees.push_back(DIRECTIONS[(dirIdx+i) % 8]);
else
break;
}
for(int i = 1; i <= 2; i++) {
if(getType(hit+DIRECTIONS[(dirIdx-i) % 8]) == EMPTY)
frees.push_back(DIRECTIONS[(dirIdx-i) % 8]);
else
break;
}
frees.push_back(DIRECTIONS[dirIdx]);
Vector normal(0,0);
for(unsigned int i = 0; i < frees.size(); i++) {
normal += frees[i];
}
return normal;
}
/**
* Bounces from straight wall in any direction.
* Direction given as normal of that wall
*/
void PhysicsObject::bounce (Vector normal) {
Vector tangent(normal.y, -normal.x);
Vector tprojection = tangent*(velocity * tangent) / (tangent.length()*tangent.length());
Vector nprojection = normal*(velocity * normal) / (normal.length()*normal.length());
velocity = tprojection - nprojection;
}
/*bool PhysicsWorld::collided (Vector oldPos, Vector newPos) {
return false;
}*/
/**
* 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) {
Derivative tmpd;
Derivative k1 = evaluate(force, 0, tmpd);
Derivative k2 = evaluate(force, 0.5f*dt, k1);
Derivative k3 = evaluate(force, 0.5f*dt, k2);
Derivative k4 = evaluate(force, dt, k3);
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;
// Engine::log(DEBUG, "PhysicsObject.integrate") << "Changes: "<< dxdt << " " << dvdt << " Time: " <<dt;
posAfterTick = posAfterTick + (dxdt * dt)/1000;
velAfterTick = velAfterTick + (dvdt * dt)/1000;
//Engine::log(DEBUG, "PhysicsObject.integrate") << "velAfterTick: " << velAfterTick;
}
Derivative PhysicsObject::evaluate(Force force, TimeMS dt, Derivative &d) {
Vector curPos = posAfterTick + (d.dx*dt)/1000;
Vector curVel = velAfterTick + (d.dv*dt)/1000;
Derivative out;
out.dx = curVel;
out.dv = acceleration(force);
//Engine::log(DEBUG, "PhysicsObject.evaluate") << "Out.dx: " << out.dx;
return out;
}
Vector PhysicsObject::acceleration(const Force &force) {
return (force/mass);
}
/**
* Adds force to the force queue. Force queue is emptied on each
* tick. Forces that last over one tick are also handled.
* @param force Force vector.
* @param dt The time the force is applied.
*/
void PhysicsObject::applyForce (Force force, TimeMS dt) {
// XXX: dt is not used? Is it assumed to be the same as the integrate() dt?
// Add applied force to the queue
forceq.push(force);
this->inAir = true;
}
void PhysicsObject::updatePhysics (Vector position, Vector velocity) {
this->position = position;
this->velocity = velocity;
}
Vector PhysicsObject::getPosition () {
return this->position;
}
std::vector<Vector>& PhysicsObject::getShape () {
return this->shape;
}
void PhysicsObject::setShape (std::vector<Vector> shape) {
this->shape = shape;
}
void PhysicsObject::tick () {
this->updatePosition();
}
/**
* simple random map generation
* first fills whole level with dirt
* then randomizes circles of empty or rock
* @param seed - seed number for random number generator
*/
void PhysicsWorld::generateTerrain(int seed) {
// generating should use own random number generator, but didn't find easily how that is done
srand(seed);
// some constants to control random generation
const int min_range = 25;
const int max_range = 80;
const int num = 50;
const int rock_rarity = 4; // 1 / rock_rarity will be rock circle
// loops for amount of circles
for(int i = 0; i < num; i++) {
// information of new circle
int midx = rand()%(int)dimensions.x;
int midy = rand()%(int)dimensions.y;
int range = rand()%(max_range-min_range)+min_range;
// put first circle in the middle of the cave
// so that we have some area we can certainly spawn into
if(i == 0) {
midx = dimensions.x/2;
midy = dimensions.y/2;
range = 150;
}
TerrainType type = EMPTY;
if(rand()%rock_rarity == 0) {
type = ROCK;
}
// loops for every pixel of circle
for(int x = std::max(0, midx-range); x < std::min((int)dimensions.x, midx+range); x++) {
for(int y = std::max(0, midy-range); y < std::min((int)dimensions.y, midy+range); y++) {
if((x-midx) * (x-midx) + (y-midy) * (y-midy) < range*range) {
// and sets it to type
terrain[x][y] = type;
}
}
}
}
}
/**
* Returns terrainType in given tile. ROCK if tile is out of area
* @param pos - coordinate of tile
*/
TerrainType PhysicsWorld::getType(Vector pos) const {
int x = (int)(pos.x);
int y = (int)(pos.y);
if(x < 0 || y < 0 || x >= dimensions.x || y >= dimensions.y) {
return ROCK;
}
return terrain[x][y];
}