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

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

    14

      gravity(gravity) {

    15

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

    16

    tick_timer.enable();

    17

}

    18

    19

void PhysicsWorld::addObject (PhysicsObject *object) {

    20

    objects.push_back(object);

    21

}

    22

    23

void PhysicsWorld::tick () {

    24

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

    25

    26

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

    27

        (*i)->tick(); 

    28

    }

    29

    30

    tick_counter++;

    31

}

    32

    33

uint32_t PhysicsWorld::getTick (void) {

    34

    return tick_counter;

    35

}

    36

    37

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

    38

                              Vector position, Vector velocity)

    39

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

    40

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

    41

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

    42

    world.addObject(this);

    43

}

    44

    45

Vector PhysicsObject::walk (bool right) {

    46

    float deltaX = right ? 1 : -1;

    47

    Vector reached = this->position;

    48

    49

    if(!possibleLocation(position+Vector(deltaX, 0))) {

    50

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

    51

            if(possibleLocation(position+Vector(deltaX, -i))) {

    52

                reached = position+Vector(deltaX, -i);

    53

                break;

    54

            }

    55

        }

    56

    } else {

    57

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

    58

            if(possibleLocation(position+Vector(deltaX, i))) {

    59

                reached = position+Vector(deltaX, i);

    60

            } else {

    61

                break;

    62

            }

    63

        }

    64

    }

    65

    return reached;

    66

    67

}

    68

    69

/**

    70

 * Makes the player jump in the air.

    71

 * @param direction -1: jump left, 0: jump up, 1: jump right

    72

 */

    73

void PhysicsObject::jump (int direction) {

    74

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

    75

    if (!this->inAir) {

    76

 	    velocity.y = -100;

    77

        switch (direction) {

    78

            case 1:

    79

                velocity.x += 20;

    80

                break;

    81

            case -1:

    82

                velocity.x -= 20;

    83

                break;

    84

            case 0:

    85

                break;

    86

            default:

    87

                throw std::logic_error("Invalid jump direction");

    88

        }

    89

	    inAir = true;

    90

    }

    91

}

    92

    93

bool PhysicsObject::possibleLocation (Vector loc) {

    94

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

    95

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

    96

            return false;

    97

    }

    98

    return true;

    99

}

   100

   101

/**

   102

 * Updates object speed and position. This function organises force

   103

 * integration and collision detection.

   104

 */   

   105

void PhysicsObject::updatePosition () {

   106

   107

    // Add gravity to the force queue

   108

    forceq.push(world.gravity);

   109

   110

    // Go trough every force in the queue

   111

    Force total;

   112

    while (!forceq.empty()) {

   113

        total += forceq.front();

   114

        forceq.pop();

   115

    }

   116

   117

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

   118

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

   119

    // detection code.

   120

   121

    if (this->velocity.sqrLength() < PLAYER_MIN_SPEED * PLAYER_MIN_SPEED) {

   122

        this->inAir = !world.collides(this->position+shape[2]+Vector(0, 1));

   123

    }

   124

   131

    if (!this->inAir) {

   132

        if (total.x != 0)

   133

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

   134

        return; 

   135

        //total.x = 0;

   136

    }

   137

   138

    Vector newPosition;

   139

    Vector velAfterTick;

   140

    integrate(total, PHYSICS_TICK_MS, newPosition, velAfterTick);

   141

    this->velocity = velAfterTick;

   142

   143

   144

    // Collision detection

   145

    bool collided = false;

   146

   147

    const Vector diffVec = newPosition-position;

   148

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

   149

    Vector reached = position;

   150

   151

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

   152

        // Check if any of the shapes points collide

   153

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

   154

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

   158

                if (inAir) {

   159

                    //                    Engine::log(DEBUG, "Here");

   160

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

   161

                                                 reached-unitVector+shape[i]));

   162

                    //this->velocity *= COLLISION_ELASTICITY;

   163

                }

   164

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

   165

                collided = true;

   166

                if (this->velocity.sqrLength() < PLAYER_MIN_SPEED * PLAYER_MIN_SPEED) {

   167

                    this->inAir = false;

   168

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

   169

                }

   170

                break;

   171

            }

   172

        }

   173

        if (collided)

   174

            break;

   175

        reached += unitVector;

   176

    }

   177

   178

   179

   180

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

   181

    if(!collided) {

   184

        } else {

   185

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

   186

        }

   187

    } else {

   188

        newPosition = reached;

   189

        onCollision();

   190

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

   191

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

   192

    }

   193

    this->position = newPosition;

   194

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

   195

}

   196

   197

