1

     2

#include "PhysicsObject.hh"

     3

#include "Engine.hh"

     4

     5

#include <cmath>

     6

     7

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

     8

    world(world), position(position), velocity(velocity), mass(mass), inAir(true), aim(0), facingRight(true), 

     9

    alive(false), shouldDelete(false), pivot(NULL)

    10

{  

    11

    if (enabled)

    12

        enable();  

    13

}

    14

    15

PhysicsObject::~PhysicsObject (void) {

    16

//    Engine::log(DEBUG, "PhysicsObject.destructor") << this /* << ": objects.size=" << ((int) world.objects.size()) */;

    17

}

    18

    19

/**

    20

 * Player walks on floor.

    21

 */

    22

Vector PhysicsObject::walk_one_step (float partial, bool right) {

    23

    // which way we are walking

    24

    float deltaX = right ? partial : -partial;

    25

    Vector reached = this->position;

    26

    if(reached.roundToInt() == (reached+Vector(deltaX, 0)).roundToInt()) {

    27

        return reached+Vector(deltaX, 0);

    28

    }

    29

    // Is there upward ramp

    30

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

    31

        // Yes. Then we check n pixels up

    32

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

    33

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

    34

                // and when there is finally EMPTY, we can walk

    35

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

    36

                break;

    37

            }

    38

        }

    39

    } else {

    40

        // Or downward ramp or flat

    41

        for(int i = 0; 1; i++) {

    42

    43

            // And when there is finally ground we can step on

    44

            // it. If there is no gound we still step there,

    45

            // but will fall one pixel down

    46

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

    47

                reached = position+Vector(deltaX, i);

    48

            } else {

    49

                break;

    50

            }

    51

    52

            // If the fall is big enough, set the worm in the air

    53

            if (i >= 2) {

    54

//                Vector back = walk(dt, !right);

    55

                this->inAir = true;

    56

//                this->velocity.x = right ? velocity : -velocity;

    57

                // Avoid stepping two pixels down when it starts to free fall

    58

                reached.y -= 2;

    59

//                this->velocity = (reached-back)*1000/dt;

    60

                break;

    61

            }

    62

        }

    63

    }

    64

    // And we return where we got

    65

    return reached;

    66

}

    67

void PhysicsObject::walk (TimeMS dt, bool right) {

    68

    float velocity = PLAYER_WALK_SPEED;

    69

    float walkAmount = (velocity*dt)/1000;

    70

    Vector reached = this->position;

    71

    while (walkAmount > 0 && !this->inAir) {

    72

        this->position = walk_one_step((1 < walkAmount ? 1 : walkAmount), right);

    73

        walkAmount--;

    74

    }

    75

    // TODO: Should the remaining walkAmount be handled somehow?

    76

}

    77

    78

/**

    79

 * Makes the player jump in the air.

    80

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

    81

 */

    82

void PhysicsObject::jump (int direction) {

    83

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

    84

    if (!this->inAir) {

    85

 	    velocity.y = -100;

    86

        switch (direction) {

    87

            case 1:

    88

                velocity.x += 20;

    89

                break;

    90

            case -1:

    91

                velocity.x -= 20;

    92

                break;

    93

            case 0:

    94

                break;

    95

            default:

    96

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

    97

        }

    98

	    inAir = true;

    99

    }

   100

}

   101

   102

bool PhysicsObject::possibleLocation (Vector loc) {

   103

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

   104

        if(world.collides(loc+shape[i]))

   105

            return false;

   106

    }

   107

    return true;

   108

}

   109

   110

/**

   111

 * Updates object speed and position. This function organises force

   112

 * integration and collision detection.

   113

 */   

   114

