1

     2

#include "Physics.hh"

     3

#include "Engine.hh"

     4

     5

#include <algorithm>

     6

#include <functional>

     7

#include <cmath>

     8

     9

PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions)

    10

    : tick_timer(PHYSICS_TICK_MS), gravity(gravity), dimensions(dimensions), terrain(dimensions.x, std::vector<TerrainType>(dimensions.y, DIRT)) {

    11

    12

	generateTerrain(1337);

    13

    14

    slots.connect(tick_timer.sig_timer(), this, &PhysicsWorld::tick);

    15

    tick_timer.enable();

    16

}

    17

    18

void PhysicsWorld::addObject (PhysicsObject *object) {

    19

    objects.push_back(object);

    20

}

    21

    22

void PhysicsWorld::tick () {

    23

//    Engine::log(DEBUG, "physics.apply_force") << "*tick*";

    24

    25

    for (std::vector<PhysicsObject*>::iterator i = objects.begin(); i != objects.end(); i++) {

    26

        (*i)->tick(); 

    27

    }

    28

}

    29

    30

PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, Vector position, Vector velocity)

    31

	: world(world), mass(mass), position(position), velocity(velocity) {

    32

    33

	this->inAir = true;

    34

    world.addObject(this);

    35

}

    36

    37

/**

    38

 * TODO This method doesnt quite work as it should but 

    39

 * it makes the worm move pretty smoothly.

    40

 *

    41

 * Make the worm walk on the ground.

    42

 * @return Final position.

    43

 */

    44

Vector PhysicsObject::walk (bool right) {

    45

	Vector cursor = right ? this->position + Vector(1,0) : this->position + Vector(-1,0);

    46

	Vector reached = this->position;

    47

    48

	//for(int steps = 0; steps < 3; steps++) {

    49

    50

		// Go up but not if the wall is over two pixels

    51

		if(world.getType(cursor) != EMPTY) {

    52

    		for(int height = 0, max = 3; height < max+42; height++) {

    53

    54

				if(height >= max)

    55

					return reached;

    56

    57

				cursor.y--;

    58

				if(world.getType(cursor) == EMPTY) {

    59

					// Check that the other parts of the worm don't collide with anything

    60

					if(possibleLocation(cursor)) {

    61

						reached = cursor;

    62

						continue;

    63

					} else {

    64

						// Can't get any further

    65

						return reached;

    66

					}

    67

				}

    68

			}

    69

		} else {

    70

			if(possibleLocation(cursor)) {

    71

				reached = cursor;

    72

			}

    73

			// Start checking if the lower squares are empty

    74

            for(int depth = 0, max = 3; depth < max+42; depth++) {

    75

    76

				if(depth >= max) {

    77

					// We can start a free fall now

    78

					this->inAir = true;

    79

                    // Put some speed there to make loke smoother

    80

					//this->velocity.y = -5;

    81

					return reached;

    82

				}

    83

    84

				cursor.y++;

    85

				if(world.getType(cursor) == EMPTY) {

    86

					// Check that the other parts of the worm don't collide with anything

    87

					if(possibleLocation(cursor)) {

    88

						reached = cursor;

    89

						continue;

    90

					} else {

    91

						// Can't get any further

    92

						return reached;

    93

					}

    94

				}

    95

			}

    96

		}      

    97

    98

	//	cursor.x += right ? 1 : -1;

    99

	//}

   100

	return reached;

   101

}

   102

   103

void PhysicsObject::jump () {

   104

 	velocity.y = -100;

   105

	inAir = true;

   106

}

   107

   108

bool PhysicsObject::possibleLocation (Vector loc) {

   109

	for(unsigned int i = 0; i < this->shape.size(); i++) {

   110

		if(world.getType(loc+shape[i]) != EMPTY)

   111

			return false;

   112

	}

   113

	return true;

   114

}

   115

   116

/**

   117

 * Updates object speed and position. This function organises force

   118

 * integration and collision detection.

   119

 */   

   120

