1

     2

#include "Physics.hh"

     3

#include "Engine.hh"

     4

#include "GameState.hh"

     5

     6

#include <algorithm>

     7

#include <functional>

     8

#include <cmath>

     9

#include <assert.h>

    10

    11

PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions)

    12

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

    13

    14

    generateTerrain(1337);

    15

    16

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

    17

    tick_timer.enable();

    18

}

    19

    20

void PhysicsWorld::addObject (PhysicsObject *object) {

    21

    objects.push_back(object);

    22

}

    23

    24

void PhysicsWorld::tick () {

    25

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

    26

    27

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

    28

        (*i)->tick(); 

    29

    }

    30

    31

    tick_counter++;

    32

}

    33

    34

uint32_t PhysicsWorld::getTick (void) {

    35

    return tick_counter;

    36

}

    37

    42

    43

    world.addObject(this);

    44

}

    45

    46

Vector PhysicsObject::walk (bool right) {

    47

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

    48

    Vector reached = this->position;

    49

    50

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

    51

    52

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

    53

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

    54

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

    55

    56

            if(height >= max)

    57

                return reached;

    58

    59

            cursor.y--;

    60

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

    61

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

    62

                if(possibleLocation(cursor)) {

    63

                    reached = cursor;

    64

                    continue;

    65

                } else {

    66

                    // Can't get any further

    67

                    return reached;

    68

                }

    69

            }

    70

        }

    71

    } else {

    72

        if(possibleLocation(cursor)) {

    73

            reached = cursor;

    74

        }

    75

        // Start checking if the lower squares are empty

    76

    77

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

    78

    79

            if(depth >= max) {

    80

                // We can start a free fall now

    81

                //

    82

                // TODO it should be set inAir if it falls from a cliff

    83

                this->inAir = true;

    84

    85

                // Put some speed there to make loke smoother

    86

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

    87

                return reached;

    88

            }

    89

    90

            cursor.y++;

    91

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

    92

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

    93

                if(possibleLocation(cursor)) {

    94

                    reached = cursor;

    95

                    continue;

    96

                } else {

    97

                    // Can't get any further

    98

                    return reached;

    99

                }

   100

            }

   101

        }

   102

    }      

   103

   104

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

   105

    //}

   106

    return reached;

   107

}

   108

   109

void PhysicsObject::jump () {

   110

    // Jump only if player is "on the ground"

   111

    if (!this->inAir) {

   112

 	velocity.y = -100;

   113

	inAir = true;

   114

    }

   115

}

   116

   117

bool PhysicsObject::possibleLocation (Vector loc) {

   118

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

   119

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

   120

            return false;

   121

    }

   122

    return true;

   123

}

   124

   125

/**

   126

 * Updates object speed and position. This function organises force

   127

 * integration and collision detection.

   128

 */   

   129

