blob: f20be5f27c025d8f5b2e64c807b38d60b717c629 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
|
#include "World.h"
#include "Body.h"
#include "Joint.h"
#include "Arbiter.h"
#include "Constants.h"
using namespace Phy2D;
using std::vector;
using std::map;
using std::pair;
typedef map<ArbiterKey, Arbiter>::iterator ArbIter;
typedef pair<ArbiterKey, Arbiter> ArbPair;
bool World::accumulateImpulses = true;
bool World::warmStarting = true;
bool World::positionCorrection = true;
void World::Add(Body* body)
{
bodies.push_back(body);
}
void World::Add(Joint* joint)
{
joints.push_back(joint);
}
void World::Clear()
{
bodies.clear();
joints.clear();
arbiters.clear();
}
void World::BroadPhase()
{
// O(n^2) broad-phase
for (int i = 0; i < (int)bodies.size(); ++i)
{
Body* bi = bodies[i];
for (int j = i + 1; j < (int)bodies.size(); ++j)
{
Body* bj = bodies[j];
if (bi->invMass == _0 && bj->invMass == _0)
continue;
Arbiter newArb(bi, bj);
ArbiterKey key(bi, bj);
if (newArb.numContacts > 0)
{
ArbIter iter = arbiters.find(key);
if (iter == arbiters.end())
{
arbiters.insert(ArbPair(key, newArb));
}
else
{
iter->second.Update(newArb.contacts, newArb.numContacts);
}
}
else
{
arbiters.erase(key);
}
}
}
}
void World::Step(fixed dt)
{
fixed inv_dt = dt > _0 ? _1 / dt : _0;
// Determine overlapping bodies and update contact points.
BroadPhase();
// Integrate forces.
for (int i = 0; i < (int)bodies.size(); ++i)
{
Body* b = bodies[i];
if (b->invMass == _0)
continue;
b->velocity += dt * (gravity + b->invMass * b->force);
b->angularVelocity += dt * b->invI * b->torque;
}
// Perform pre-steps.
for (ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb)
{
arb->second.PreStep(inv_dt);
}
for (int i = 0; i < (int)joints.size(); ++i)
{
joints[i]->PreStep(inv_dt);
}
// Perform iterations
for (int i = 0; i < iterations; ++i)
{
for (ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb)
{
arb->second.ApplyImpulse();
}
for (int j = 0; j < (int)joints.size(); ++j)
{
joints[j]->ApplyImpulse();
}
}
// Integrate Velocities
for (int i = 0; i < (int)bodies.size(); ++i)
{
Body* b = bodies[i];
b->position += dt * b->velocity;
b->rotation += dt * b->angularVelocity;
b->force.Set(_0, _0);
b->torque = _0;
}
}
|
