1

     2

#include "Physics.hh"

     3

#include "Engine.hh"

     4

#include "GameState.hh"

     5

#include "Terrain.hh"

     6

     7

#include <algorithm>

     8

#include <functional>

     9

#include <cmath>

    10

#include <assert.h>

    11

    12

PhysicsWorld::PhysicsWorld (Vector gravity, Vector dimensions)

    13

    : tick_timer(PHYSICS_TICK_MS), tick_counter(0), dimensions(dimensions),

    14

      gravity(gravity) {

    15

    Engine::log(DEBUG, "Yeah");

    16

    terrain = Terrain(1337);

    17

    Engine::log(DEBUG, "Yeah2");

    18

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

    19

    tick_timer.enable();

    20

}

    21

    22

void PhysicsWorld::addObject (PhysicsObject *object) {

    23

    objects.push_back(object);

    24

}

    25

    26

void PhysicsWorld::tick () {

    27

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

    28

    29

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

    30

        (*i)->tick(); 

    31

    }

    32

    33

    tick_counter++;

    34

}

    35

    36

uint32_t PhysicsWorld::getTick (void) {

    37

    return tick_counter;

    38

}

    39

    40

PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, 

    41

                              Vector position, Vector velocity)

    42

    : world(world), position(position), velocity(velocity),

    43

      mass(mass), inAir(true), aim(0), facingRight(true) {

    44

    // TODO: Is thir the right way to do this?

    45

    world.addObject(this);

    46

}

    47

    48

Vector PhysicsObject::walk (bool right) {

    49

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

    50

    Vector reached = this->position;

    51

    52

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

    53

    54

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

    55

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

    56

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

    57

    58

            if(height >= max)

    59

                return reached;

    60

    61

            cursor.y--;

    62

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

    63

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

    64

                if(possibleLocation(cursor)) {

    65

                    reached = cursor;

    66

                    continue;

    67

                } else {

    68

                    // Can't get any further

    69

                    return reached;

    70

                }

    71

            }

    72

        }

    73

    } else {

    74

        if(possibleLocation(cursor)) {

    75

            reached = cursor;

    76

        }

    77

        // Start checking if the lower squares are empty

    78

    79

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

    80

    81

            if(depth >= max) {

    82

                // We can start a free fall now

    83

                //

    84

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

    85

                this->inAir = true;

    86

    87

                // Put some speed there to make loke smoother

    88

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

    89

                return reached;

    90

            }

    91

    92

            cursor.y++;

    93

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

    94

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

    95

                if(possibleLocation(cursor)) {

    96

                    reached = cursor;

    97

                    continue;

    98

                } else {

    99

                    // Can't get any further

   100

                    return reached;

   101

                }

   102

            }

   103

        }

   104

    }      

   105

   106

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

   107

    //}

   108

    return reached;

   109

}

   110

   111

void PhysicsObject::jump () {

   112

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

   113

    if (!this->inAir) {

   114

 	velocity.y = -100;

   115

	inAir = true;

   116

    }

   117

}

   118

   119

bool PhysicsObject::possibleLocation (Vector loc) {

   120

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

   121

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

   122

            return false;

   123

    }

   124

    return true;

   125

}

   126

   127

/**

   128

 * Updates object speed and position. This function organises force

   129

 * integration and collision detection.

   130

 */   

   131

void PhysicsObject::updatePosition () {

   132

   133

    // Add gravity to the force queue

   134

    forceq.push(world.gravity);

   135

   136

    // Go trough every force in the queue

   137

    Force total;

   138

    posAfterTick = position;

   139

    velAfterTick = velocity;

   140

    while (!forceq.empty()) {

   141

        total += forceq.front();

   142

        forceq.pop();

   143

    }

   144

   145

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

   146

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

   147

    // detection code.

   148

   149

    if (!this->inAir) {

   150

        if (total.x != 0)

   151

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

   152

        return; // argh

   153

    }

   154

   155

    integrate(total, PHYSICS_TICK_MS);

   156

   157

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

   158

    this->velocity = velAfterTick;

   159

   160

   161

    // Collision detection

   162

    bool collided = false;

   163

   164

    const Vector diffVec = newPosition-position;

   165

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

   166

    Vector reached = position;

   167

   168

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

   169

        // Check if any of the shapes points collide

   170

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

   171

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

   172

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

   173

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

   174

                collided = true;

   175

                this->velocity *= COLLISION_ELASTICITY;

   176

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

   177

                    this->inAir = false;

   178

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

   179

                }

   180

                break;

   181

            }

   182

        }

   183

        if (collided)

   184

            break;

   185

        reached += unitVector;

   186

    }

   187

   188

   189

    /*

   190

      bool collided = false;

   191

   192

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

   193

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

   194

   195

      Vector tmpVector = position;

   196

      Vector reached = position;

   197

   198

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

   199

   200

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

   201

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

   202

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

   203

   204

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

   205

      tmpVector += unitVector;

   206

   207

      float minVelocity = 10;

   208

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

   209

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

   210

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

   211

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

   212

      collided = true;

   213

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

   214

      this->velocity *= 0.4;

   215

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

   216

      this->inAir = false;

   217

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

   218

      }

   219

      break;

   220

      }

   221

      }

   222

      if(collided)

   223

      break;

   224

      }

   225

    */

   226

   227

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

   228

    if(!collided) {

   229

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

   230

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

   231

   232

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

   233

            // There was error, and there is ground

   234

            //newPosition = tmpVector;

   235

        } else {

   236

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

   237

        }

   238

    } else {

   239

        newPosition = reached;

   240

        onCollision();

   241

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

   242

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

   243

    }

   244

    this->position = newPosition;

   245

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

   246

}

   247

   248

