1

#include "PhysicsObject.hh"

     2

#include "Engine.hh"

     3

     4

#include <cmath>

     5

#include <utility>

     6

#include <queue>

     7

    10

    world(world), position(position), velocity(velocity), mass(mass), inAir(true), aim(0), facing(FACING_RIGHT), 

    12

{  

    13

    if (enabled)

    14

        enable();  

    15

}

    16

    17

PhysicsObject::~PhysicsObject (void) {

    18

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

    19

}

    20

    21

/**

    22

 * Player walks on floor.

    23

 */

    24

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

    25

    // which way we are walking

    26

    float deltaX = right ? partial : -partial;

    27

    Vector reached = this->position;

    28

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

    29

        return reached+Vector(deltaX, 0);

    30

    }

    31

    // Is there upward ramp

    32

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

    33

        // Yes. Then we check n pixels up

    34

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

    35

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

    36

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

    37

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

    38

                break;

    39

            }

    40

        }

    41

    } else {

    42

        // Or downward ramp or flat

    43

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

    44

    45

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

    46

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

    47

            // but will fall one pixel down

    48

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

    49

                reached = position+Vector(deltaX, i);

    50

            } else {

    51

                break;

    52

            }

    53

    54

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

    55

            if (i >= 2) {

    56

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

    57

                this->inAir = true;

    58

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

    59

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

    60

                reached.y -= 2;

    61

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

    62

                break;

    63

            }

    64

        }

    65

    }

    66

    // And we return where we got

    67

    return reached;

    68

}

    69

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

    70

    float velocity = PLAYER_WALK_SPEED;

    71

    float walkAmount = (velocity*dt)/1000;

    72

    Vector reached = this->position;

    73

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

    74

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

    75

        walkAmount--;

    76

    }

    77

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

    78

}

    79

    80

/**

    81

 * Makes the player jump in the air.

    82

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

    83

 */

    84

void PhysicsObject::jump (int direction) {

    85

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

    86

    if (!this->inAir) {

    87

 	    velocity.y = -100;

    88

        switch (direction) {

    89

            case 1:

    90

                velocity.x += 20;

    91

                break;

    92

            case -1:

    93

                velocity.x -= 20;

    94

                break;

    95

            case 0:

    96

                break;

    97

            default:

    98

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

    99

        }

   100

	    inAir = true;

   101

    }

   102

}

   103

   104

bool PhysicsObject::possibleLocation (Vector loc) {

   105

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

   106

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

   107

            return false;

   108

    }

   109

    return true;

   110

}

   111

   112

/**

   113

 * Updates object speed and position. This function organises force

   114

 * integration and collision detection.

   115

 */   

   116

void PhysicsObject::updatePosition (TimeMS dt) {

   117

    // Add gravity to the force queue

   118

    applyForce(world.gravity);

   119

   120

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

   121

    // a force towards that

   122

    if (pivot != NULL) {

   123

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

   124

        float magnitude = pivot->getPivotForce(this);

   125

        float length = direction.length();

   126

        if (length > 0) {

   127

            direction = direction / length * magnitude;

   128

            applyForce(direction);

   129

        }

   130

    }

   131

   132

    std::pair<Force, TimeMS> force;

   133

    std::queue<std::pair<Force, TimeMS> > newfq;

   134

    Force total;

   135

    while (!forceq.empty()) {

   136

        force = forceq.front();

   137

        if (force.second > dt) {

   138

            force.second -= dt;

   139

            newfq.push(force);

   140

        }

   141

        total += force.first;

   142

        forceq.pop();

   143

    }

   144

    forceq = newfq;

   145

   146

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

   147

    // set inAir false

   148

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

   149

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

   150

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

   151

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

   152

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

   153

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

   154

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

   155

        if (total.y < 0.01 || abs(total.x) > PLAYER_MOVE_FORCE + 0.1)

   156

            this->inAir = true;

   157

    }

   158

   159

    if(!possibleLocation(position)) {

   160

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

   161

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

   162

        //return;

   163

    }

   164

   165

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

   166

    // otherwise integrate the new position and velocity

   167

    if (!this->inAir) {

   168

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

   169

        if (total.x != 0) {

   170

            walk(dt, total.x > 0);

   171

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

   172

        }   

   173

        // Now the possible walking has been done so we can return from this function.

   174

        // In walk inAir could have been set true, but that will be handled in the next tick.

   175

        return;

   176

    }

   177

   178

    if (!possibleLocation(position))

   179

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

   180

   181

    Vector newPosition;

   182

    Vector velAfterTick;

   183

    // Calculate new position and velocity to the given references

   184

    integrate(total, dt, newPosition, velAfterTick);

   185

    this->velocity = velAfterTick;

   186

   187

    // Collision detection

   188

    bool collided = false;

   189

    Vector collisionPoint;

   190

   192

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

   193

    Vector reached = position;

   194

   196

        reached += unitVector;

   198

        // Check if any of the shapes points collide

   199

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

   200

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

   201

   202

                collisionPoint = reached+shape[i];

   203

   207

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

   208

                collided = true;

   211

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

   213

                break;

   214

            }

   215

        }

   217

        if (collided)

   218

            break;

   219

    }

   220

   221

   222

    if (!possibleLocation(reached))

   223

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

   224

   225

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

   226

    if (!collided) {

   227

        if (!possibleLocation(newPosition)) {

   228

            newPosition = reached;

   229

        } else {

   230

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

   231

        }

   232

   233

        this->position = newPosition;

   234

   235

    } else {

   236

        newPosition = reached;

   237

        this->position = newPosition;

   238

   239

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

   240

        onCollision(collisionPoint, NULL);

   241

   242

        return;

   243

    }

   244

}

   245

   246