void PhysicsObject::updatePosition () {

   130

   131

    // Add gravity to the force queue

   132

    forceq.push(world.gravity);

   133

   134

    // Go trough every force in the queue

   135

    Force total;

   136

    posAfterTick = position;

   137

    velAfterTick = velocity;

   138

    while (!forceq.empty()) {

   139

        total += forceq.front();

   140

        forceq.pop();

   141

    }

   142

   143

    // TODO: This is _ugly_ (but not so ugly as the last one I think)

   144

    // hack. I think we should handle walking from the collision

   145

    // detection code.

   146

   147

    if (!this->inAir) {

   148

        if (total.x != 0)

   149

            this->position = walk(total.x > 0);

   150

        return; // argh

   151

    }

   152

   153

    integrate(total, PHYSICS_TICK_MS);

   154

   155

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

   156

    this->velocity = velAfterTick;

   157

   158

   159

    // Collision detection

   160

    bool collided = false;

   161

   162

    const Vector diffVec = newPosition-position;

   163

    const Vector unitVector = diffVec / diffVec.length();

   164

    Vector reached = position;

   165

   166

    while ((position-reached).length() < diffVec.length()) {

   167

        // Check if any of the shapes points collide

   168

        for (uint64_t i = 0; i < shape.size(); i ++) {

   169

            if (world.getType(reached+shape[i]) != EMPTY) {  // Collision

   170

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

   171

                reached = reached - unitVector; // Return to last point

   172

                collided = true;

   173

                this->velocity *= COLLISION_ELASTICITY;

   174

                if (this->velocity.length() < PLAYER_MIN_SPEED) {

   175

                    this->inAir = false;

   176

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

   177

                }

   178

                break;

   179

            }

   180

        }

   181

        if (collided)

   182

            break;

   183

        reached += unitVector;

   184

    }

   185

   186

   187

    /*

   188

      bool collided = false;

   189

   190

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

   191

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

   192

   193

      Vector tmpVector = position;

   194

      Vector reached = position;

   195

   196

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

   197

   198

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

   199

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

   200

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

   201

   202

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

   203

      tmpVector += unitVector;

   204

   205

      float minVelocity = 10;

   206

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

   207

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

   208

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

   209

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

   210

      collided = true;

   211

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

   212

      this->velocity *= 0.4;

   213

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

   214

      this->inAir = false;

   215

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

   216

      }

   217

      break;

   218

      }

   219

      }

   220

      if(collided)

   221

      break;

   222

      }

   223

    */

   224

   225

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

   226

    if(!collided) {

   227

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

   228

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

   229

   230

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

   231

            // There was error, and there is ground

   232

            //newPosition = tmpVector;

   233

        } else {

   234

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

   235

        }

   236

    } else {

   237

        newPosition = reached;

   238

        onCollision();

   239

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

   240

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

   241

    }

   242

    this->position = newPosition;

   243

   244

}

   245

   246

/**

   247

 * Gets the index of the given coordinate direction

   248

 * referring to the DIRECTIONS table in Physics.hh

   249

 */

   250

int getDirectionIndex (Vector dir) {

   251

    if(dir.x == 0 && dir.y == -1) {

   252

        return 0;

   253

    } else if(dir.x == 1 && dir.y == -1) {

   254

        return 1;

   255

    } else if(dir.x == 1 && dir.y == 0) {

   256

        return 2;

   257

    } else if(dir.x == 1 && dir.y == 1) {

   258

        return 3;

   259

    } else if(dir.x == 0 && dir.y == 1) {

   260

        return 4;

   261

    } else if(dir.x == -1 && dir.y == 1) {

   262

        return 5;

   263

    } else if(dir.x == -1 && dir.y == 0) {

   264

        return 6;

   265

    } else if(dir.x == -1 && dir.y == -1) {

   266

        return 7;

   267

    }

   268

    Engine::log(DEBUG, "physics.getDirectionIndex ") << "invalid direction: " << dir;

   269

    return 0;

   270

}

   271

   272

/**

   273

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

   274

 */

   275

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

   276

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

   277

    std::vector<Vector> frees;

   278

    Vector hit(hitPoint);

   279

    Vector prev(prevPoint);

   280

   281

    assert(hit != prev);

   282

   283

    int dirIdx = getDirectionIndex(prev.roundToInt() - hit.roundToInt());

   284

    //float tmp1 = hit.x-prev.x;

   285

    //float tmp2 = hit.y-prev.y;

   286

    //    Engine::log(DEBUG, "physics.getNormal ") << dirIdx << " " << tmp1 << " " << tmp2;

   287

   288

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

   289

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

   290

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

   291

        else 

   292

            break;

   293

    }

   294

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

   295

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

   296

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

   297

        else

   298

            break;

   299

    }

   300

    frees.push_back(DIRECTIONS[dirIdx]);

   301

   302

    Vector normal(0,0);

   303

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

   304

        normal += frees[i];

   305

    }

   306

    Engine::log(DEBUG, "physics.getNormal ") << "normal: " << normal;

   307

    return normal;

   308

}

   309

   310

/**

   311

 * Bounces from straight wall in any direction.

   312

 * Direction given as normal of that wall

   313

 */

   314

// TODO Bounce doesnt work kun oikealle tai vasemmalle alaviistoon mennään suoralla (tangentaalinen arvo väärän suuntainen)

   315

void PhysicsObject::bounce (Vector normal) {

   316

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

   317

    Vector tprojection = tangent*(velocity * tangent) / (tangent.length()*tangent.length());

   318

    Vector nprojection = normal*(velocity * normal) / (normal.length()*normal.length());

   319

    velocity = tprojection - nprojection;

   320

}

   321

   322

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

   323

  return false;

   324

  }*/

   325

   326

