135 lines
3.2 KiB
C
135 lines
3.2 KiB
C
#include <math.h>
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
|
|
#include "physics.h"
|
|
|
|
#ifndef M_PI
|
|
#define M_PI 3.14159265358979323846f
|
|
#endif
|
|
|
|
// Pretty efficient, no need for sine table!!
|
|
#define SIN(dir)(sinf(dir))
|
|
#define COS(dir)(cosf(dir))
|
|
|
|
// ==Update method.========================================
|
|
// d^2 x(t) / d t^2 = a, a = constant (acceleration)
|
|
// x'(0) = v, x(0) = p
|
|
//
|
|
// d x(t) / d t = a*t + v, v = constant (initial velocity)
|
|
// x(t) = a/2*t + v*t + p, p = constant (initial position)
|
|
//
|
|
// Since dt isn't actually differential this gives us an
|
|
// error, so watch out with big values for dt.
|
|
// ========================================================
|
|
|
|
static void simple_update(Solid* obj, const FP dt) {
|
|
if(obj->force) {
|
|
Vec2 acc;
|
|
acc.x = obj->force / obj->mass * COS(obj->dir);
|
|
acc.y = obj->force / obj->mass * SIN(obj->dir);
|
|
|
|
obj->pos.x += acc.x * dt;
|
|
obj->vel.y += acc.y * dt;
|
|
|
|
obj->pos.x += obj->vel.x * dt + 0.5 * acc.x / obj->mass * dt * dt;
|
|
obj->pos.y += obj->vel.y * dt + 0.5 * acc.y / obj->mass * dt * dt;
|
|
} else {
|
|
obj->pos.x += obj->vel.x * dt;
|
|
obj->pos.y += obj->vel.y * dt;
|
|
}
|
|
}
|
|
|
|
// ==Runge-Kutta 4th method.===============================
|
|
// d^2 x(t) / d t^2 = a, a = constant(acceleration)
|
|
// x'(0) = v, x(0) = p
|
|
// x'' = f(t, x, x') = (x', a)
|
|
//
|
|
// x_ {n+1} = x_n + h/6 (k1 + 2*k2 + 3*k3 + k4)
|
|
// h = (b-a)/2
|
|
// k1 = f(t_n, X_n), X_n = (x_n, x'_n)
|
|
// k2 = f(t_n + h/2, X_n + h/2*k1)
|
|
// k3 = f(t_n + h/2, X_n + h/2*k2)
|
|
// k4 = f(t_n + h, X_n + h*k3)
|
|
//
|
|
// x_{n+1} = x_n + h/6x'_n + 3*h*a, 4*a)
|
|
// ========================================================
|
|
|
|
#define RK4_N 4
|
|
static void rk4_update(Solid* obj, const FP dt) {
|
|
FP h = dt / RK4_N; // Step.
|
|
|
|
if(obj->force) { // Force applied on object.
|
|
int i;
|
|
Vec2 initial, tmp;
|
|
|
|
Vec2 acc;
|
|
acc.x = obj->force / obj->mass * COS(obj->dir);
|
|
acc.y = obj->force / obj->mass * SIN(obj->dir);
|
|
|
|
for(i = 0; i < RK4_N; i++) {
|
|
// X component.
|
|
tmp.x = initial.x = obj->vel.x;
|
|
tmp.x += 2*initial.x + h*tmp.x;
|
|
tmp.x += 2*initial.x + h*tmp.x;
|
|
tmp.x += initial.x + h*tmp.x;
|
|
tmp.x *= h/6;
|
|
|
|
obj->pos.x += tmp.x;
|
|
obj->vel.x += acc.x*h;
|
|
|
|
// Y component.
|
|
tmp.y = initial.y = obj->vel.y;
|
|
tmp.y += 2*(initial.y + h/2*tmp.y);
|
|
tmp.y += 2*(initial.y + h/2*tmp.y);
|
|
tmp.y += initial.y + h*tmp.y;
|
|
tmp.y *= h/6;
|
|
|
|
obj->pos.y += tmp.y;
|
|
obj->pos.y += acc.y*h;
|
|
}
|
|
} else {
|
|
obj->pos.x += dt*obj->vel.x;
|
|
obj->pos.y += dt*obj->vel.y;
|
|
}
|
|
}
|
|
|
|
// Initialize a new solid.
|
|
void solid_init(Solid* dest, const FP mass, const Vec2* vel, const Vec2* pos) {
|
|
dest->mass = mass;
|
|
|
|
dest->force = 0;
|
|
dest->dir = 0;
|
|
|
|
if(vel == NULL)
|
|
dest->vel.x = dest->vel.y = 0.0;
|
|
else {
|
|
dest->vel.x = vel->x;
|
|
dest->vel.y = vel->y;
|
|
}
|
|
|
|
if(pos == NULL)
|
|
dest->pos.x = dest->pos.y = 0.0;
|
|
else {
|
|
dest->pos.x = pos->x;
|
|
dest->pos.y = pos->y;
|
|
}
|
|
|
|
dest->update = rk4_update;
|
|
}
|
|
|
|
// Create a new solid.
|
|
Solid* solid_create(const FP mass, const Vec2* vel, const Vec2* pos) {
|
|
Solid* dyn = MALLOC_L(Solid);
|
|
assert(dyn != NULL);
|
|
solid_init(dyn, mass, vel, pos);
|
|
return dyn;
|
|
}
|
|
|
|
// Free an existing solid.
|
|
void solid_free(Solid* src) {
|
|
free(src);
|
|
src = NULL;
|
|
}
|
|
|