coeng
annotate src/co_phys.cc @ 8:8cce82794f90
seems to work nicely
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Sun, 15 Feb 2015 05:14:20 +0200 |
| parents | 2f872a179914 |
| children |
| rev | line source |
|---|---|
| nuclear@6 | 1 #include <assert.h> |
| nuclear@8 | 2 #include <vmath/vmath.h> |
| nuclear@5 | 3 #include "co_phys.h" |
| nuclear@6 | 4 #include "sim.h" |
| nuclear@8 | 5 #include "co_collider.h" |
| nuclear@8 | 6 |
| nuclear@8 | 7 #define FIXED_MASS 1e9 |
| nuclear@3 | 8 |
| nuclear@5 | 9 static CoRigid reg_co_rigid; |
| nuclear@3 | 10 |
| nuclear@5 | 11 static Component *cons_rigid() { return new CoRigid; } |
| nuclear@3 | 12 |
| nuclear@5 | 13 CoRigid::CoRigid() |
| nuclear@3 | 14 { |
| nuclear@3 | 15 mass = 1.0; |
| nuclear@8 | 16 elast = 0.85; |
| nuclear@3 | 17 friction = 0.0; |
| nuclear@8 | 18 fixed = false; |
| nuclear@3 | 19 |
| nuclear@3 | 20 name = "rigid"; |
| nuclear@8 | 21 co_prs = 0; |
| nuclear@3 | 22 |
| nuclear@3 | 23 register_component(name, cons_rigid); |
| nuclear@3 | 24 } |
| nuclear@3 | 25 |
| nuclear@6 | 26 const char **CoRigid::update_before() const |
| nuclear@3 | 27 { |
| nuclear@6 | 28 static const char *before[] = { "prs", 0 }; |
| nuclear@6 | 29 return before; |
| nuclear@3 | 30 } |
| nuclear@6 | 31 |
| nuclear@8 | 32 void CoRigid::set_fixed(bool f) |
| nuclear@8 | 33 { |
| nuclear@8 | 34 fixed = f; |
| nuclear@8 | 35 } |
| nuclear@8 | 36 |
| nuclear@8 | 37 bool CoRigid::get_fixed() const |
| nuclear@8 | 38 { |
| nuclear@8 | 39 return fixed; |
| nuclear@8 | 40 } |
| nuclear@8 | 41 |
| nuclear@8 | 42 void CoRigid::set_mass(float mass) |
| nuclear@8 | 43 { |
| nuclear@8 | 44 this->mass = mass; |
| nuclear@8 | 45 } |
| nuclear@8 | 46 |
| nuclear@8 | 47 float CoRigid::get_mass() const |
| nuclear@8 | 48 { |
| nuclear@8 | 49 return fixed ? FIXED_MASS : mass; |
| nuclear@8 | 50 } |
| nuclear@8 | 51 |
| nuclear@6 | 52 void CoRigid::add_impulse(const Vector3 &v) |
| nuclear@6 | 53 { |
| nuclear@6 | 54 impulse += v; |
| nuclear@6 | 55 } |
| nuclear@6 | 56 |
| nuclear@6 | 57 void CoRigid::update(float dt) |
| nuclear@6 | 58 { |
| nuclear@8 | 59 if(!gobj || fixed) return; |
| nuclear@6 | 60 |
| nuclear@6 | 61 if(!co_prs) { |
| nuclear@6 | 62 if(!(co_prs = COCAST(CoPRS, gobj->get_component("prs")))) { |
| nuclear@6 | 63 assert(co_prs); |
| nuclear@6 | 64 return; |
| nuclear@6 | 65 } |
| nuclear@6 | 66 } |
| nuclear@6 | 67 |
| nuclear@8 | 68 // collision detection |
| nuclear@8 | 69 CoCollider *co_col; |
| nuclear@8 | 70 if(world && (co_col = gobj_co_collider(gobj, COGET_FAIL))) { |
| nuclear@8 | 71 HitPoint hit; |
| nuclear@8 | 72 |
| nuclear@8 | 73 // dot product subexpression is to ignore collisions when moving OUT |
| nuclear@8 | 74 // of the object after an interpenetration |
| nuclear@8 | 75 if(world->collide(co_col, &hit) && dot_product(vel, hit.normal) <= 0.0) { |
| nuclear@8 | 76 Vector3 refl = -vel.reflection(hit.normal); |
| nuclear@8 | 77 Vector3 vert = hit.normal * dot_product(refl, hit.normal); |
| nuclear@8 | 78 Vector3 tang = refl - vert; |
| nuclear@8 | 79 vel = vert * elast + tang * friction; |
| nuclear@8 | 80 // TODO add an impulse to the other object and take account of the relative masses |
| nuclear@8 | 81 } |
| nuclear@8 | 82 } |
| nuclear@8 | 83 |
| nuclear@8 | 84 |
| nuclear@6 | 85 float damping = world ? world->get_damping() : 0.005; |
| nuclear@6 | 86 |
| nuclear@6 | 87 Vector3 newpos = co_prs->pos + vel * dt; |
| nuclear@6 | 88 |
| nuclear@6 | 89 Vector3 accel = impulse; |
| nuclear@6 | 90 impulse.x = impulse.y = impulse.z = 0.0f; // reset impulse |
| nuclear@6 | 91 |
| nuclear@6 | 92 if(world) { |
| nuclear@6 | 93 accel += world->get_gravity(); |
| nuclear@6 | 94 } |
| nuclear@6 | 95 |
| nuclear@6 | 96 vel = (vel - vel * damping * dt) + accel * dt; |
| nuclear@8 | 97 if(vel.length() < 1e-3) { |
| nuclear@8 | 98 vel.x = vel.y = vel.z = 0.0f; |
| nuclear@8 | 99 } |
| nuclear@6 | 100 co_prs->pos = newpos; |
| nuclear@6 | 101 } |
| nuclear@8 | 102 |
| nuclear@8 | 103 CoRigid *gobj_co_rigid(const GObject *obj, bool nofail) |
| nuclear@8 | 104 { |
| nuclear@8 | 105 CoRigid *co = COCAST(CoRigid, obj->get_component("rigid")); |
| nuclear@8 | 106 if(co) return co; |
| nuclear@8 | 107 |
| nuclear@8 | 108 return nofail ? ®_co_rigid : 0; |
| nuclear@8 | 109 } |