void PhysicsObject::updatePosition () {

   121

   122

    if(!this->inAir) {

   123

        /*if(!forceq.empty()) {

   124

          if(forceq.front().y == 0) {

   125

          this->position = moveVertically(forceq.front().x > 0);

   126

          forceq.pop();

   127

          }

   128

          } */

   129

        return;

   130

	}

   131

	// TODO HERE

   132

   133

    // Add gravity to the force queue

   134

    forceq.push(world.gravity);

   135

   136

    // Go trough every force in the queue

   137

    // TODO: It might be possible to optimize by adding forces together

   138

    Force total;

   139

    posAfterTick = position;

   140

    velAfterTick = velocity;

   141

    while (!forceq.empty()) {

   142

        total += forceq.front();

   143

        forceq.pop();

   144

        //        Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Current position: " << posAfterTick;

   145

    }

   146

    integrate(total, PHYSICS_TICK_MS);

   147

   148

    Vector newPosition = posAfterTick /*+ (velAfterTick * PHYSICS_TICK_MS)/1000*/;

   149

    this->velocity = velAfterTick;

   150

    //Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Nopeus: "<<this->velocity;

   151

    /*

   152

    this->velocity += world.gravity * (PHYSICS_TICK_MS / 1000.0);

   153

   154

    Vector newPosition = position + velocity * (PHYSICS_TICK_MS / 1000.0);

   155

    */

   156

   157

    //TODO Handle the object as a square or a polygon

   158

   159

    bool collided = false;

   160

   161

    //goes 1 unit forward every step and check if has hit anything

   162

    Vector unitVector = (newPosition-position) / (newPosition-position).length();

   163

   164

	Vector tmpVector = position;

   165

    Vector reached = position;

   166

	int steps = (int) (newPosition-position).length() + 2;

   167

   168

	//Engine::log(DEBUG, "physics.update_position") << unitVector-newPosition;

   169

	//Vector foo = position+unitVector*steps-newPosition;

   170

    //Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Virhe: "<< foo;

   171

   172

    for(int i = 0; i < steps; i++) {

   173

        tmpVector += unitVector;

   174

   175

		float minVelocity = 10;

   176

		// Check if any of the four corners of the worm collide

   177

		for(int sh = 0; sh < 4; sh++) {

   178

            if(world.getType(tmpVector+shape[2]) != EMPTY) {

   179

				reached = position + unitVector*(i-1);

   180

				collided = true;

   181

				this->bounce(world.getNormal(tmpVector+shape[2], tmpVector-unitVector+shape[2]));

   182

				this->velocity *= 0.6;

   183

				if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {

   184

					this->inAir = false;

   185

					this->velocity = Vector(0,0);

   186

				}

   187

				break;

   188

			}

   189

		}

   190

		if(collided)

   191

			break;

   192

		/*

   193

		if(velocity.y > 0) {

   194

        	if(world.getType(tmpVector+shape[2]) != EMPTY) {

   195

            	reached = position + unitVector*(i-1);

   196

            	collided = true;

   197

				this->velocity.y *= -0.3;

   198

				this->velocity.x *= 0.7;

   199

				if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {

   200

            		this->inAir = false;

   201

					this->velocity = Vector(0,0);

   202

				}

   203

            	break;

   204

			}

   205

		} else {

   206

        	if(world.getType(tmpVector+shape[0]) != EMPTY) {

   207

            	reached = position + unitVector*(i-1);

   208

            	collided = true;

   209

				this->velocity.y *= -0.3;

   210

				this->velocity.x *= 0.7;

   211

				if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {

   212

            		this->inAir = false;

   213

					this->velocity = Vector(0,0);

   214

				}

   215

            	break;

   216

			}

   217

        }

   218

   219

		if(velocity.x > 0) {

   220

        	if(world.getType(tmpVector+shape[1]) != EMPTY) {

   221

            	reached = position + unitVector*(i-1);

   222

            	collided = true;

   223

				this->velocity.x *= -0.6;

   224

				this->velocity.y *= 0.7;

   225

				if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {

   226

            		this->inAir = false;

   227

					this->velocity = Vector(0,0);

   228

				}

   229

            	break;

   230

			}

   231

		} else {

   232

        	if(world.getType(tmpVector+shape[3]) != EMPTY) {

   233

            	reached = position + unitVector*(i-1);

   234

            	collided = true;

   235

				this->velocity.x *= -0.6;

   236

				this->velocity.y *= 0.7;

   237

				if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {

   238

            		this->inAir = false;

   239

					this->velocity = Vector(0,0);

   240

				}

   241

            	break;

   242

			}

   243

        }*/

   244

   245

		// This col. det. doesn't let worms inside the ground, but on the other hand the worms get often stuck

   246

		/*if(world.getType(tmpVector+shape[0]) != EMPTY || (world.getType(tmpVector+shape[2]) != EMPTY)) {

   247

            reached = position + unitVector*(i-1);

   248

            collided = true;

   249

			this->velocity.y *= -0.3;

   250

			if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {

   251

            	this->inAir = false;

   252

				this->velocity = Vector(0,0);

   253

			}

   254

            break;

   255

        }

   256

		if(world.getType(tmpVector+shape[1]) != EMPTY || (world.getType(tmpVector+shape[3]) != EMPTY)) {

   257

            reached = position + unitVector*(i-1);

   258

            collided = true;

   259

			this->velocity.x *= -0.6;

   260

			if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {

   261

            	this->inAir = false;

   262

				this->velocity = Vector(0,0);

   263

			}

   264

            break;

   265

		}*/

   266

   267

			//Engine::log(DEBUG, "physics.update_position") << "didnt hit";

   268

    }

   269

   270

    // In case of some float error check the final coordinate

   271

    if(!collided) {

   272

        if(world.getType(newPosition+shape[0]) != EMPTY || (world.getType(newPosition+shape[1]) != EMPTY) 

   273

			|| (world.getType(newPosition+shape[2]) != EMPTY) || (world.getType(newPosition+shape[3]) != EMPTY)) {

   274

   275

			Engine::log(DEBUG, "physics.update_position") << "didnt hit";

   276

			// There was error, and there is ground

   277

            //newPosition = tmpVector;

   278

        } else {

   279

            // This means everything was ok, so no need to do anything

   280

        }

   281

    } else {

   282

        newPosition = reached;

   283

        //this->velocity = Vector(0, 0);

   284

        //TODO: it shouldn't just stop on collision

   285

    }

   286

    this->position = newPosition;

   287

   288

}

   289

   290