/**

   249

 * Gets the index of the given coordinate direction

   250

 * referring to the DIRECTIONS table in Physics.hh

   251

 */

   252

int getDirectionIndex (Vector dir) {

   253

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

   254

        return 0;

   255

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

   256

        return 1;

   257

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

   258

        return 2;

   259

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

   260

        return 3;

   261

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

   262

        return 4;

   263

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

   264

        return 5;

   265

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

   266

        return 6;

   267

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

   268

        return 7;

   269

    }

   270

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

   271

    return 0;

   272

}

   273

   274

/**

   275

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

   276

 */

   277

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

   279

}

   280

   281

/**

   282

 * Bounces from straight wall in any direction.

   283

 * Direction given as normal of that wall

   284

 */

   285

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

   286

void PhysicsObject::bounce (Vector normal) {

   287

    if (normal.length() != 0) {

   288

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

   289

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

   290

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

   291

        velocity = tprojection - nprojection;

   292

    }

   293

}

   294

   295

/**

   296

 * Integrates given force over time and stores new position to

   297

 * posAfterTick and new velocity to velAfterTick.

   298

 * @param force Force vector.

   299

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

   300

 */

   301

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

   302

    Derivative tmpd;

   303

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

   304

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

   305

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

   306

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

   307

   308

   309

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

   310

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

   311

   312

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

   313

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

   314

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

   315

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

   316

}

   317

   318

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

   319

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

   320

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

   321

   322

    Derivative out;

   323

    out.dx = curVel;

   324

    out.dv = acceleration(force);

   325

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

   326

    return out;

   327

}

   328

   329

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

   330

    return (force/mass);

   331

}

   332

   333

void PhysicsObject::applyForce (Force force) {

   334

    // Add applied force to the queue

   335

    forceq.push(force);

   336

}

   337

   338

void PhysicsObject::changeAim(float da) {

   339

    this->aim += da;

   340

   341

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

   342

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

   343

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

   344

}

   345

   346

void PhysicsObject::setFacing(bool facingRight) {

   347

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

   348

    this->facingRight = facingRight;

   349

}

   350

   351

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

   352

    this->position = position;

   353

    this->velocity = velocity;

   354

    this->inAir = inAir;

   355

}

   356

   357

Vector PhysicsObject::getPosition () {

   358

    return this->position;

   359

}

   360

   361

bool PhysicsObject::getFacing() {

   362

    return this->facingRight;

   363

}

   364

   365

float PhysicsObject::getAim() {

   366

    return this->aim;

   367

}

   368

   369

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

   370

    return this->shape;

   371

}

   372

   373

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

   374

    this->shape = shape;

   375

}

   376

   377

void PhysicsObject::tick () {

   378

    this->updatePosition();

   379

}

   380

   381

/**

   382

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

   383

 * @param pos - coordinate of tile

   384

 */

   385

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

   386

    return terrain.getType((int32_t)x,(int32_t)y);

   387

}

   388

TerrainType PhysicsWorld::getType(Vector pos) const {

   389

    return terrain.getType(pos.x, pos.y);

   390

}

   391

   392

/**

   393

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

   394

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

   395

 * @param r - radius of circle

   396

 */

   397

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

   398

    terrain.removeGround(Vector(x,y), r);

   399

}

   400

void PhysicsWorld::removeGround(Vector pos, float r) {

   401

    terrain.removeGround(pos, r);

   402

}

   403

   404

void PhysicsWorld::drawTerrain(CL_GraphicContext *gc) {

   405

    terrain.draw(gc);

   406

}