242 lines
6.1 KiB
C
242 lines
6.1 KiB
C
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#include "lephisto.h"
|
|
#include "log.h"
|
|
#include "physics.h"
|
|
|
|
/* ================ */
|
|
/* MISC */
|
|
/* ================ */
|
|
double angle_diff(const double ref, double a) {
|
|
double d;
|
|
if(a < M_PI) a += 2*M_PI;
|
|
d = fmod((a-ref), 2*M_PI);
|
|
return (d <= M_PI) ? d : d - 2*M_PI;
|
|
}
|
|
|
|
void limit_speed(Vec2* vel, const double speed, const double dt) {
|
|
double vmod;
|
|
vmod = VMOD(*vel);
|
|
if(vmod > speed) /* Should not go faster. */
|
|
vect_pset(vel, (vmod-speed)*(1.-dt*3.) + speed, VANGLE(*vel));
|
|
}
|
|
|
|
/* ================ */
|
|
/* VEC2 */
|
|
/* ================ */
|
|
|
|
/* Set the vector value using cartesian coords. */
|
|
void vect_cset(Vec2* v, const double x, const double y) {
|
|
v->x = x;
|
|
v->y = y;
|
|
v->mod = MOD(x,y);
|
|
v->angle = ANGLE(x, y);
|
|
}
|
|
|
|
/* Create a minimal vector, only valid for blitting. */
|
|
void vect_csetmin(Vec2* v, const double x, const double y) {
|
|
v->x = x;
|
|
v->y = y;
|
|
}
|
|
|
|
/* Set the vector value using polar coords. */
|
|
void vect_pset(Vec2* v, const double mod, const double angle) {
|
|
v->mod = mod;
|
|
v->angle = angle;
|
|
v->x = v->mod*cos(v->angle);
|
|
v->y = v->mod*sin(v->angle);
|
|
}
|
|
|
|
/* Copy vector source to destination. */
|
|
void vectcpy(Vec2* dest, const Vec2* src) {
|
|
dest->x = src->x;
|
|
dest->y = src->y;
|
|
dest->mod = src->mod;
|
|
dest->angle = src->angle;
|
|
}
|
|
|
|
/* Null a vector. */
|
|
void vectnull(Vec2* v) {
|
|
v->x = v->y = v->mod = v->angle = 0.;
|
|
}
|
|
|
|
/* Get the direction pointed to by two vectors (from ref to v). */
|
|
double vect_angle(const Vec2* ref, const Vec2* v) {
|
|
return ANGLE(v->x - ref->x, v->y - ref->y);
|
|
}
|
|
|
|
void vect_cadd(Vec2* v, const double x, const double y) {
|
|
v->x += x;
|
|
v->y += y;
|
|
v->mod = MOD(v->x, v->y);
|
|
v->angle = ANGLE(v->x, v->y);
|
|
}
|
|
|
|
/* Mirrors a vector off another, stores results in vector. */
|
|
void vect_reflect(Vec2* r, Vec2* v, Vec2* n) {
|
|
double dot;
|
|
|
|
dot = (v->x*n->x) + (v->y*n->y);
|
|
r->x = v->x - ((2. * dot) * n->x);
|
|
r->y = v->y - ((2. * dot) * n->y);
|
|
r->mod = MOD(r->x, r->y);
|
|
r->angle = MOD(r->x, r->y);
|
|
|
|
}
|
|
|
|
/* ================ */
|
|
/* SOLID! */
|
|
/* ================ */
|
|
|
|
/* ==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. */
|
|
/* ======================================================== */
|
|
#if 0 /* Simply commenting this out to avoid silly warnings. */
|
|
static void simple_update(Solid* obj, const double dt) {
|
|
/* Make sure angle doesn't flip. */
|
|
obj->dir += M_PI/360.*obj->dir_vel*dt;
|
|
if(obj->dir > 2*M_PI) obj->dir -= 2*M_PI;
|
|
if(obj->dir < 0.) obj->dir += 2*M_PI;
|
|
|
|
double px, py, vx, vy;
|
|
px = obj->pos->x;
|
|
py = obj->pos->y;
|
|
vx = obj->vel->x;
|
|
vy = obj->vel->y;
|
|
|
|
if(obj->force.mod) { /* Force applied on an object. */
|
|
double ax, ay;
|
|
ax = obj->force->x/obj->mass;
|
|
ay = obj->force->y/obj->mass;
|
|
|
|
vx += ax*dt;
|
|
vy += ay*dt;
|
|
|
|
px += vx*dt + 0.5*ax * dt*dt;
|
|
py += vy*dt + 0.5*ay * dt*dt;
|
|
|
|
obj->vel.mod = MOD(vx, vy);
|
|
obj->vel.angle = ANGLE(vx, vy);
|
|
} else {
|
|
px += vx*dt;
|
|
py += vy*dt;
|
|
}
|
|
obj->pos.mod = MOD(px, py);
|
|
obj->pos.angle = ANGLE(px, py);
|
|
}
|
|
#endif
|
|
|
|
/* ==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_MIN_H 0.01 /* Minimal pass we want. */
|
|
static void rk4_update(Solid* obj, const double dt) {
|
|
int i, N; /* For iteration and pass calculation. */
|
|
double h, px, py, vx, vy; /* Pass and position/velocity values. */
|
|
double ix, iy, tx, ty, ax, ay; /* Initial and temp cartesian vector values. */
|
|
/* Make sure angle doesn't flip. */
|
|
obj->dir += M_PI/180.*obj->dir_vel*dt;
|
|
if(obj->dir >= 2.*M_PI) obj->dir -= 2*M_PI;
|
|
else if(obj->dir < 0.) obj->dir += 2*M_PI;
|
|
|
|
N = (dt > RK4_MIN_H) ? (int)(dt/RK4_MIN_H) : 1;
|
|
h = dt / (double)N; /* Step. */
|
|
|
|
px = obj->pos.x;
|
|
py = obj->pos.y;
|
|
vx = obj->vel.x;
|
|
vy = obj->vel.y;
|
|
|
|
if(obj->force.mod) { /* Force applied on object. */
|
|
/* Movement quantity theorem : m*a = \sum f. */
|
|
ax = obj->force.x / obj->mass;
|
|
ay = obj->force.y / obj->mass;
|
|
|
|
for(i = 0; i < N; i++) {
|
|
/* X component. */
|
|
tx = ix = vx;
|
|
tx += 2.*ix + h*tx;
|
|
tx += 2.*ix + h*tx;
|
|
tx += ix + h*tx;
|
|
tx *= h/6.;
|
|
|
|
px += tx;
|
|
vx += ax*h;
|
|
|
|
/* Y component. */
|
|
ty = iy = vy;
|
|
ty += 2.*(iy + h/2.*ty);
|
|
ty += 2.*(iy + h/2.*ty);
|
|
ty += iy + h*ty;
|
|
ty *= h/6.;
|
|
|
|
py += ty;
|
|
vy += ay*h;
|
|
}
|
|
vect_cset(&obj->vel, vx, vy);
|
|
} else {
|
|
/* Euler method -> p = v*t + 0.5*a*t^2 (no accel, so no error). */
|
|
px += dt*vx;
|
|
py += dt*vy;
|
|
}
|
|
vect_cset(&obj->pos, px, py);
|
|
}
|
|
|
|
/* Initialize a new solid. */
|
|
void solid_init(Solid* dest, const double mass, const double dir,
|
|
const Vec2* pos, const Vec2* vel) {
|
|
dest->mass = mass;
|
|
|
|
dest->dir_vel = 0.;
|
|
|
|
vect_cset(&dest->force, 0., 0.);
|
|
dest->dir = dir;
|
|
if((dest->dir > 2.*M_PI) || (dest->dir < 0.))
|
|
dest->dir = fmod(dest->dir, 2*M_PI);
|
|
|
|
if(vel == NULL) vectnull(&dest->vel);
|
|
else vectcpy(&dest->vel, vel);
|
|
|
|
if(pos == NULL) vectnull(&dest->pos);
|
|
else vectcpy(&dest->pos, pos);
|
|
|
|
dest->update = rk4_update;
|
|
}
|
|
|
|
/* Create a new solid. */
|
|
Solid* solid_create(const double mass, const double dir,
|
|
const Vec2* pos, const Vec2* vel) {
|
|
Solid* dyn = MALLOC_L(Solid);
|
|
if(dyn == NULL) ERR("Out of memory");
|
|
solid_init(dyn, mass, dir, pos, vel);
|
|
return dyn;
|
|
}
|
|
|
|
/* Free an existing solid. */
|
|
void solid_free(Solid* src) {
|
|
free(src);
|
|
src = NULL;
|
|
}
|
|
|