/**

   291

 * Gets the index of the given coordinate direction

   292

 * referring to the DIRECTIONS table in Physics.hh

   293

 */

   294

int getDirectionIndex (Vector dir) {

   295

	int index = 0;

   296

	if(dir.x < -0.1 || (dir.y > 0.1 && dir.x < 0.1)) {

   297

		index += 4;

   298

        }

   299

	if((dir.x > 0.1 && dir.y > 0.1) || (dir.y > 0.1 && dir.x < 0.1)) {

   300

		index += 2;

   301

        }

   302

	if(!(dir.x > -0.1 && dir.y < 0.1) && !(dir.y < 0.1 && dir.y > -0.1)) {

   303

		index += 1;

   304

        }

   305

	return index;

   306

}

   307

   308

/**

   309

 * Computes hitten wall's normal. Calculated from 3*3 grid

   310

 */

   311

Vector PhysicsWorld::getNormal (Vector hitPoint, Vector prevPoint) {

   312

	// Search free points with bfs and put them to vector

   313

	std::vector<Vector> frees;

   314

	Vector hit = Vector((int)hitPoint.x, (int)hitPoint.y);

   315

	Vector prev = Vector((int)prevPoint.x, (int)prevPoint.y);

   316

	int dirIdx = getDirectionIndex(hit-prev);

   317

	for(int i = 1; i <= 2; i++) {

   318

		if(getType(hit+DIRECTIONS[dirIdx+i]) == EMPTY)

   319

			frees.push_back(DIRECTIONS[dirIdx+i]);

   320

		else

   321

			break;

   322

	}

   323

	for(int i = 1; i <= 2; i++) {

   324

		if(getType(hit+DIRECTIONS[dirIdx-i]) == EMPTY)

   325

			frees.push_back(DIRECTIONS[dirIdx-i]);

   326

		else

   327

			break;

   328

	}

   329

    frees.push_back(DIRECTIONS[dirIdx]);

   330

   331

	Vector normal(0,0);

   332

	for(int i = 0; i < frees.size(); i++) {

   333

		normal += frees[i];

   334

	}

   335

	return normal;

   336

}

   337

   338

/**

   339

 * Bounces from straight wall in any direction.

   340

 * Direction given as normal of that wall

   341

 */

   342

void PhysicsObject::bounce (Vector normal) {

   343

    Vector tangent(normal.y, -normal.x);

   346

    velocity = tprojection - nprojection;

   347

}

   348

   349

/*bool PhysicsWorld::collided (Vector oldPos, Vector newPos) {

   350

	return false;

   351

}*/

   352

   353

/**

   354

 * Integrates given force over time and stores new position to

   355

 * posAfterTick and new velocity to velAfterTick.

   356

 * @param force Force vector.

   357

 * @param dt The time the force is applied (<=PHYSICS_TICK_MS)

   358

 */

   359

void PhysicsObject::integrate(Force force, TimeMS dt) {

   360

    Derivative tmpd;

   361

    Derivative k1 = evaluate(force, 0, tmpd);

   362

    Derivative k2 = evaluate(force, 0.5f*dt, k1);

   363

    Derivative k3 = evaluate(force, 0.5f*dt, k2);

   364

    Derivative k4 = evaluate(force, dt, k3);

   365

   366

   367

    const Vector dxdt = (k1.dx + (k2.dx + k3.dx) * 2.0f + k4.dx) * 1.0f/6.0f;

   368

    const Vector dvdt = (k1.dv + (k2.dv + k3.dv) * 2.0f + k4.dv) * 1.0f/6.0f;

   369

   370

    //    Engine::log(DEBUG, "PhysicsObject.integrate") << "Changes: "<< dxdt << " " << dvdt << " Time: " <<dt;

   371

    posAfterTick = posAfterTick + (dxdt * dt)/1000;

   372

    velAfterTick = velAfterTick + (dvdt * dt)/1000;

   373

    //Engine::log(DEBUG, "PhysicsObject.integrate") << "velAfterTick: " << velAfterTick;

   374

}

   375

   376

