]> git.cworth.org Git - akamaru/blob - akamaru.c
Get rid of 1024x768 screen size assumption.
[akamaru] / 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 = 8;
25 const double gravity = 50;
26
27 void
28 object_init (Object *object, double x, double y, double mass)
29 {
30   object->position.x = x;
31   object->position.y = y;
32   object->previous_position.x = x;
33   object->previous_position.y = y;
34   object->mass = mass;
35 }
36
37 void
38 spring_init (Spring *spring, Object *a, Object *b, double length)
39 {
40   spring->a = a;
41   spring->b = b;
42   spring->length = length;
43 }
44
45 void
46 stick_init (Stick *stick, Object *a, Object *b, double length)
47 {
48   stick->a = a;
49   stick->b = b;
50   stick->length = length;
51 }
52
53 void
54 string_init (String *string, Object *a, Object *b, double length)
55 {
56   string->a = a;
57   string->b = b;
58   string->length = length;
59 }
60
61 void
62 offset_spring_init (OffsetSpring *spring, Object *a, Object *b,
63                     double dx, double dy)
64 {
65   spring->a = a;
66   spring->b = b;
67   spring->dx = dx;
68   spring->dy = dy;
69 }
70
71 void
72 spacer_init (Spacer *spacer, Object *a, Object *b, double length)
73 {
74   spacer->a = a;
75   spacer->b = b;
76   spacer->length = length;
77 }
78
79 void
80 anchor_init (Anchor *anchor, Object *object, double x, double y)
81 {
82   anchor->object = object;
83   anchor->x = x;
84   anchor->y = y;
85 }
86
87 void
88 polygon_init (Polygon *p, int enclosing, int num_points, ...)
89 {
90   double dx, dy, length;
91   int i, j;
92   va_list ap;
93
94   /* Polygons are defined counter-clock-wise in a coordinate system
95    * with the y-axis pointing down. */
96
97   va_start (ap, num_points);
98   p->num_points = num_points;
99   p->points = g_new (Point, num_points);
100   p->enclosing = enclosing;
101
102   for (i = 0; i < num_points; i++) {
103     p->points[i].x = va_arg (ap, double);
104     p->points[i].y = va_arg (ap, double);
105   }
106   va_end (ap);
107   
108   p->normals = g_new (Vector, p->num_points);
109   /* Compute outward pointing normals.  p->normals[i] is the normal
110    * for the edged between p->points[i] and p->points[i + 1]. */
111   for (i = 0; i < p->num_points; i++) {
112     j = (i + 1) % p->num_points;
113     dx = p->points[j].x - p->points[i].x;
114     dy = p->points[j].y - p->points[i].y;
115     length = sqrt (dx * dx + dy * dy);
116     p->normals[i].x = -dy / length;
117     p->normals[i].y = dx / length;
118   }
119 }
120
121 void
122 polygon_init_diamond (Polygon *polygon, double x, double y)
123 {
124   return polygon_init (polygon, FALSE, 5, 
125                        x, y, 
126                        x + 10, y + 40,
127                        x + 90, y + 40,
128                        x + 100, y,
129                        x + 50, y - 20);
130 }
131
132 void
133 polygon_init_rectangle (Polygon *polygon, double x0, double y0,
134                         double x1, double y1)
135 {
136   return polygon_init (polygon, FALSE, 4, x0, y0, x0, y1, x1, y1, x1, y0);
137 }
138
139 void
140 polygon_init_enclosing_rectangle (Polygon *polygon, double x0, double y0,
141                                 double x1, double y1)
142 {
143   return polygon_init (polygon, TRUE, 4, x0, y0, x0, y1, x1, y1, x1, y0);
144 }
145
146 void
147 model_fini (Model *model)
148 {
149   int i;
150
151   g_free (model->objects);
152   g_free (model->sticks);
153   g_free (model->strings);
154   for (i = 0; i < model->num_offsets; i++)
155     g_free (model->offsets[i].objects);
156   g_free (model->springs);
157   g_free (model->offset_springs);
158   g_free (model->spacers);
159   for (i = 0; i < model->num_polygons; i++)
160     g_free (model->polygons[i].points);
161   g_free (model->polygons);
162
163   memset (model, 0, sizeof *model);
164 }
165
166 static void
167 model_accumulate_forces (Model *model)
168 {
169   int i;
170   double x, y, dx, dy, distance, displacement;
171   Point middle;
172   Vector u, v;
173
174   for (i = 0; i < model->num_objects; i++) {
175     /* Gravity */
176     model->objects[i].force.x = 0;
177     model->objects[i].force.y = gravity * model->objects[i].mass;
178
179     /* Friction */
180     v.x = model->objects[i].position.x - model->objects[i].previous_position.x;
181     v.y = model->objects[i].position.y - model->objects[i].previous_position.y;
182     model->objects[i].force.x -= v.x * friction;
183     model->objects[i].force.y -= v.y * friction;
184   }
185
186   for (i = 0; i < model->num_springs; i++) {
187     x = model->springs[i].a->position.x;
188     y = model->springs[i].a->position.y;
189     dx = model->springs[i].b->position.x - x;
190     dy = model->springs[i].b->position.y - y;
191     distance = sqrt (dx * dx + dy * dy);
192     u.x = dx / distance;
193     u.y = dy / distance;
194     displacement = distance - model->springs[i].length;
195     model->springs[i].a->force.x += u.x * model->k * displacement;
196     model->springs[i].a->force.y += u.y * model->k * displacement;
197     model->springs[i].b->force.x -= u.x * model->k * displacement;
198     model->springs[i].b->force.y -= u.y * model->k * displacement;
199   }
200
201   for (i = 0; i < model->num_offset_springs; i++) {
202     middle.x = 
203       (model->offset_springs[i].a->position.x + 
204        model->offset_springs[i].b->position.x) / 2;
205     middle.y = 
206       (model->offset_springs[i].a->position.y + 
207        model->offset_springs[i].b->position.y) / 2;
208
209     x = middle.x - model->offset_springs[i].dx / 2;
210     y = middle.y - model->offset_springs[i].dy / 2;
211
212     dx = x - model->offset_springs[i].a->position.x;
213     dy = y - model->offset_springs[i].a->position.y;
214
215     model->offset_springs[i].a->force.x += dx * model->k;
216     model->offset_springs[i].a->force.y += dy * model->k;
217     model->offset_springs[i].b->force.x -= dx * model->k;
218     model->offset_springs[i].b->force.y -= dy * model->k;
219   }
220
221   for (i = 0; i < model->num_objects; i++) {
222     double f = 
223       model->objects[i].force.x * model->objects[i].force.x +
224       model->objects[i].force.y * model->objects[i].force.y;
225
226     if (f > 100000000)
227       abort();
228   }
229 }
230
231 static void
232 model_integrate (Model *model, double step)
233 {
234   double x, y;
235   Object *o;
236   int i;
237
238   for (i = 0; i < model->num_objects; i++) {
239     o = &model->objects[i];
240     x = o->position.x;
241     y = o->position.y;
242     
243     o->position.x =
244       x + (x - o->previous_position.x) + o->force.x * step * step;
245     o->position.y =
246       y + (y - o->previous_position.y) + o->force.y * step * step;
247
248     o->previous_position.x = x;
249     o->previous_position.y = y;
250   }
251 }
252
253 /* The square root in the distance computation for the string and
254  * stick constraints can be aproximated using Newton:
255  *
256  *    distance = 
257  *      (model->sticks[i].length +
258  *       (dx * dx + dy * dy) / model->sticks[i].length) / 2;
259  *
260  * This works really well, since the constraints aren't typically
261  * violated much.  Thus, the distance is really close to the stick
262  * length, which then makes a good initial guess.  However, the
263  * approximation seems to be slower that just calling sqrt()...
264  */
265
266 static inline double
267 estimate_distance (double dx, double dy, double r)
268 {
269 #ifdef APPROXIMATE_SQUARE_ROOTS
270   return (r + (dx * dx + dy * dy) / r) / 2;
271 #else
272   return sqrt (dx * dx + dy * dy);
273 #endif
274 }
275
276 static int
277 polygon_contains_point (Polygon *polygon, Point *point)
278 {
279   int i;
280   double dx, dy;
281
282   for (i = 0; i < polygon->num_points; i++) {
283     dx = point->x - polygon->points[i].x;
284     dy = point->y - polygon->points[i].y;
285
286     if (polygon->normals[i].x * dx + polygon->normals[i].y * dy >= 0)
287       return polygon->enclosing;
288   }
289
290   return !polygon->enclosing;
291 }
292
293 static void
294 polygon_reflect_object (Polygon *polygon, Object *object)
295 {
296   int i, edge;
297   double d, distance;
298   Vector *n;
299
300   distance = -1000;
301   for (i = 0; i < polygon->num_points; i++) {
302     d = polygon->normals[i].x * (object->position.x - polygon->points[i].x) +
303       polygon->normals[i].y * (object->position.y - polygon->points[i].y);
304
305     if (d > distance) {
306       distance = d;
307       edge = i;
308       n = &polygon->normals[i];
309     }
310   }
311
312   object->position.x -= (1 + elasticity) * distance * n->x;
313   object->position.y -= (1 + elasticity) * distance * n->y;
314
315   distance =
316     n->x * (object->previous_position.x - polygon->points[edge].x) +
317     n->y * (object->previous_position.y - polygon->points[edge].y);
318
319   object->previous_position.x -= (1 + elasticity) * distance * n->x;
320   object->previous_position.y -= (1 + elasticity) * distance * n->y;
321 }
322
323 static void
324 model_constrain_polygon (Model *model, Polygon *polygon)
325 {
326   int i;
327
328   for (i = 0; i < model->num_objects; i++) {
329     if (polygon_contains_point (polygon, &model->objects[i].position))
330       polygon_reflect_object (polygon, &model->objects[i]);
331   }
332 }
333
334 static void
335 model_constrain_anchor (Model *model, Anchor *anchor)
336 {
337   anchor->object->position.x = anchor->x;
338   anchor->object->position.y = anchor->y;
339   anchor->object->previous_position.x = anchor->x;
340   anchor->object->previous_position.y = anchor->y;
341 }
342
343 static void
344 model_constrain_offset (Model *model, Offset *offset)
345 {
346   double x, y;
347   int i;
348
349   x = 0;
350   y = 0;
351   for (i = 0; i < offset->num_objects; i++) {
352     x += offset->objects[i]->position.x;
353     y += offset->objects[i]->position.y;
354   }
355
356   x = x / offset->num_objects - offset->dx * (offset->num_objects - 1) / 2;
357   y = y / offset->num_objects - offset->dy * (offset->num_objects - 1) / 2;
358     
359   for (i = 0; i < offset->num_objects; i++) {
360     offset->objects[i]->position.x = x + offset->dx * i;
361     offset->objects[i]->position.y = y + offset->dy * i;
362   }
363 }
364
365 static void
366 model_constrain (Model *model)
367 {
368   double dx, dy, x, y, distance, fraction;
369   int i;
370
371   if (model->mouse_anchor.object != NULL)
372     model_constrain_anchor (model, &model->mouse_anchor);
373   for (i = 0; i < model->num_anchors; i++)
374     model_constrain_anchor (model, &model->anchors[i]);
375
376   /* String constraints. */
377   for (i = 0; i < model->num_strings; i++) {
378     x = model->strings[i].a->position.x;
379     y = model->strings[i].a->position.y;
380     dx = model->strings[i].b->position.x - x;
381     dy = model->strings[i].b->position.y - y;
382     distance = estimate_distance (dx, dy, model->strings[i].length);
383     if (distance < model->strings[i].length)
384       continue;
385     fraction = (distance - model->strings[i].length) / distance / 2;
386     model->strings[i].a->position.x = x + dx * fraction;
387     model->strings[i].a->position.y = y + dy * fraction;
388     model->strings[i].b->position.x = x + dx * (1 - fraction);
389     model->strings[i].b->position.y = y + dy * (1 - fraction);
390   }
391
392   /* Spacer constraints. */
393   for (i = 0; i < model->num_spacers; i++) {
394     x = model->spacers[i].a->position.x;
395     y = model->spacers[i].a->position.y;
396     dx = model->spacers[i].b->position.x - x;
397     dy = model->spacers[i].b->position.y - y;
398     distance = estimate_distance (dx, dy, model->spacers[i].length);
399     if (distance > model->spacers[i].length)
400       continue;
401     fraction = (distance - model->spacers[i].length) / distance / 2;
402     model->spacers[i].a->position.x = x + dx * fraction;
403     model->spacers[i].a->position.y = y + dy * fraction;
404     model->spacers[i].b->position.x = x + dx * (1 - fraction);
405     model->spacers[i].b->position.y = y + dy * (1 - fraction);
406   }
407
408   /* Stick constraints. */
409   for (i = 0; i < model->num_sticks; i++) {
410     x = model->sticks[i].a->position.x;
411     y = model->sticks[i].a->position.y;
412     dx = model->sticks[i].b->position.x - x;
413     dy = model->sticks[i].b->position.y - y;
414     distance = estimate_distance (dx, dy, model->sticks[i].length);
415     fraction = (distance - model->sticks[i].length) / distance / 2;
416     model->sticks[i].a->position.x = x + dx * fraction;
417     model->sticks[i].a->position.y = y + dy * fraction;
418     model->sticks[i].b->position.x = x + dx * (1 - fraction);
419     model->sticks[i].b->position.y = y + dy * (1 - fraction);
420   }
421
422   /* Offset constraints. */
423   for (i = 0; i < model->num_offsets; i++)
424     model_constrain_offset (model, &model->offsets[i]);
425
426   /* Polygon constraints. */
427   for (i = 0; i < model->num_polygons; i++)
428     model_constrain_polygon (model, &model->polygons[i]);
429 }
430
431 void
432 model_step (Model *model, double delta_t)
433 {
434   int i;
435
436   model_accumulate_forces (model);
437   model_integrate (model, delta_t);
438   for (i = 0; i < 20; i++)
439     model_constrain (model);
440
441   model->theta += delta_t;
442 }
443
444 static double
445 object_distance (Object *object, double x, double y)
446 {
447   double dx, dy;
448
449   dx = object->position.x - x;
450   dy = object->position.y - y;
451
452   return sqrt (dx*dx + dy*dy);
453 }
454
455 Object *
456 model_find_nearest (Model *model, double x, double y)
457 {
458   Object *object;
459   double distance, min_distance;
460   int i;
461
462   for (i = 0; i < model->num_objects; i++) {
463     distance = object_distance (&model->objects[i], x, y);
464     if (i == 0 || distance < min_distance) {
465       min_distance = distance;
466       object = &model->objects[i];
467     }
468   }
469
470   return object;
471 }