/**

   198

 * Bounces from straight wall in any direction.

   199

 * Direction given as normal of that wall

   200

 */

   201

void PhysicsObject::bounce (Vector normal) {

   202

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

   203

        // Engine::log(DEBUG, "PhysicsObject.bounce") << "Velocity: " << velocity;

   204

        Vector nvel = velocity;

   205

        //        Engine::log(DEBUG, "PhysicsObject.bounce") << "New Velocity: " << nvel;

   206

//        nvel = nvel - ((1+COLLISION_ELASTICITY)*((nvel*normal)/(normal*normal))*normal);

   207

   208

        nvel = nvel - ((2)*((nvel*normal)/(normal*normal))*normal);

   209

//        Engine::log(DEBUG, "PhysicsObject.bounce") << "Projection: " << nvel;

   210

        velocity = nvel;

   211

        this->velocity *= COLLISION_ELASTICITY;

   212

    }

   213

}

   214

   215

/**

   216

 * Integrates given force over time and stores new position to

   217

 * posAfterTick and new velocity to velAfterTick.

   218

 * @param force Force vector.

   219

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

   220

 */

   221

void PhysicsObject::integrate(Force force, TimeMS dt, Vector &posAfterTick, Vector &velAfterTick) {

   222

    posAfterTick = position;

   223

    velAfterTick = velocity;

   224

    Derivative tmpd;

   225

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

   226

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

   227

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

   228

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

   229

   230

   231

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

   232

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

   233

   234

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

   235

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

   236

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

   237

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

   238

}

   239

   240

Derivative PhysicsObject::evaluate(Force force, TimeMS dt, Derivative &d, const Vector &posAfterTick, const Vector &velAfterTick) {

   241

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

   242

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

   243

   244

    Derivative out;

   245

    out.dx = curVel;

   246

    out.dv = acceleration(force);

   247

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

   248

    return out;

   249

}

   250

   251

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

   252

    return (force/mass);

   253

}

   254

   255

void PhysicsObject::applyForce (Force force) {

   256

    // Add applied force to the queue

   257

    forceq.push(force);

   258

}

   259

   260

void PhysicsObject::changeAim(float da) {

   261

    this->aim += da;

   262

   263

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

   264

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

   265

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

   266

}

   267

   268

void PhysicsObject::setFacing(bool facingRight) {

   269

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

   270

    this->facingRight = facingRight;

   271

}

   272

   273

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

   274

    this->position = position;

   275

    this->velocity = velocity;

   276

    this->inAir = inAir;

   277

}

   278

   279

Vector PhysicsObject::getPosition () {

   280

    return this->position;

   281

}

   282

   283

bool PhysicsObject::getFacing() {

   284

    return this->facingRight;

   285

}

   286

   287

float PhysicsObject::getAim() {

   288

    return this->aim;

   289

}

   290

   291

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

   292

    return this->shape;

   293

}

   294

   295

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

   296

    this->shape = shape;

   297

}

   298

   299

void PhysicsObject::tick () {

   300

    this->updatePosition();

   301

}

   302

   303

void PhysicsObject::draw(CL_GraphicContext *gc) {

   304

    CL_Quad player(

   305

                   (position+shape[0]).x, (position+shape[0]).y,

   306

                   (position+shape[1]).x, (position+shape[1]).y,

   307

                   (position+shape[2]).x, (position+shape[2]).y,

   308

                   (position+shape[3]).x, (position+shape[3]).y

   309

                   );

   310

   311

    gc->fill_quad(player, CL_Color::green);

   312

   313

    const uint16_t chlen = 10;

   314

    uint16_t x = player.center().x;

   315

    uint16_t y = player.center().y;

   316

    if (facingRight) {

   317

        gc->draw_line(x, y,

   318

                      x + std::cos(aim)*chlen,

   319

                      y - std::sin(aim)*chlen,

   320

                      CL_Color::black);

   321

    } else {

   322

        gc->draw_line(x, y,

   323

                      x - std::cos(aim)*chlen,

   324

                      y - std::sin(aim)*chlen,

   325

                      CL_Color::black);

   326

    }

   327

}

   328