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