git.cworth.org Git - akamaru/blob - akamaru.c

   1 /*                                           -*- mode: c; c-basic-offset: 2 -*-
   2  * See:
   3  *
   4  *     http://en.wikipedia.org/wiki/Verlet_integration
   5  *     http://www.teknikus.dk/tj/gdc2001.htm
   6  *
   7  * TODO:
   8  *
   9  * - Add code to add boxes
  10  * - Add circle object
  11  * - Try out this idea: make constraint solver take mean of all
  12  *   corrections at the end instead of meaning as it goes.
  13  */
  14
  15 #include <glib.h>
  16 #include <stdlib.h>
  17 #include <string.h>
  18 #include <stdarg.h>
  19 #include <math.h>
  20
  21 #include "akamaru.h"
  22
  23 const double elasticity = 0.7;
  24 const double friction = 1;
  25 const double gravity = 20;
  26
  27 void
  28 polygon_init (Polygon *p, int num_points, ...)
  29 {
  30   double dx, dy, length;
  31   int i, j;
  32   va_list ap;
  33
  34   /* Polygons are defined counter-clock-wise in a coordinate system
  35    * with the y-axis pointing down. */
  36
  37   va_start (ap, num_points);
  38   p->num_points = num_points;
  39   p->points = g_new (Point, num_points);
  40
  41   for (i = 0; i < num_points; i++) {
  42     p->points[i].x = va_arg (ap, double);
  43     p->points[i].y = va_arg (ap, double);
  44   }
  45   va_end (ap);
  46
  47   p->normals = g_new (Vector, p->num_points);
  48   /* Compute outward pointing normals.  p->normals[i] is the normal
  49    * for the edged between p->points[i] and p->points[i + 1]. */
  50  for (i = 0; i < p->num_points; i++) {
  51     j = (i + 1) % p->num_points;
  52     dx = p->points[j].x - p->points[i].x;
  53     dy = p->points[j].y - p->points[i].y;
  54     length = sqrt (dx * dx + dy * dy);
  55     p->normals[i].x = -dy / length;
  56     p->normals[i].y = dx / length;
  57   }
  58 }
  59
  60 void
  61 polygon_init_diamond (Polygon *polygon, double x, double y)
  62 {
  63   return polygon_init (polygon, 5,
  64                        x, y,
  65                        x + 10, y + 40,
  66                        x + 90, y + 40,
  67                        x + 100, y,
  68                        x + 50, y - 20);
  69 }
  70
  71 void
  72 polygon_init_rectangle (Polygon *polygon, double x0, double y0,
  73                         double x1, double y1)
  74 {
  75   return polygon_init (polygon, 4, x0, y0, x0, y1, x1, y1, x1, y0);
  76 }
  77
  78 void
  79 model_fini (Model *model)
  80 {
  81   int i;
  82
  83   g_free (model->objects);
  84   g_free (model->sticks);
  85   g_free (model->strings);
  86   for (i = 0; i < model->num_offsets; i++)
  87     g_free (model->offsets[i].objects);
  88   g_free (model->springs);
  89   g_free (model->offset_springs);
  90   for (i = 0; i < model->num_polygons; i++)
  91     g_free (model->polygons[i].points);
  92   g_free (model->polygons);
  93
  94   memset (model, 0, sizeof *model);
  95 }
  96
  97 static void
  98 model_accumulate_forces (Model *model)
  99 {
 100   int i;
 101   double x, y, dx, dy, distance, displacement;
 102   Point middle;
 103   Vector u, v;
 104
 105   for (i = 0; i < model->num_objects; i++) {
 106     /* Gravity */
 107     model->objects[i].force.x = 0;
 108     model->objects[i].force.y = gravity * model->objects[i].mass;
 109
 110     /* Friction */
 111     v.x = model->objects[i].position.x - model->objects[i].previous_position.x;
 112     v.y = model->objects[i].position.y - model->objects[i].previous_position.y;
 113     model->objects[i].force.x -= v.x * friction;
 114     model->objects[i].force.y -= v.y * friction;
 115   }
 116
 117   for (i = 0; i < model->num_springs; i++) {
 118     x = model->springs[i].a->position.x;
 119     y = model->springs[i].a->position.y;
 120     dx = model->springs[i].b->position.x - x;
 121     dy = model->springs[i].b->position.y - y;
 122     distance = sqrt (dx * dx + dy * dy);
 123     u.x = dx / distance;
 124     u.y = dy / distance;
 125     displacement = distance - model->springs[i].length;
 126     model->springs[i].a->force.x += u.x * model->k * displacement;
 127     model->springs[i].a->force.y += u.y * model->k * displacement;
 128     model->springs[i].b->force.x -= u.x * model->k * displacement;
 129     model->springs[i].b->force.y -= u.y * model->k * displacement;
 130   }
 131
 132   for (i = 0; i < model->num_offset_springs; i++) {
 133     middle.x =
 134       (model->offset_springs[i].a->position.x +
 135        model->offset_springs[i].b->position.x) / 2;
 136     middle.y =
 137       (model->offset_springs[i].a->position.y +
 138        model->offset_springs[i].b->position.y) / 2;
 139
 140     x = middle.x - model->offset_springs[i].dx / 2;
 141     y = middle.y - model->offset_springs[i].dy / 2;
 142
 143     dx = x - model->offset_springs[i].a->position.x;
 144     dy = y - model->offset_springs[i].a->position.y;
 145
 146     model->offset_springs[i].a->force.x += dx * model->k;
 147     model->offset_springs[i].a->force.y += dy * model->k;
 148     model->offset_springs[i].b->force.x -= dx * model->k;
 149     model->offset_springs[i].b->force.y -= dy * model->k;
 150   }
 151
 152   for (i = 0; i < model->num_objects; i++) {
 153     double f =
 154       model->objects[i].force.x * model->objects[i].force.x +
 155       model->objects[i].force.y * model->objects[i].force.y;
 156
 157     if (f > 100000000)
 158       abort();
 159   }
 160 }
 161
 162 static void
 163 model_integrate (Model *model, double step)
 164 {
 165   double x, y;
 166   Object *o;
 167   int i;
 168
 169   for (i = 0; i < model->num_objects; i++) {
 170     o = &model->objects[i];
 171     x = o->position.x;
 172     y = o->position.y;
 173
 174     o->position.x =
 175       x + (x - o->previous_position.x) + o->force.x * step * step;
 176     o->position.y =
 177       y + (y - o->previous_position.y) + o->force.y * step * step;
 178
 179     o->previous_position.x = x;
 180     o->previous_position.y = y;
 181   }
 182 }
 183
 184 /* The square root in the distance computation for the string and
 185  * stick constraints can be aproximated using Newton:
 186  *
 187  *    distance =
 188  *      (model->sticks[i].length +
 189  *       (dx * dx + dy * dy) / model->sticks[i].length) / 2;
 190  *
 191  * This works really well, since the constraints aren't typically
 192  * violated much.  Thus, the distance is really close to the stick
 193  * length, which then makes a good initial guess.  However, the
 194  * approximation seems to be slower that just calling sqrt()...
 195  */
 196
 197 static inline double
 198 estimate_distance (double dx, double dy, double r)
 199 {
 200 #ifdef APPROXIMATE_SQUARE_ROOTS
 201   return (r + (dx * dx + dy * dy) / r) / 2;
 202 #else
 203   return sqrt (dx * dx + dy * dy);
 204 #endif
 205 }
 206
 207 static int
 208 polygon_contains_point (Polygon *polygon, Point *point)
 209 {
 210   int i;
 211   double dx, dy;
 212
 213   for (i = 0; i < polygon->num_points; i++) {
 214     dx = point->x - polygon->points[i].x;
 215     dy = point->y - polygon->points[i].y;
 216
 217     if (polygon->normals[i].x * dx + polygon->normals[i].y * dy >= 0)
 218       return FALSE;
 219   }
 220
 221   return TRUE;
 222 }
 223
 224 static void
 225 polygon_reflect_object (Polygon *polygon, Object *object)
 226 {
 227   int i, edge;
 228   double d, distance;
 229   Vector *n;
 230
 231   distance = -1000;
 232   for (i = 0; i < polygon->num_points; i++) {
 233     d = polygon->normals[i].x * (object->position.x - polygon->points[i].x) +
 234       polygon->normals[i].y * (object->position.y - polygon->points[i].y);
 235
 236     if (d > distance) {
 237       distance = d;
 238       edge = i;
 239       polygon->edge = i;
 240       n = &polygon->normals[i];
 241     }
 242   }
 243
 244   object->position.x -= (1 + elasticity) * distance * n->x;
 245   object->position.y -= (1 + elasticity) * distance * n->y;
 246
 247   distance =
 248     n->x * (object->previous_position.x - polygon->points[edge].x) +
 249     n->y * (object->previous_position.y - polygon->points[edge].y);
 250
 251   object->previous_position.x -= (1 + elasticity) * distance * n->x;
 252   object->previous_position.y -= (1 + elasticity) * distance * n->y;
 253 }
 254
 255 static void
 256 model_constrain_polygon (Model *model, Polygon *polygon)
 257 {
 258   int i;
 259
 260   for (i = 0; i < model->num_objects; i++) {
 261     if (polygon_contains_point (polygon, &model->objects[i].position))
 262       polygon_reflect_object (polygon, &model->objects[i]);
 263   }
 264 }
 265
 266 static void
 267 model_constrain_offset (Model *model, Offset *offset)
 268 {
 269   double x, y;
 270   int i;
 271
 272   x = 0;
 273   y = 0;
 274   for (i = 0; i < offset->num_objects; i++) {
 275     x += offset->objects[i]->position.x;
 276     y += offset->objects[i]->position.y;
 277   }
 278
 279   x = x / offset->num_objects - offset->dx * (offset->num_objects - 1) / 2;
 280   y = y / offset->num_objects - offset->dy * (offset->num_objects - 1) / 2;
 281
 282   for (i = 0; i < offset->num_objects; i++) {
 283     offset->objects[i]->position.x = x + offset->dx * i;
 284     offset->objects[i]->position.y = y + offset->dy * i;
 285   }
 286 }
 287
 288 static void
 289 model_constrain (Model *model)
 290 {
 291   double dx, dy, x, y, distance, fraction;
 292   int i;
 293
 294   /* Anchor object constraint. */
 295   if (model->anchor_object != NULL) {
 296     model->anchor_object->position.x = model->anchor_position.x;
 297     model->anchor_object->position.y = model->anchor_position.y;
 298     model->anchor_object->previous_position.x = model->anchor_position.x;
 299     model->anchor_object->previous_position.y = model->anchor_position.y;
 300   }
 301
 302   /* String constraints. */
 303   for (i = 0; i < model->num_strings; i++) {
 304     x = model->strings[i].a->position.x;
 305     y = model->strings[i].a->position.y;
 306     dx = model->strings[i].b->position.x - x;
 307     dy = model->strings[i].b->position.y - y;
 308     distance = estimate_distance (dx, dy, model->strings[i].length);
 309     if (distance < model->strings[i].length)
 310       continue;
 311     fraction = (distance - model->strings[i].length) / distance / 2;
 312     model->strings[i].a->position.x = x + dx * fraction;
 313     model->strings[i].a->position.y = y + dy * fraction;
 314     model->strings[i].b->position.x = x + dx * (1 - fraction);
 315     model->strings[i].b->position.y = y + dy * (1 - fraction);
 316   }
 317
 318   /* Stick constraints. */
 319   for (i = 0; i < model->num_sticks; i++) {
 320     x = model->sticks[i].a->position.x;
 321     y = model->sticks[i].a->position.y;
 322     dx = model->sticks[i].b->position.x - x;
 323     dy = model->sticks[i].b->position.y - y;
 324     distance = estimate_distance (dx, dy, model->sticks[i].length);
 325     fraction = (distance - model->sticks[i].length) / distance / 2;
 326     model->sticks[i].a->position.x = x + dx * fraction;
 327     model->sticks[i].a->position.y = y + dy * fraction;
 328     model->sticks[i].b->position.x = x + dx * (1 - fraction);
 329     model->sticks[i].b->position.y = y + dy * (1 - fraction);
 330   }
 331
 332   /* Offset constraints. */
 333   for (i = 0; i < model->num_offsets; i++)
 334     model_constrain_offset (model, &model->offsets[i]);
 335
 336   /* Polygon constraints. */
 337   for (i = 0; i < model->num_polygons; i++)
 338     model_constrain_polygon (model, &model->polygons[i]);
 339 }
 340
 341 void
 342 model_step (Model *model, double delta_t)
 343 {
 344   int i;
 345
 346   model_accumulate_forces (model);
 347   model_integrate (model, delta_t);
 348   for (i = 0; i < 50; i++)
 349     model_constrain (model);
 350
 351   model->theta += delta_t;
 352 }
 353
 354 static double
 355 object_distance (Object *object, double x, double y)
 356 {
 357   double dx, dy;
 358
 359   dx = object->position.x - x;
 360   dy = object->position.y - y;
 361
 362   return sqrt (dx*dx + dy*dy);
 363 }
 364
 365 Object *
 366 model_find_nearest (Model *model, double x, double y)
 367 {
 368   Object *object;
 369   double distance, min_distance;
 370   int i;
 371
 372   for (i = 0; i < model->num_objects; i++) {
 373     distance = object_distance (&model->objects[i], x, y);
 374     if (i == 0 || distance < min_distance) {
 375       min_distance = distance;
 376       object = &model->objects[i];
 377     }
 378   }
 379
 380   return object;
 381 }