28 PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, Vector position, Vector velocity) |
28 PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, Vector position, Vector velocity) |
29 : world(world), mass(mass), position(position), velocity(velocity) { |
29 : world(world), mass(mass), position(position), velocity(velocity) { |
30 |
30 |
31 world.addObject(this); |
31 world.addObject(this); |
32 } |
32 } |
33 |
33 |
|
34 /** |
|
35 * Updates object speed and position. This function organises force |
|
36 * integration and collision detection. |
|
37 */ |
34 void PhysicsObject::updatePosition () { |
38 void PhysicsObject::updatePosition () { |
35 |
39 // Add gravity to the force queue |
36 // Calculate gravity's influence on the velocity vector |
40 forceq.push(Force(world.gravity, PHYSICS_TICK_MS)); |
|
41 |
|
42 // Go trough every force in the queue |
|
43 // TODO: It might be possible to optimize by adding forces together |
|
44 std::queue<Force> newfq; |
|
45 Force tmpf; |
|
46 posAfterTick = position; |
|
47 velAfterTick = velocity; |
|
48 while (!forceq.empty()) { |
|
49 tmpf = forceq.front(); |
|
50 if (tmpf.dt <= PHYSICS_TICK_MS) { // Force affects only one tick |
|
51 integrate(tmpf.force, tmpf.dt); |
|
52 } else { // Add remaining time to next tick |
|
53 newfq.push(Force(tmpf.force, tmpf.dt - PHYSICS_TICK_MS)); |
|
54 integrate(tmpf.force, PHYSICS_TICK_MS); |
|
55 } |
|
56 forceq.pop(); |
|
57 // Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Current position: " << posAfterTick; |
|
58 } |
|
59 forceq = newfq; |
|
60 |
|
61 Vector newPosition = posAfterTick + (velAfterTick * PHYSICS_TICK_MS)/1000; |
|
62 this->velocity = velAfterTick; |
|
63 Engine::log(DEBUG, "PhysicsObject.updatePosition") << "Nopeus: "<<this->velocity; |
|
64 /* |
37 this->velocity += world.gravity * (PHYSICS_TICK_MS / 1000.0); |
65 this->velocity += world.gravity * (PHYSICS_TICK_MS / 1000.0); |
38 |
66 |
39 Vector newPosition = position + velocity * (PHYSICS_TICK_MS / 1000.0); |
67 Vector newPosition = position + velocity * (PHYSICS_TICK_MS / 1000.0); |
|
68 */ |
40 |
69 |
41 //TODO Handle the object as a square or a polygon |
70 //TODO Handle the object as a square or a polygon |
42 |
71 |
43 bool collided = false; |
72 bool collided = false; |
44 |
73 |
45 //goes 1 unit forward every step and check if has hit anything |
74 //goes 1 unit forward every step and check if has hit anything |
46 Vector unitVector = (newPosition-position) / (newPosition-position).length(); |
75 Vector unitVector = (newPosition-position) / (newPosition-position).length(); |
47 |
76 |
73 newPosition = reached; |
102 newPosition = reached; |
74 this->velocity = Vector(0, 0); |
103 this->velocity = Vector(0, 0); |
75 //TODO: it shouldn't just stop on collision |
104 //TODO: it shouldn't just stop on collision |
76 } |
105 } |
77 this->position = newPosition; |
106 this->position = newPosition; |
|
107 |
78 } |
108 } |
79 |
109 |
80 bool PhysicsWorld::collided (Vector oldPos, Vector newPos) { |
110 bool PhysicsWorld::collided (Vector oldPos, Vector newPos) { |
81 int deltaX = oldPos.x - newPos.x; |
111 int deltaX = oldPos.x - newPos.x; |
82 int deltaY = oldPos.y - newPos.y; |
112 int deltaY = oldPos.y - newPos.y; |
94 return true; |
124 return true; |
95 } |
125 } |
96 return false; |
126 return false; |
97 } |
127 } |
98 |
128 |
|
129 /** |
|
130 * Integrates given force over time and stores new position to |
|
131 * posAfterTick and new velocity to velAfterTick. |
|
132 * @param force Force vector. |
|
133 * @param dt The time the force is applied (<=PHYSICS_TICK_MS) |
|
134 */ |
99 void PhysicsObject::integrate(Vector force, TimeMS dt) { |
135 void PhysicsObject::integrate(Vector force, TimeMS dt) { |
100 // TODO |
136 Derivative tmpd; |
101 } |
137 Derivative k1 = evaluate(force, 0, tmpd); |
102 |
138 Derivative k2 = evaluate(force, 0.5f*dt, k1); |
|
139 Derivative k3 = evaluate(force, 0.5f*dt, k2); |
|
140 Derivative k4 = evaluate(force, dt, k3); |
|
141 |
|
142 |
|
143 const Vector dxdt = (k1.dx + (k2.dx + k3.dx) * 2.0f + k4.dx) * 1.0f/6.0f; |
|
144 const Vector dvdt = (k1.dv + (k2.dv + k3.dv) * 2.0f + k4.dv) * 1.0f/6.0f; |
|
145 |
|
146 // Engine::log(DEBUG, "PhysicsObject.integrate") << "Changes: "<< dxdt << " " << dvdt << " Time: " <<dt; |
|
147 posAfterTick = posAfterTick + (dxdt * dt)/1000; |
|
148 velAfterTick = velAfterTick + (dvdt * dt)/1000; |
|
149 //Engine::log(DEBUG, "PhysicsObject.integrate") << "velAfterTick: " << velAfterTick; |
|
150 } |
|
151 |
|
152 Derivative PhysicsObject::evaluate(Vector force, TimeMS dt, Derivative &d) { |
|
153 Vector curPos = posAfterTick + (d.dx*dt)/1000; |
|
154 Vector curVel = velAfterTick + (d.dv*dt)/1000; |
|
155 |
|
156 Derivative out; |
|
157 out.dx = curVel; |
|
158 out.dv = acceleration(force); |
|
159 //Engine::log(DEBUG, "PhysicsObject.evaluate") << "Out.dx: " << out.dx; |
|
160 return out; |
|
161 } |
|
162 |
|
163 Vector PhysicsObject::acceleration(const Vector &force) { |
|
164 return (force/mass); |
|
165 } |
|
166 |
|
167 /** |
|
168 * Adds force to the force queue. Force queue is emptied on each |
|
169 * tick. Forces that last over one tick are also handled. |
|
170 * @param force Force vector. |
|
171 * @param dt The time the force is applied. |
|
172 */ |
103 void PhysicsObject::applyForce (Vector force, TimeMS dt) { |
173 void PhysicsObject::applyForce (Vector force, TimeMS dt) { |
104 Vector oldVelocity = velocity; |
174 // Add applied force to the queue |
105 |
175 forceq.push(Force(force, dt)); |
106 this->velocity += force * dt / 1000 / mass; // The last factor denotes the time. |
|
107 // It should be scaled somehow. |
|
108 |
|
109 // Engine::log(DEBUG, "physics.apply_force") << "force=" << force << ", velocity " << oldVelocity << " -> " << velocity; |
|
110 } |
176 } |
111 |
177 |
112 void PhysicsObject::updatePhysics (Vector position, Vector velocity) { |
178 void PhysicsObject::updatePhysics (Vector position, Vector velocity) { |
113 this->position = position; |
179 this->position = position; |
114 this->velocity = velocity; |
180 this->velocity = velocity; |