/**

   247

 * Bounces from straight wall in any direction.

   248

 * Direction given as normal of that wall

   249

 */

   250

void PhysicsObject::bounce (Vector normal) {

   251

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

   252

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

   253

        Vector nvel = velocity;

   254

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

   255

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

   256

        velocity = nvel;

   257

        // We lose some of our speed on collision

   258

        this->velocity *= this->collision_elasticity;

   259

    }

   260

}

   261

   262

/**

   263

 * Integrates given force over time and stores new position to

   264

 * posAfterTick and new velocity to velAfterTick.

   265

 * @param force Force vector.

   266

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

   267

 */

   268

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

   269

    posAfterTick = position;

   270

    velAfterTick = velocity;

   271

    Derivative tmpd;

   272

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

   273

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

   274

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

   275

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

   276

   277

   278

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

   279

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

   280

   281

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

   282

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

   283

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

   284

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

   285

}

   286

   287

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

   288

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

   289

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

   290

   291

    Derivative out;

   292

    out.dx = curVel;

   293

    out.dv = acceleration(force);

   294

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

   295

    return out;

   296

}

   297

   298

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

   299

    return (force/mass);

   300

}

   301

   302

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

   303

    // Add applied force to the queue

   304

    forceq.push(std::make_pair(force, dt));

   305

}

   306

   307

void PhysicsObject::changeAim(float da) {

   308

    this->aim += da;

   309

   310

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

   311

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

   312

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

   313

}

   314

   315

void PhysicsObject::setFacing (FacingDirection facing) {

   316

    this->facing = facing;

   317

}

   318

   319

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

   320

    this->position = position;

   321

    this->velocity = velocity;

   322

    this->inAir = inAir;

   323

    this->facing = facing;

   324

    this->aim = aim;

   325

}

   326

   327

Vector PhysicsObject::getPosition (void) const {

   328

    return position;

   329

}

   330

   331

PixelCoordinate PhysicsObject::getCoordinate (void) const {

   332

    return world.getPixelCoordinate(position);

   333

}

   334

   335

Vector PhysicsObject::getVelocity (void) const {

   336

    return velocity;

   337

}

   338

   339

FacingDirection PhysicsObject::getFacing (void) const {

   340

    return facing;

   341

}

   342

   343

float PhysicsObject::getAim (void) const {

   344

    return aim;

   345

}

   346

   347

Vector PhysicsObject::getDirection (void) const {

   348

    return facing == FACING_RIGHT ? Vector(cos(aim), -sin(aim)) : Vector(-cos(aim), -sin(aim));

   349

}

   350

   351

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

   352

    return shape;

   353

}

   354

   355

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

   356

    this->shape = shape;

   357

}

   358

   359

void PhysicsObject::setPivot (PhysicsObject *pivot) {

   360

    this->pivot = pivot;

   361

}

   362

   363

void PhysicsObject::tick (TimeMS tick_length) {

   364

    this->updatePosition(tick_length);

   365

}

   366

   367

void PhysicsObject::enable (void) {

   368

    // only enable once until disabled

   369

    if (alive)

   370

        return;

   371

   372

    // mark as alive

   373

    alive = true;

   374

   375

    // add the world objects list

   376

    world.addPhysicsObject(this);

   377

}

   378

   379

void PhysicsObject::disable (void) {

   380

    // mark as disabled

   381

    alive = false;

   382

}

   383

   384

void PhysicsObject::destroy (void) {

   385

    // mark as disabled and for deletion

   386

    alive = false;

   387

    shouldDelete = true;

   388

}

   389

   390

bool PhysicsObject::isDestroyed (void) {

   391

    return !alive;

   392

}

   393

   394

bool PhysicsObject::removeIfDestroyed (void) {

   395

    if (!alive) {

   396

        if (shouldDelete)

   397

            delete this;

   398

   399

        return true;

   400

   401

    } else {

   402

        return false;

   403

    }

   404

}

   405

   406

float PhysicsObject::getPivotForce (PhysicsObject *bob) { 

   407

    return 0.0; 

   408

}

   409

   410

bool PhysicsObject::collides (const PhysicsObject &obj) {

   411

    const std::vector<Vector> oShape = obj.getShape();

   412

    Vector p1, p2, p3;

   413

    int8_t sign, nsign;

   414

    for (std::vector<Vector>::const_iterator i = oShape.begin(); i != oShape.end(); i++) { // For every point in other shape

   415

        p3 = *i + obj.getPosition();

   416

        sign = 0;

   417

        for (std::vector<Vector>::const_iterator j = shape.begin(); j != shape.end(); j++) {

   418

            p1 = *j + position;

   419

            if ( (j+1) == shape.end() ) p2 = *shape.begin() + position;

   420

            else p2 = *(j+1) + position;

   421

            nsign = crossProduct(p1, p2, p3);

   422

            if ( sign == 0 ) sign = nsign;

   423

            else if ( ((sign < 0) && (nsign < 0)) || ((sign > 0) && (nsign > 0)) ) continue;

   424

            else return false;

   425

        }

   426

    }

   427

    return true;

   428

}

   429

   430

int8_t crossProduct (const Vector &p1, const Vector &p2, const Vector &p3) {

   431

    float p = (p2.x - p1.x)*(p3.y - p1.y) - (p2.y - p1.y)*(p3.x - p1.x);

   432

    if (p < 0)

   433

        return -1;

   434

    else

   435

        return 1;

   436

}