void PhysicsObject::updatePosition (TimeMS dt) {

   115

    // Add gravity to the force queue

   116

    forceq.push(world.gravity);

   117

   118

    // If the object (practically player) has a pivot point add

   119

    // a force towards that

   120

    if (pivot != NULL) {

   121

        Vector direction(pivot->getPosition()-position);

   122

        float magnitude = pivot->getPivotForce(this);

   123

        float length = direction.length();

   124

        if (length > 0) {

   125

            direction = direction / length * magnitude;

   126

            forceq.push(direction);

   127

        }

   128

    }

   129

   130

    // Go trough every force in the queue

   131

    Force total;

   132

    while (!forceq.empty()) {

   133

        total += forceq.front();

   134

        forceq.pop();

   135

    }

   136

   137

    // If the player has stopped and there's some ground under some of the 3 some of the 3t

   138

    // set inAir false

   139

    if (this->velocity == Vector(0,0)) {

   140

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

   141

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

   142

                      && !world.collides(this->position+shape[3]+Vector(0, 1));

   143

        // If, however, there's a force caused by a bomb, e.g., set it in air.

   144

        // Still, we have to be able to separate forces caused by walking attempts

   145

        // and bombs etc (+0.1 because float comparison can be dangerous)

   147

            this->inAir = true;

   148

    }

   149

   150

    if(!possibleLocation(position)) {

   151

        //if we are trapped in ground form dirtball or something

   152

        //we might want to just return and set velocity to some value

   153

        //return;

   154

    }

   155

   156

    // If the worm is not in the air make it walk,

   157

    // otherwise integrate the new position and velocity

   158

    if (!this->inAir) {

   159

        // It walks only if there's some vertical force

   160

        if (total.x != 0) {

   161

            walk(dt, total.x > 0);

   162

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

   167

    }

   168

   169

    if (!possibleLocation(position))

   170

        Engine::log(DEBUG, "PhysicsObject.updatePosition") << "impossible location: " << position;

   171

   172

    Vector newPosition;

   173

    Vector velAfterTick;

   174

    // Calculate new position and velocity to the given references

   175

    integrate(total, dt, newPosition, velAfterTick);

   176

    this->velocity = velAfterTick;

   177

   178

   179

    // Collision detection

   180

    bool collided = false;

   181

   182

    const Vector diffVec = newPosition-position;

   183

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

   184

    Vector reached = position;

   187

   188

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

   189

        reached += unitVector;

   190

        // Check if any of the shapes points collide

   191

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

   192

            if (world.collides(reached+shape[i])) {  // Collision

   193

                if (inAir) {

   194

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

   195

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

   196

                                                 reached-unitVector+shape[i]));

   197

                    //this->velocity *= COLLISION_ELASTICITY;

   198

                }

   199

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

   200

                collided = true;

   201

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

   202

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

   203

                }

   204

                break;

   205

            }

   206

        }

   207

        if (collided)

   208

            break;

   209

//        reached += unitVector;

   210

    }

   211

   212

   213

    if (!possibleLocation(reached))

   214

        Engine::log(DEBUG, "PhysicsObject.updatePosition") << "impossible location: " << position << ", diffVec=" << diffVec;

   215

   216

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

   217

    if (!collided) {

   218

        if (!possibleLocation(newPosition)) {

   219

            newPosition = reached;

   220

        } else {

   221

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

   222

        }

   223

   224

        this->position = newPosition;

   225

   226

    } else {

   227

        newPosition = reached;

   228

        this->position = newPosition;

   229

   230

        // the following may delete this object, so it must be the last thing called

   231

        onCollision();

   232

   233

        return;

   234

    }

   235

}

   236

   237

/**

   238

 * Bounces from straight wall in any direction.

   239

 * Direction given as normal of that wall

   240

 */

   241

void PhysicsObject::bounce (Vector normal) {

   242

    // normal.sqrLength can't be 0 when got from getNormal()

   243

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

   244

        Vector nvel = velocity;

   245

        // We project the velocity on normal and remove twice that much from velocity

   246

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

   247

        velocity = nvel;

   248

        // We lose some of our speed on collision

   249

        this->velocity *= this->collision_elasticity;

   250

    }

   251

}

   252

   253

