4 files changed, 681 insertions, 1 deletions

diff --git a/src/Makefile.am b/src/Makefile.am
index b26574c..5011c1f 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -26,7 +26,9 @@ libutouch_grail_la_SOURCES = \
         grail-event.c \
         grail-impl.h \
         grail-api.c \
-        grail-legacy.c
+        grail-legacy.c \
+        grail-init.c \
+        grail-frame.c
 
 # These are flags required to properly set up
 # the build. -fvisibility requires GCC > 4 (or clang)
diff --git a/src/grail-frame.c b/src/grail-frame.c
new file mode 100644
index 0000000..cf38452
--- /dev/null
+++ b/src/grail-frame.c
@@ -0,0 +1,399 @@
+/*****************************************************************************
+ *
+ * grail - Gesture Recognition And Instantiation Library
+ *
+ * Copyright (C) 2010 Canonical Ltd.
+ * Copyright (C) 2010 Henrik Rydberg <rydberg@bitmath.org>
+ *
+ * This program is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+#include "grail-impl.h"
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+static void set_center_velocity_and_radius(struct grail_impl *impl,
+                                           struct grail_element *slot)
+{
+        const struct utouch_contact **tc = slot->touches;
+        double x, y, vx, vy, r2, dx, dy;
+        int i;
+
+        switch (slot->num_touches) {
+        case 1:
+                x = tc[0]->x;
+                y = tc[0]->y;
+                vx = tc[0]->vx;
+                vy = tc[0]->vy;
+                r2 = 0;
+                break;
+        case 2:
+                dx = 0.5 * (tc[1]->x - tc[0]->x);
+                dy = 0.5 * (tc[1]->y - tc[0]->y);
+                x = tc[0]->x + dx;
+                y = tc[0]->y + dy;
+                vx = 0.5 * (tc[0]->vx + tc[1]->vx);
+                vy = 0.5 * (tc[0]->vy + tc[1]->vy);
+                r2 = dx * dx + dy * dy;
+                break;
+        default:
+                x = y = vx = vy = r2 = 0;
+                for (i = 0; i < slot->num_touches; i++) {
+                        x += tc[i]->x;
+                        y += tc[i]->y;
+                        vx += tc[i]->vx;
+                        vy += tc[i]->vy;
+                }
+                x /= slot->num_touches;
+                y /= slot->num_touches;
+                vx /= slot->num_touches;
+                vy /= slot->num_touches;
+                for (i = 0; i < slot->num_touches; i++) {
+                        dx = tc[i]->x - x;
+                        dy = tc[i]->y - y;
+                        r2 += dx * dx + dy * dy;
+                }
+                r2 /= slot->num_touches;
+                break;
+        }
+
+        slot->center.x = x;
+        slot->center.y = y;
+        slot->velocity.x = 1000 * vx;
+        slot->velocity.y = 1000 * vy;
+        slot->radius2 = r2;
+}
+
+static void set_moveness_pivot_and_drag(struct grail_impl *impl,
+                                        struct grail_element *slot,
+                                        double ds, double dc)
+{
+        const struct grail_control *ctl = impl->ctl;
+        const struct grail_element *pslot = slot->prev;
+        double mx = slot->center.x - pslot->center.x;
+        double my = slot->center.y - pslot->center.y;
+        float *T = slot->transform;
+
+        slot->moveness = 1;
+        slot->pivot = pslot->center;
+
+        if (slot->num_touches > 1) {
+                double wx = (1 - dc) * mx + ds * my;
+                double wy = (1 - dc) * my - ds * mx;
+                double w2 = wx * wx + wy * wy;
+                if (w2 > 0) {
+                        double q = (mx * mx + my * my) / w2;
+                        double s = ctl->pivot_unbound ?
+                                q : sqrt(pslot->radius2 / w2);
+                        if (s < q) {
+                                slot->moveness = 1 - s / q;
+                                slot->pivot.x += s * wx;
+                                slot->pivot.y += s * wy;
+                        } else {
+                                slot->moveness = 0;
+                                slot->pivot.x += q * wx;
+                                slot->pivot.y += q * wy;
+                        }
+                }
+        }
+
+        mx *= slot->moveness;
+        my *= slot->moveness;
+
+        T[0] = dc;
+        T[1] = ds;
+        T[2] = (1 - dc) * slot->pivot.x - ds * slot->pivot.y + mx;
+        T[3] = -ds;
+        T[4] = dc;
+        T[5] = (1 - dc) * slot->pivot.y + ds * slot->pivot.x + my;
+
+        slot->drag.x = pslot->drag.x + mx;
+        slot->drag.y = pslot->drag.y + my;
+}
+
+static void start_slot(struct grail_impl *impl,
+                       struct grail_element *slot,
+                       const struct utouch_frame *touch)
+{
+        float *T = slot->transform;
+
+        slot->id = impl->seqid++ & GRAIL_ID_MAX;
+        slot->expect_mask = GRAIL_EXPECT_MASK;
+        slot->active_mask = 0;
+        slot->start_time = touch->time;
+        set_center_velocity_and_radius(impl, slot);
+        slot->start_center = slot->center;
+        T[0] = T[4] = 1;
+        T[1] = T[2] = T[3] = T[5] = 0;
+        slot->pivot = slot->center;
+        slot->drag.x = 0;
+        slot->drag.y = 0;
+        slot->scale2 = 1;
+        slot->angle = 0;
+}
+
+static void update_slot(struct grail_impl *impl,
+                        struct grail_element *slot,
+                        double ds, double dc)
+{
+        const struct grail_element *pslot = slot->prev;
+
+        slot->id = pslot->id;
+        slot->start_time = pslot->start_time;
+        slot->start_center = pslot->start_center;
+        slot->expect_mask = pslot->expect_mask;
+        slot->active_mask = pslot->active_mask;
+
+        set_center_velocity_and_radius(impl, slot);
+        set_moveness_pivot_and_drag(impl, slot, ds, dc);
+
+        slot->scale2 = pslot->scale2 * (ds * ds + dc * dc);
+        slot->angle = pslot->angle + ds / dc; /* atan2(ds, dc) */
+}
+
+static void stop_slot(struct grail_impl *impl,
+                      struct grail_element *slot)
+{
+        const struct grail_element *pslot = slot->prev;
+        float *T = slot->transform;
+
+        slot->id = -1;
+        slot->num_touches = 0;
+        slot->start_time = pslot->start_time;
+        slot->start_center = pslot->start_center;
+        slot->expect_mask = 0;
+        slot->active_mask = pslot->active_mask;
+        slot->center = pslot->center;
+        slot->velocity = pslot->velocity;
+        slot->radius2 = pslot->radius2;
+        T[0] = T[4] = 1;
+        T[1] = T[2] = T[3] = T[5] = 0;
+        slot->moveness = 1;
+        slot->pivot = pslot->pivot;
+        slot->drag = pslot->drag;
+        slot->scale2 = pslot->scale2;
+        slot->angle = pslot->angle;
+}
+
+static void set_slot_one(struct grail_impl *impl,
+                         struct grail_element *slot,
+                         const struct utouch_frame *touch,
+                         const struct utouch_contact *t1)
+{
+        const struct grail_element *pslot = slot->prev;
+        const struct utouch_contact *p1 = pslot->touches[0];
+
+        if (!t1->active) {
+                stop_slot(impl, slot);
+                return;
+        }
+
+        slot->touches[0] = t1;
+        slot->num_touches = 1;
+
+        if (pslot->num_touches != slot->num_touches || t1->id != p1->id) {
+                start_slot(impl, slot, touch);
+                return;
+        }
+
+        update_slot(impl, slot, 0, 1);
+}
+
+static void set_slot_two(struct grail_impl *impl,
+                         struct grail_element *slot,
+                         const struct utouch_frame *touch,
+                         const struct utouch_contact *t1,
+                         const struct utouch_contact *t2)
+{
+        const struct grail_element *pslot = slot->prev;
+        const struct utouch_contact *p1 = pslot->touches[0];
+        const struct utouch_contact *p2 = pslot->touches[1];
+        double tx, ty, px, py, d2;
+
+        if (!t1->active || !t2->active) {
+                stop_slot(impl, slot);
+                return;
+        }
+
+        slot->touches[0] = t1;
+        slot->touches[1] = t2;
+        slot->num_touches = 2;
+
+        if (pslot->num_touches != slot->num_touches ||
+            t1->id != p1->id || t2->id != p2->id) {
+                start_slot(impl, slot, touch);
+                return;
+        }
+
+        tx = t2->x - t1->x;
+        ty = t2->y - t1->y;
+        px = p2->x - p1->x;
+        py = p2->y - p1->y;
+
+        d2 = px * px + py * py;
+        if (d2 > 0) {
+                px /= d2;
+                py /= d2;
+        }
+
+        update_slot(impl, slot, tx * py - ty * px, tx * px + ty * py);
+}
+
+static void set_slot_multi(struct grail_impl *impl,
+                           struct grail_element *slot,
+                           struct grail_frame *frame,
+                           const struct utouch_frame *touch)
+{
+        const struct grail_element *pslot = slot->prev;
+        struct grail_element **slots = frame->slots;
+        int i, j, n = impl->num_touches;
+        struct grail_element *best = 0;
+
+        if (touch->num_active < 3) {
+                stop_slot(impl, slot);
+                return;
+        }
+
+        memcpy(slot->touches, touch->active,
+               touch->num_active * sizeof(slot->touches[0]));
+        slot->num_touches = touch->num_active;
+
+        if (pslot->num_touches != slot->num_touches) {
+                start_slot(impl, slot, touch);
+                return;
+        }
+
+        for (i = 0; i < slot->num_touches; i++) {
+                if (slot->touches[i]->id != pslot->touches[i]->id) {
+                        start_slot(impl, slot, touch);
+                        return;
+                }
+        }
+
+        for (i = 0; i < impl->num_touches; i++) {
+                for (j = i + 1; j < impl->num_touches; j++) {
+                        struct grail_element *s = slots[n++];
+                        if (!s->num_touches)
+                                continue;
+                        if (!best || s->radius2 > best->radius2)
+                                best = s;
+                }
+        }
+
+        update_slot(impl, slot, best->transform[1], best->transform[0]);
+}
+
+static void set_slots(struct grail_impl *impl,
+                      struct grail_frame *frame,
+                      const struct utouch_frame *touch)
+{
+        struct grail_element **slots = frame->slots;
+        struct utouch_contact *const *tc = touch->slots;
+        int i, j, n = 0;
+
+        for (i = 0; i < impl->num_touches; i++)
+                set_slot_one(impl, slots[n++], touch, tc[i]);
+
+        for (i = 0; i < impl->num_touches; i++)
+                for (j = i + 1; j < impl->num_touches; j++)
+                        set_slot_two(impl, slots[n++], touch, tc[i], tc[j]);
+
+        set_slot_multi(impl, slots[n++], frame, touch);
+}
+
+static void collect_transforms(struct grail_impl *impl,
+                               struct grail_frame *frame,
+                               const struct utouch_frame *touch)
+{
+        const struct utouch_surface *s = utouch_frame_get_surface(impl->fh);
+        const struct grail_control *ctl = impl->ctl;
+        float c_x = ctl->bar_center_x * (s->mapped_max_x - s->mapped_min_x);
+        float c_y = ctl->bar_center_y * (s->mapped_max_y - s->mapped_min_y);
+        float d_x = ctl->bar_drag_x * (s->mapped_max_x - s->mapped_min_x);
+        float d_y = ctl->bar_drag_y * (s->mapped_max_y - s->mapped_min_y);
+        float ds2 = ctl->bar_scale * ctl->bar_scale;
+        float dt;
+        int i;
+
+        for (i = 0; i < impl->num_slots; i++) {
+                struct grail_element *s = frame->slots[i];
+
+                if (!s->num_touches)
+                        continue;
+
+                dt = touch->time - s->start_time;
+                if (dt > ctl->glue_ms) {
+                        unsigned int mask = s->active_mask;
+
+                        if (fabs(s->center.x - s->start_center.x) > c_x)
+                                mask |= GRAIL_EXPECT_CENTER_X;
+                        if (fabs(s->center.y - s->start_center.y) > c_y)
+                                mask |= GRAIL_EXPECT_CENTER_Y;
+                        if (fabs(s->drag.x) > d_x)
+                                mask |= GRAIL_EXPECT_DRAG_X;
+                        if (fabs(s->drag.y) > d_y)
+                                mask |= GRAIL_EXPECT_DRAG_Y;
+                        if (fabs(s->scale2 - 1) > ds2)
+                                mask |= GRAIL_EXPECT_SCALE;
+                        if (fabs(s->angle) > ctl->bar_angle)
+                                mask |= GRAIL_EXPECT_ANGLE;
+
+                        s->active_mask = mask;
+
+                        if (dt < ctl->drop_x_ms)
+                                mask |= GRAIL_EXPECT_CENTER_X;
+                        if (dt < ctl->drop_y_ms)
+                                mask |= GRAIL_EXPECT_CENTER_Y;
+                        if (dt < ctl->drop_x_ms)
+                                mask |= GRAIL_EXPECT_DRAG_X;
+                        if (dt < ctl->drop_y_ms)
+                                mask |= GRAIL_EXPECT_DRAG_Y;
+                        if (dt < ctl->drop_scale_ms)
+                                mask |= GRAIL_EXPECT_SCALE;
+                        if (dt < ctl->drop_angle_ms)
+                                mask |= GRAIL_EXPECT_ANGLE;
+
+                        s->expect_mask &= mask;
+                }
+
+                frame->ongoing[frame->num_ongoing++] = s;
+        }
+}
+
+const struct grail_frame GRAIL_PUBLIC *
+grail_pump_frame(grail_handle ge, const struct utouch_frame *touch)
+{
+        struct grail_impl *impl = ge->impl;
+        struct grail_frame *frame = impl->frames[impl->nextframe];
+        const struct grail_frame *prev = frame->prev;
+        int i;
+
+        if (touch->slot_revision == touch->prev->slot_revision &&
+            !prev->num_ongoing)
+                return 0;
+
+        frame->touch = touch;
+        frame->num_ongoing = 0;
+        for (i = 0; i < impl->num_slots; i++)
+                frame->slots[i]->prev = prev->slots[i];
+
+        set_slots(impl, frame, touch);
+        collect_transforms(impl, frame, touch);
+
+        impl->nextframe = (impl->nextframe + 1) % impl->num_frames;
+
+        return frame;
+}
diff --git a/src/grail-impl.h b/src/grail-impl.h
index 8f73a2d..e9b1038 100644
--- a/src/grail-impl.h
+++ b/src/grail-impl.h
@@ -33,6 +33,32 @@
 
 #define DIM_TOUCH               32
 #define DIM_TOUCH_BYTES         ((DIM_TOUCH + 7) >> 3)