/**

   327

 * Integrates given force over time and stores new position to

   328

 * posAfterTick and new velocity to velAfterTick.

   329

 * @param force Force vector.

   330

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

   331

 */

   332

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

   333

    Derivative tmpd;

   334

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

   335

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

   336

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

   337

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

   338

   339

   340

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

   341

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

   342

   343

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

   344

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

   345

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

   346

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

   347

}

   348

   349

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

   350

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

   351

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

   352

   353

    Derivative out;

   354

    out.dx = curVel;

   355

    out.dv = acceleration(force);

   356

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

   357

    return out;

   358

}

   359

   360

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

   361

    return (force/mass);

   362

}

   363

   364

void PhysicsObject::applyForce (Force force) {

   365

    // Add applied force to the queue

   366

    forceq.push(force);

   367

}

   368

   369

void PhysicsObject::changeAim(float da) {

   370

    this->aim += da;

   371

   372

    if (this->aim > PLAYER_AIM_MAX) this->aim = PLAYER_AIM_MAX;

   373

    if (this->aim < PLAYER_AIM_MIN) this->aim = PLAYER_AIM_MIN;

   374

    //Engine::log(DEBUG, "PhysicsObject.changeAim") << "Player aim: " << this->aim;

   375

}

   376

   378

    //Engine::log(DEBUG, "PhysicsObject.setFacing") << "Facing: " << right;

   380

}

   381

   382

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

   383

    this->position = position;

   384

    this->velocity = velocity;

   385

    this->inAir = inAir;

   386

}

   387

   388

Vector PhysicsObject::getPosition () {

   389

    return this->position;

   390

}

   391

   392

bool PhysicsObject::getFacing() {

   394

}

   395

   396

float PhysicsObject::getAim() {

   397

    return this->aim;

   398

}

   399

   400

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

   401

    return this->shape;

   402

}

   403

   404

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

   405

    this->shape = shape;

   406

}

   407

   408

void PhysicsObject::tick () {

   409

    this->updatePosition();

   410

}

   411

   412

/**

   413

 * simple random map generation

   414

 * first fills whole level with dirt

   415

 * then randomizes circles of empty or rock

   416

 * @param seed - seed number for random number generator

   417

 */

   418

void PhysicsWorld::generateTerrain(int seed) {

   419

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

   420

    srand(seed);

   421

   422

    // some constants to control random generation

   423

    const int min_range = 25;

   424

    const int max_range = 80;

   425

    const int num = 50;

   426

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

   427

   428

    // loops for amount of circles

   429

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

   430

        // information of new circle

   431

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

   432

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

   433

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

   434

   435

        // put first circle in the middle of the cave

   436

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

   437

        if(i == 0) {

   438

            midx = dimensions.x/2;

   439

            midy = dimensions.y/2;

   440

            range = 150;

   441

        }

   442

   443

        TerrainType type = EMPTY;

   444

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

   445

            type = ROCK;

   446

        }

   447

   448

        // loops for every pixel of circle

   449

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

   450

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

   451

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

   452

                // and sets it to type

   453

                terrain[x][y] = type;

   454

                //}

   455

            }

   456

        }

   457

    }

   458

}

   459

   460

/**

   461

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

   462

 * @param pos - coordinate of tile

   463

 */

   464

TerrainType PhysicsWorld::getType(int x, int y) const {

   465

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

   466

        return ROCK;

   467

    }

   468

    return terrain[x][y];

   469

}

   470

TerrainType PhysicsWorld::getType(Vector pos) const {

   471

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

   472

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

   473

    return getType(x, y);

   474

}

   475

   476

/**

   477

 * Removes ground from given circle. ROCK is not removed.

   478

 * @param (x, y) or pos - center of circle.

   479

 * @param r - radius of circle

   480

 */

   481

void PhysicsWorld::removeGround(int x, int y, float r) {

   482

    for(int i = x-(int)r; i < x+r; i++) {

   483

        for(int j = y-(int)r; j < y+r; j++) {

   484

            if(getType(i, j) != ROCK) {

   485

                if((i-x)*(i-x)+(j-y)*(j-y) < r*r) {

   486

                    // Safe because getType returns ROCK if tile is out of bounds