/**

   254

 * Integrates given force over time and stores new position to

   255

 * posAfterTick and new velocity to velAfterTick.

   256

 * @param force Force vector.

   257

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

   258

 */

   259

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

   260

    posAfterTick = position;

   261

    velAfterTick = velocity;

   262

    Derivative tmpd;

   263

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

   264

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

   265

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

   266

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

   267

   268

   269

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

   270

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

   271

   272

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

   273

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

   274

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

   275

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

   276

}

   277

   278

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

   279

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

   280

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

   281

   282

    Derivative out;

   283

    out.dx = curVel;

   284

    out.dv = acceleration(force);

   285

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

   286

    return out;

   287

}

   288

   289

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

   290

    return (force/mass);

   291

}

   292

   293

void PhysicsObject::applyForce (Force force) {

   294

    // Add applied force to the queue

   295

    forceq.push(force);

   296

}

   297

   298

void PhysicsObject::changeAim(float da) {

   299

    this->aim += da;

   300

   301

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

   302

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

   303

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

   304

}

   305

   306

void PhysicsObject::setFacing(bool facingRight) {

   307

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

   308

    this->facingRight = facingRight;

   309

}

   310

   311

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

   312

    this->position = position;

   313

    this->velocity = velocity;

   314

    this->inAir = inAir;

   315

    this->facingRight = facingRight;

   316

    this->aim = aim;

   317

}

   318

   319

Vector PhysicsObject::getPosition () {

   320

    return this->position;

   321

}

   322

   323

Vector PhysicsObject::getVelocity () {

   324

    return this->velocity;

   325

}

   326

   327

bool PhysicsObject::getFacing() {

   328

    return this->facingRight;

   329

}

   330

   331

float PhysicsObject::getAim() {

   332

    return this->aim;

   333

}

   334

   335

Vector PhysicsObject::getDirection (void) {

   336

    return facingRight ? Vector(cos(aim), -sin(aim)) : Vector(-cos(aim), -sin(aim));

   337

}

   338

   339

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

   340

    return this->shape;

   341

}

   342

   343

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

   344

    this->shape = shape;

   345

}

   346

   347

void PhysicsObject::setPivot (PhysicsObject *pivot) {

   348

    this->pivot = pivot;

   349

}

   350

   351

void PhysicsObject::tick (TimeMS tick_length) {

   352

    this->updatePosition(tick_length);

   353

}

   354

   355

void PhysicsObject::enable (void) {

   356

    // only enable once until disabled

   357

    if (alive)

   358

        return;

   359

   360

    // mark as alive

   361

    alive = true;

   362

   363

    // add the world objects list

   364

    world.addPhysicsObject(this);

   365

}

   366

   367

void PhysicsObject::disable (void) {

   368

    // mark as disabled

   369

    alive = false;

   370

}

   371

   372

void PhysicsObject::destroy (void) {

   373

    // mark as disabled and for deletion

   374

    alive = false;

   375

    shouldDelete = true;

   376

}

   377

   378

bool PhysicsObject::isDestroyed (void) {

   379

    return !alive;

   380

}

   381

   382

bool PhysicsObject::removeIfDestroyed (void) {

   383

    if (!alive) {

   384

        if (shouldDelete)

   385

            delete this;

   386

   387

        return true;

   388

   389

    } else {

   390

        return false;

   391

    }

   392

}

   393

   394

float PhysicsObject::getPivotForce (PhysicsObject *bob) { return 0.0; }

   395

   396

void PhysicsObject::draw(CL_GraphicContext *gc) {

   397

    CL_Quad player(

   398

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

   399

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

   400

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

   401

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

   402

                   );

   403

   404

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

   405

   406

    const uint16_t chlen = 10;

   407

    uint16_t x = player.center().x;

   408

    uint16_t y = player.center().y;

   409

    if (facingRight) {

   410

        gc->draw_line(x, y,

   411

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

   412

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

   413

                      CL_Color::black);

   414

    } else {

   415

        gc->draw_line(x, y,

   416

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

   417

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

   418

                      CL_Color::black);

   419

    }

   420

}

   421