Derivative PhysicsObject::evaluate(Force force, TimeMS dt, Derivative &d) {

   377

    Vector curPos = posAfterTick + (d.dx*dt)/1000;

   378

    Vector curVel = velAfterTick + (d.dv*dt)/1000;

   379

   380

    Derivative out;

   381

    out.dx = curVel;

   382

    out.dv = acceleration(force);

   383

    //Engine::log(DEBUG, "PhysicsObject.evaluate") << "Out.dx: " << out.dx;

   384

    return out;

   385

}

   386

   387

Vector PhysicsObject::acceleration(const Force &force) {

   388

    return (force/mass);

   389

}

   390

   391

/**

   392

 * Adds force to the force queue. Force queue is emptied on each

   393

 * tick. Forces that last over one tick are also handled.

   394

 * @param force Force vector.

   395

 * @param dt The time the force is applied.

   396

 */

   397

void PhysicsObject::applyForce (Force force, TimeMS dt) {

   398

    // XXX: dt is not used? Is it assumed to be the same as the integrate() dt?

   399

   400

    // Add applied force to the queue

   401

    forceq.push(force);

   402

	this->inAir = true;

   403

}

   404

   405

void PhysicsObject::updatePhysics (Vector position, Vector velocity) {

   406

    this->position = position;

   407

    this->velocity = velocity;

   408

}

   409

   410

Vector PhysicsObject::getPosition () {

   411

    return this->position;

   412

}

   413

   414

std::vector<Vector>& PhysicsObject::getShape () {

   415

    return this->shape;

   416

}

   417

   418

void PhysicsObject::setShape (std::vector<Vector> shape) {

   419

 	this->shape = shape;

   420

}

   421

   422

void PhysicsObject::tick () {

   423

    this->updatePosition();

   424

}

   425

   426

/**

   427

 * simple random map generation

   428

 * first fills whole level with dirt

   429

 * then randomizes circles of empty or rock

   430

 * @param seed - seed number for random number generator

   431

 */

   432

void PhysicsWorld::generateTerrain(int seed) {

   433

    // generating should use own random number generator, but didn't find easily how that is done

   434

    srand(seed);

   435

   436

    // some constants to control random generation

   437

    const int min_range = 10;

   438

    const int max_range = 40;

   439

    const int num = 30;

   440

    const int rock_rarity = 4; // 1 / rock_rarity will be rock circle

   441

   442

    // loops for amount of circles

   443

    for(int i = 0; i < num; i++) {

   444

        // information of new circle

   445

        int midx = rand()%(int)dimensions.x;

   446

        int midy = rand()%(int)dimensions.y;

   447

        int range = rand()%(max_range-min_range)+min_range;

   448

   449

        // put first circle in the middle of the cave

   450

		// so that we have some area we can certainly spawn into 

   451

		if(i == 0) {

   452

			midx = dimensions.x/2;

   453

			midy = dimensions.y/2;

   454

			range = 150;

   455

		}

   456

   457

        TerrainType type = EMPTY;

   458

        if(rand()%rock_rarity == 0) {

   459

            type = ROCK;

   460

        }

   461

   462

        // loops for every pixel of circle

   463

        for(int x = std::max(0, midx-range); x < std::min((int)dimensions.x, midx+range); x++) {

   464

            for(int y = std::max(0, midy-range); y < std::min((int)dimensions.y, midy+range); y++) {

   465

                if((x-midx) * (x-midx) + (y-midy) * (y-midy) < range*range) {

   466

                    // and sets it to type

   467

                    terrain[x][y] = type;

   468

                }

   469

            }

   470

        }

   471

    }

   472

}

   473

   474

/**

   475

 * Returns terrainType in given tile. ROCK if tile is out of area

   476

 * @param pos - coordinate of tile

   477

 */

   478

TerrainType PhysicsWorld::getType(Vector pos) const {

   479

    int x = (int)(pos.x);

   480

    int y = (int)(pos.y);

   481

    if(x < 0 || y < 0 || x >= dimensions.x || y >= dimensions.y) {

   482

        return ROCK;

   483

    }

   484

    return terrain[x][y];

   485

}