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 }
|