1 /*
2 Copyright (C) 2009 Facundo Domínguez
3
4 This file is part of Spacejunk.
5
6 Spacejunk is free software: you can redistribute it and/or modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation, either version 3 of the License, or
9 (at your option) any later version.
10
11 Foobar is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 GNU General Public License for more details.
15
16 You should have received a copy of the GNU General Public License
17 along with Foobar. If not, see <http://www.gnu.org/licenses/>.
18 */
19
20 #include "physicbody.h"
21 #include <math.h>
22 #include <algorithm>
23 #include <vector>
24
25 using namespace std;
26
27 template<class T>
28 class DynArray {
29 private:
30 std::vector<T> data;
31
32 public:
DynArray(size_t len)33 DynArray(size_t len) : data(len) { }
34
operator const T*() const35 operator const T*() const { return data.data(); }
operator T*()36 operator T*() { return data.data(); }
37 };
38
39 const double PhysicEngine::G=6.6726e-17;
40
PhysicBody(real m,real x0,real y0,real vx0,real vy0,real w)41 PhysicBody::PhysicBody(real m,real x0,real y0,real vx0,real vy0,real w):
42 mass(m),w(w),pos(x0,y0),vel(vx0,vy0) {
43 angle=0;
44 w=0;
45 waccel=0;
46 lastTestPos=previous_pos=next_pos=pos;
47 previous_vel=next_vel=vel;
48 id=-1;
49 flags=DEFAULTS;
50 }
51
PhysicBody(Vector2d pos,Vector2d vel,real m,real w)52 PhysicBody::PhysicBody(Vector2d pos,Vector2d vel,real m,real w) :
53 mass(m),w(w),pos(pos),vel(vel) {
54 angle=0;
55 w=0;
56 waccel=0;
57 lastTestPos=previous_pos=next_pos=pos;
58 previous_vel=next_vel=vel;
59 id=-1;
60 flags=DEFAULTS;
61 }
62
setVelocity(const Vector2d & vel)63 void PhysicBody::setVelocity(const Vector2d & vel) {
64 this->vel=previous_vel=next_vel=vel;
65 };
setPosition(const Vector2d & pos)66 void PhysicBody::setPosition(const Vector2d & pos) {
67 lastTestPos=this->pos=previous_pos=next_pos=pos;
68 };
69
70
saveState()71 void PhysicBody::saveState() {
72 previous_pos=pos;
73 previous_vel=vel;
74 }
75
76
testForCollision()77 void PhysicBody::testForCollision() {
78 lastTestPos=getPosition();
79 };
lastTestPosition()80 Vector2d PhysicBody::lastTestPosition() {
81 return lastTestPos;
82 };
83
stepRotation(int delta)84 void PhysicBody::stepRotation(int delta) {
85 real a=(w+waccel*delta/2)*delta;
86 if (a!=0) setAngle(getAngle()+a);
87 if (angle>360) angle-=360;
88 else if (angle<0) angle+=360;
89 w+=waccel*delta;
90 };
91
influence(const Vector2d & dist,real mass)92 inline Vector2d influence(const Vector2d & dist,real mass) {
93 double d=dist.dist();
94 d*=d*d;
95 return (d>0 ? dist*(PhysicEngine::G*mass/d) : Vector2d());
96 }
97
clone()98 PhysicBody * PhysicBody::clone() {
99 return new PhysicBody(*this);
100 };
101
copy(PhysicBody * dest) const102 void PhysicBody::copy(PhysicBody * dest) const {
103 *dest=*this;
104 };
105
~PhysicBody()106 PhysicBody::~PhysicBody() {};
107
PhysicEngine(const PlayerState & ps)108 PhysicEngine::PhysicEngine(const PlayerState & ps) : ps(ps) {
109 posFactor=0;
110 posTime=0;
111 loadtime=1;
112 loadcount=6;
113 bigStep=160;
114 minDist=0;
115 };
116
PhysicEngine(const PhysicEngine & p)117 PhysicEngine::PhysicEngine(const PhysicEngine & p): ps(p.ps) {
118 *this=p;
119 };
120
clear()121 void PhysicEngine::clear() {
122 ps.remainingtrash=0;
123
124 posFactor=0;
125 loadtime=1;
126 loadcount=6;
127 heavyBodies.clear();
128 bodies.clear();
129 erased.clear();
130 };
131
heavy(const PhysicBody * b)132 inline bool heavy(const PhysicBody * b) {
133 return b->mass>=1e10;
134 }
135
addBody(PhysicBody * b)136 void PhysicEngine::addBody(PhysicBody * b) {
137 if (heavy(b)) heavyBodies.push_back(b);
138 if (erased.empty()) {
139 b->id=bodies.size();
140 bodies.push_back(b);
141 } else {
142 b->id=erased.front();
143 bodies[b->id]=b;
144 erased.pop_front();
145 }
146 };
147
addBody(int id,PhysicBody * b)148 void PhysicEngine::addBody(int id,PhysicBody * b) {
149 if (heavy(b)) heavyBodies.push_back(b);
150 if (int(bodies.size())<id) {
151 bodies.reserve(id+1);
152 bodies.resize(id+1,NULL);
153 }
154 if (!bodies[id]) erased.remove(id);
155 b->id=id;
156 bodies[b->id]=b;
157 };
158
deleteBody(int id)159 void PhysicEngine::deleteBody(int id) {
160 if (!bodies[id]) return;
161 heavyBodies.remove(bodies[id]);
162 bodies[id]=NULL;
163 erased.push_front(id);
164 };
165
166
refitBody(PhysicBody * b)167 void PhysicEngine::refitBody(PhysicBody *b) {
168 if (heavyBodies.end()==find(heavyBodies.begin(),heavyBodies.end(),b)) {
169 if (b->mass>=1e10) heavyBodies.push_back(b);
170 } else if (b->mass<1e10) heavyBodies.remove(b);
171 };
172
calculateAcceleration(const PhysicBody & affected)173 inline Vector2d PhysicEngine::calculateAcceleration(const PhysicBody & affected) {
174 Vector2d acc;
175 for (list<PhysicBody*>::iterator i=heavyBodies.begin();i!=heavyBodies.end();i++)
176 if (*i!=&affected) acc+=influence((*i)->pos-affected.pos,(*i)->mass);
177 return acc;
178 }
179
_vstep(int delta)180 void PhysicEngine::_vstep (int delta) {
181 #define DELTA_LIMIT 3
182 #define ERROR_LIMIT 0.001
183 minDist=0;
184 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
185 if (!*j) continue;
186 (*j)->saveState();
187 }
188
189 real error;
190 real tempdelta=delta;
191 real time=0;
192 int n=bodies.size();
193 DynArray<Vector2d> accels(n);
194 calculateForces(0,accels);
195 loadcount++;
196 DynArray<Vector2d> temp_pos(n);
197 DynArray<Vector2d> temp_vel(n);
198 do {
199 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
200 if (!*j) continue;
201 temp_pos[(*j)->id]=(*j)->pos;
202 temp_vel[(*j)->id]=(*j)->vel;
203 }
204 error = vstepRK5(tempdelta,accels);
205 real delta_before=tempdelta;
206 if (error>0) {
207 if (ERROR_LIMIT>=error && real(loadcount)/real(loadtime)<15)
208 tempdelta=tempdelta*0.9*pow(ERROR_LIMIT/error,0.20);
209 else
210 tempdelta=tempdelta*0.9*pow(ERROR_LIMIT/error,0.25);
211 if (tempdelta<DELTA_LIMIT)
212 tempdelta=DELTA_LIMIT;
213 } else
214 tempdelta=delta-time;
215 if ((error>ERROR_LIMIT && real(loadcount)/real(loadtime)<15) || tempdelta<DELTA_LIMIT)
216 if (tempdelta>DELTA_LIMIT) {
217 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
218 if (!*j) continue;
219 (*j)->pos=temp_pos[(*j)->id];
220 (*j)->vel=temp_vel[(*j)->id];
221 }
222 } else {
223 time+=delta_before;
224 if (time<delta) {
225 calculateForces(time,accels);
226 ++loadcount;
227 }
228 }
229 else {
230 time+=delta_before;
231 if (time<delta) {
232 calculateForces(time,accels);
233 ++loadcount;
234 }
235 }
236 if (tempdelta>delta-time)
237 tempdelta=delta-time;
238 loadcount+=5;
239 } while (time<delta);
240 loadtime++;
241 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
242 if (!*j) continue;
243 (*j)->next_pos=(*j)->pos;
244 (*j)->next_vel=(*j)->vel;
245 }
246 }
247
248
step(int delta)249 void PhysicEngine::step(int delta) {
250 step(delta,null);
251 }
252
callBodiesStep(int delta)253 inline void PhysicEngine::callBodiesStep(int delta) {
254 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
255 if (*j) (*j)->step(delta);
256 }
257 };
258
_step(int delta,bool best)259 void PhysicEngine::_step(int delta,bool best) {
260 if (delta<posFactor) {
261 for (vector<PhysicBody*>::iterator i=bodies.begin();i!=bodies.end();i++) {
262 if (*i && !((*i)->flags & PhysicBody::FIXED)) {
263 Vector2d veldiff=delta/real(posTime)*((*i)->next_vel-(*i)->previous_vel);
264 (*i)->pos+=delta*((*i)->vel+veldiff/2.0);
265 (*i)->vel+=veldiff;
266 (*i)->stepRotation(delta);
267 }
268 }
269 posFactor-=delta;
270 } else {
271 for (vector<PhysicBody*>::iterator i=bodies.begin();i!=bodies.end();i++) {
272 if (*i && !((*i)->flags & PhysicBody::FIXED)) {
273 (*i)->pos=(*i)->next_pos;
274 (*i)->vel=(*i)->next_vel;
275 (*i)->stepRotation(posFactor);
276 }
277 }
278 if (posTime>0) {
279 callBodiesStep(posTime);
280 posTime=0;
281 }
282 if (delta>posFactor) {
283 delta-=posFactor;
284 int original_delta=delta;
285 if (!best && delta<bigStep)
286 delta=bigStep;
287
288 _vstep(delta);
289 if (original_delta<delta) {
290 for (vector<PhysicBody*>::iterator i=bodies.begin();i!=bodies.end();i++) {
291 if (*i) {
292 if (!((*i)->flags & PhysicBody::FIXED)) {
293 Vector2d veldiff=original_delta/real(delta)*((*i)->next_vel-(*i)->previous_vel);
294 (*i)->pos=(*i)->previous_pos+original_delta*((*i)->previous_vel+veldiff/2.0);
295 (*i)->vel=(*i)->previous_vel+veldiff;
296 }
297 (*i)->stepRotation(original_delta);
298 }
299 }
300 posTime=delta;
301 posFactor=delta-original_delta;
302 } else {
303 for (vector<PhysicBody*>::iterator i=bodies.begin();i!=bodies.end();i++) {
304 if (*i)
305 (*i)->stepRotation(original_delta);
306 }
307 callBodiesStep(original_delta);
308 posTime=posFactor=0;
309 }
310 } else posFactor=0;
311 }
312 }
313
314 /*
315 Old and lovely Runge-Kutta. (5th order).
316 */
vstepRK5(real delta,Vector2d initaccels[])317 real PhysicEngine::vstepRK5 (real delta,Vector2d initaccels[]) {
318 real maxdist=0;
319 int n=bodies.size();
320 DynArray<Vector2d> accels(n);
321 DynArray<Vector2d> mipos(n);
322 DynArray<Vector2d> k1pos(n);
323 DynArray<Vector2d> k1vel(n);
324 DynArray<Vector2d> k2pos(n);
325 DynArray<Vector2d> k2vel(n);
326 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
327 if (!*j || ((*j)->flags & PhysicBody::FIXED)) continue;
328 mipos[(*j)->id]=(*j)->pos;
329 k1pos[(*j)->id]=(*j)->vel*delta;
330 k1vel[(*j)->id]=initaccels[(*j)->id]*delta;
331 k2pos[(*j)->id]=((*j)->vel+k1vel[(*j)->id]*0.2)*delta;
332 (*j)->pos=mipos[(*j)->id]+k1pos[(*j)->id]*0.2;
333 }
334 calculateForces(delta*0.2,accels);
335 DynArray<Vector2d> k3pos(n);
336 DynArray<Vector2d> k3vel(n);
337 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
338 if (!*j || ((*j)->flags & PhysicBody::FIXED)) continue;
339 k2vel[(*j)->id]=accels[(*j)->id]*delta;
340 k3pos[(*j)->id]=((*j)->vel+k1vel[(*j)->id]*(3.0/40.0)+k2vel[(*j)->id]*(9.0/40.0))*delta;
341 (*j)->pos=mipos[(*j)->id]+k1pos[(*j)->id]*(3.0/40.0)+k2pos[(*j)->id]*(9.0/40.0);
342 }
343 calculateForces(delta*0.3,accels);
344 DynArray<Vector2d> k4pos(n);
345 DynArray<Vector2d> k4vel(n);
346 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
347 if (!*j || ((*j)->flags & PhysicBody::FIXED)) continue;
348 k3vel[(*j)->id]=accels[(*j)->id]*delta;
349 k4pos[(*j)->id]=((*j)->vel+k1vel[(*j)->id]*0.3+k2vel[(*j)->id]*(-0.9)+k3vel[(*j)->id]*1.2)*delta;
350 (*j)->pos=mipos[(*j)->id]+k1pos[(*j)->id]*0.3+k2pos[(*j)->id]*(-0.9)+k3pos[(*j)->id]*1.2;
351 }
352 calculateForces(delta*0.6,accels);
353 DynArray<Vector2d> k5pos(n);
354 DynArray<Vector2d> k5vel(n);
355 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
356 if (!*j || ((*j)->flags & PhysicBody::FIXED)) continue;
357 k4vel[(*j)->id]=accels[(*j)->id]*delta;
358 k5pos[(*j)->id]=((*j)->vel+k1vel[(*j)->id]*(-11.0/54.0)+k2vel[(*j)->id]*2.5+k3vel[(*j)->id]*(-70.0/27.0)+k4vel[(*j)->id]*(35.0/27.0))*delta;
359 (*j)->pos=mipos[(*j)->id]+k1pos[(*j)->id]*(-11.0/54.0)+k2pos[(*j)->id]*2.5+k3pos[(*j)->id]*(-70.0/27.0)+k4pos[(*j)->id]*(35.0/27.0);
360 }
361 calculateForces(delta,accels);
362 DynArray<Vector2d> k6pos(n);
363 DynArray<Vector2d> k6vel(n);
364 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
365 if (!*j || ((*j)->flags & PhysicBody::FIXED)) continue;
366 k5vel[(*j)->id]=accels[(*j)->id]*delta;
367 k6pos[(*j)->id]=((*j)->vel+k1vel[(*j)->id]*(1631.0/55296.0)+k2vel[(*j)->id]*(175.0/512.0)+k3vel[(*j)->id]*(575.0/13824.0)+k4vel[(*j)->id]*(44275.0/110592.0)+k5vel[(*j)->id]*(253.0/4096.0))*delta;
368 (*j)->pos=mipos[(*j)->id]+k1pos[(*j)->id]*(1631.0/55296.0)+k2pos[(*j)->id]*(175.0/512.0)+k3pos[(*j)->id]*(575.0/13824.0)+k4pos[(*j)->id]*(44275.0/110592.0)+k5pos[(*j)->id]*(253.0/4096.0);
369 }
370 calculateForces(delta*7.0/8,accels);
371 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
372 if (!*j || ((*j)->flags & PhysicBody::FIXED)) continue;
373 k6vel[(*j)->id]=accels[(*j)->id]*delta;
374
375 (*j)->pos=mipos[(*j)->id]+(37.0/378.0)*k1pos[(*j)->id]+(250.0/621.0)*k3pos[(*j)->id]+(125.0/594.0)*k4pos[(*j)->id]+(512.0/1771.0)*k6pos[(*j)->id];
376 Vector2d pos2=mipos[(*j)->id]+(2825.0/27648.0)*k1pos[(*j)->id]+(18575.0/48384.0)*k3pos[(*j)->id]+(13525.0/55296.0)*k4pos[(*j)->id]+(277.0/14336.0)*k5pos[(*j)->id]+0.25*k6pos[(*j)->id];
377 (*j)->vel+=(37.0/378.0)*k1vel[(*j)->id]+(250.0/621.0)*k3vel[(*j)->id]+(125.0/594.0)*k4vel[(*j)->id]+(512.0/1771.0)*k6vel[(*j)->id];
378
379 real dist=((*j)->pos-pos2).dist();
380 if (dist>maxdist)
381 maxdist=dist;
382 }
383
384 return maxdist;
385 }
386
influenceForce(const Vector2d & dist)387 inline Vector2d PhysicEngine::influenceForce(const Vector2d & dist) {
388 double d=dist.dist();
389 if (d<minDist && 0<d)
390 minDist=d;
391 d*=d*d;
392 return (d>100 ? dist*(PhysicEngine::G/d) : Vector2d());
393 }
394
calculateForces(real delta,Vector2d accels[])395 void PhysicEngine::calculateForces(real delta,Vector2d accels[]) {
396 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
397 if (*j)
398 accels[(*j)->id]=(*j)->getAcceleration(delta);
399 }
400
401 for (list<PhysicBody*>::iterator j=heavyBodies.begin();j!=heavyBodies.end();j++) {
402 if (*j) {
403 list<PhysicBody*>::iterator i=j;
404 for (i++;i!=heavyBodies.end();i++) {
405 if (*i && !((*j)->flags & (*i)->flags & PhysicBody::FIXED)) {
406 Vector2d acc=influenceForce((*i)->pos-(*j)->pos);
407 accels[(*i)->id]-=acc*(*j)->mass;
408 accels[(*j)->id]+=acc*(*i)->mass;
409 }
410 }
411 }
412 }
413
414 for (vector<PhysicBody*>::iterator j=bodies.begin();j!=bodies.end();j++) {
415 if (*j && !heavy(*j)) {
416 for (list<PhysicBody*>::iterator i=heavyBodies.begin();i!=heavyBodies.end();i++) {
417 if (*i && !((*j)->flags & (*i)->flags & PhysicBody::FIXED)) {
418 Vector2d acc=influenceForce((*i)->pos-(*j)->pos);
419 accels[(*i)->id]-=acc*(*j)->mass;
420 accels[(*j)->id]+=acc*(*i)->mass;
421 }
422 }
423 }
424 }
425 }
426
unstep()427 int PhysicEngine::unstep() {
428 int backTime=0;
429 if (posFactor) {
430 backTime=posTime-posFactor;
431 for (vector<PhysicBody*>::iterator i=bodies.begin();i!=bodies.end();i++) {
432 if (!*i) continue;
433 (*i)->unstep();
434 (*i)->stepRotation(-backTime);
435 }
436 posTime=posFactor=0;
437 }
438 if (loadcount>1000000)
439 loadcount=6;
440 loadtime=1;
441
442 return backTime;
443 }
444
getInterpolatedTime()445 int PhysicEngine::getInterpolatedTime() {
446 return posFactor? posTime-posFactor :0;
447 }
448
449
operator =(const PhysicEngine & pe)450 PhysicEngine & PhysicEngine::operator = (const PhysicEngine & pe) {
451 minDist=pe.minDist;
452 posFactor=pe.posFactor;
453 posTime=pe.posTime;
454 bigStep=pe.bigStep;
455 loadcount=pe.loadcount;
456 loadtime=pe.loadtime;
457 bodies.clear();
458 ps=pe.ps;
459 heavyBodies.clear();
460 erased.clear();
461
462 bodies.reserve(pe.bodies.size());
463 for (vector<PhysicBody*>::const_iterator i=pe.bodies.begin();i!=pe.bodies.end();i++)
464 if (*i) addBody((*i)->clone());
465 else bodies.push_back(NULL);
466
467 erased=pe.erased;
468
469 return *this;
470 };
471
472