+#define GRAIL_ID_MAX            0xffff
+
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#define MAX(a, b) ((a) > (b) ? (a) : (b))
+
+typedef void *grail_select_callback;
+
+/*
+ * In this implementation, there can be N one-gestures, N (N - 1) / 2
+ * two-gestures, and one global gesture.
+ */
+static inline int get_slot_count(int n)
+{
+        return n + n * (n - 1) / 2 + 1;
+}
+
+int create_grail2(struct grail_impl *x,
+                  utouch_frame_handle fh,
+                  unsigned int num_frames,
+                  void *select,
+                  unsigned int version,
+                  unsigned int control_size,
+                  unsigned int frame_size,
+                  unsigned int slot_size);
+
+void destroy_grail2(struct grail_impl *x);
 
 struct grail_impl {
         struct evemu_device *evemu;
@@ -45,6 +71,18 @@ struct grail_impl {
         int ongoing;
         int gesture;
         FILE *fptest;
+        /* new stuff below */
+        struct grail_control *ctl;
+        grail_select_callback select;
+        int num_frames;
+        int num_slots;
+        int num_touches;
+        int nextframe;
+        int seqid;
+        unsigned int control_size;
+        unsigned int frame_size;
+        unsigned int slot_size;
+        struct grail_frame **frames;
 };
 
 #endif
diff --git a/src/grail-init.c b/src/grail-init.c
new file mode 100644
index 0000000..18e6996
--- /dev/null
+++ b/src/grail-init.c
@@ -0,0 +1,241 @@
+/*****************************************************************************
+ *
+ * grail - Gesture Recognition And Instantiation Library
+ *
+ * Copyright (C) 2010 Canonical Ltd.
+ * Copyright (C) 2010 Henrik Rydberg <rydberg@bitmath.org>
+ *
+ * This program is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+#include "grail-impl.h"
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+static struct grail_control *create_control(int size)
+{
+        struct grail_control *c = calloc(1, size);
+
+        if (!c)
+                return 0;
+
+        c->glue_ms = 60;
+        c->bar_center_x = 0.03;
+        c->bar_center_y = 0.03;
+        c->bar_drag_x = 0.03;
+        c->bar_drag_y = 0.03;
+        c->bar_scale = 0.3;
+        c->bar_angle = 0.1;
+        c->drop_x_ms = 300;
+        c->drop_y_ms = 300;
+        c->drop_scale_ms = 300;
+        c->drop_angle_ms = 300;
+
+        return c;
+}
+
+static void destroy_slots(struct grail_element **slots, int nslot)
+{
+        int i;
+
+        if (slots) {
+                for (i = nslot - 1; i >= 0; i--)
+                        free(slots[i]);
+                free(slots);
+        }
+}
+
+static void destroy_frame(struct grail_frame *frame, int nslot)
+{
+        if (frame) {
+                destroy_slots(frame->slots, nslot);
+                free(frame->ongoing);
+                free(frame);
+        }
+}
+
+static void destroy_frames(struct grail_frame **frames, int nframe, int nslot)
+{
+        int i;
+
+        if (frames) {
+                for (i = nframe - 1; i >= 0; i--)
+                        destroy_frame(frames[i], nslot);
+                free(frames);
+        }
+}
+
+static struct grail_element **create_slots(int nslot, int ntouch, int size)
+{
+        struct grail_element **slots;
+        struct grail_element *s;
+        int i;
+
+        slots = calloc(nslot, sizeof(slots[0]));
+        if (!slots)
+                return 0;
+
+        for (i = 0; i < nslot; i++) {
+                s = calloc(1, size + ntouch * sizeof(void *));
+                if (!s)
+                        goto out;
+                s->slot = i;
+                s->id = -1;
+                s->touches = (void *)((char *)s + size);
+                slots[i] = s;
+        }
+
+        return slots;
+ out:
+        destroy_slots(slots, nslot);
+        return 0;
+}
+
+static struct grail_frame *create_frame(int nslot, int ntouch,
+                                        int frame_size, int slot_size)
+{
+        struct grail_frame *frame;
+        int i;
+
+        frame = calloc(1, frame_size);
+        if (!frame)
+                return 0;
+
+        frame->ongoing = calloc(nslot, sizeof(frame->ongoing[0]));
+        frame->slots = create_slots(nslot, ntouch, slot_size);
+        if (!frame->ongoing || !frame->slots)
+                goto out;
+
+        return frame;
+ out:
+        destroy_frame(frame, nslot);
+        return 0;
+}
+
+static struct grail_frame **create_frames(int nframe, int nslot, int ntouch,
+                                          int frame_size, int slot_size)
+{
+        struct grail_frame **frames;
+        struct grail_frame *f;
+        int i;
+
+        frames = calloc(nframe, sizeof(frames[0]));
+        if (!frames)
+                return 0;
+
+        for (i = 0; i < nframe; i++) {
+                f = create_frame(nslot, ntouch, frame_size, slot_size);
+                if (!f)
+                        goto out;
+                frames[i] = f;
+        }
+
+        return frames;
+ out:
+        destroy_frames(frames, nframe, nslot);
+        return 0;
+}
+
+int create_grail2(struct grail_impl *x,
+                  utouch_frame_handle fh,
+                  unsigned int num_frames,
+                  grail_select_callback select,
+                  unsigned int version,
+                  unsigned int control_size,
+                  unsigned int frame_size,
+                  unsigned int slot_size)
+{
+        struct utouch_surface *s = utouch_frame_get_surface(fh);
+        unsigned int ntouch = utouch_frame_get_num_slots(fh);
+        unsigned int nslot = get_slot_count(ntouch);
+        int i, j;
+
+        x->select = select;
+        x->control_size = MAX(control_size, sizeof(struct grail_control));
+        x->frame_size = MAX(frame_size, sizeof(struct grail_frame));
+        x->slot_size = MAX(slot_size, sizeof(struct grail_element));
+
+        x->num_frames = num_frames;
+        x->num_slots = nslot;
+        x->num_touches = ntouch;
+
+        x->ctl = create_control(x->control_size);
+        if (!x->ctl)
+                goto freemem;
+
+        x->frames = create_frames(num_frames, nslot, ntouch,
+                                  x->frame_size, x->slot_size);
+        if (!x->frames)
+                goto freemem;
+
+        for (i = 0; i < num_frames; i++)
+                x->frames[(i + 1) % num_frames]->prev = x->frames[i];
+
+        return 0;
+
+ freemem:
+        destroy_grail2(x);
+        return -ENOMEM;
+}
+
+void destroy_grail2(struct grail_impl *x)
+{
+        destroy_frames(x->frames, x->num_frames, x->num_slots);
+        free(x->ctl);
+}
+
+grail_handle GRAIL_PUBLIC grail_new_raw(utouch_frame_handle fh,
+                                        unsigned int num_frames,
+                                        grail_select_callback select,
+                                        unsigned int version,
+                                        unsigned int control_size,
+                                        unsigned int frame_size,
+                                        unsigned int slot_size)
+{
+        struct grail *ge;
+        struct grail_impl *x;
+
+        ge = calloc(1, sizeof(*ge));
+        if (!ge)
+                return 0;
+        x = calloc(1, sizeof(*x));
+        if (!x)
+                goto out;
+        x->fh = fh;
+        if (create_grail2(x, fh, num_frames, select,
+                          version, control_size, frame_size, slot_size))
+                goto out;
+
+        ge->impl = x;
+        return ge;
+
+ out:
+        free(x);
+        free(ge);
+        return 0;
+}
+
+void GRAIL_PUBLIC grail_delete(grail_handle ge)
+{
+        destroy_grail2(ge->impl);
+        free(ge->impl);
+        free(ge);
+}
+
+struct grail_control GRAIL_PUBLIC *grail_get_control(grail_handle ge)
+{
+        return ge->impl->ctl;
+}