Skip to content

Instantly share code, notes, and snippets.

@guozhou

guozhou/distance.cpp

Created December 24, 2015 08:43
Show Gist options
  • Save guozhou/9f050d0f685eee0ded46 to your computer and use it in GitHub Desktop.
Save guozhou/9f050d0f685eee0ded46 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. guozhou created this gist Dec 24, 2015.
    394 changes: 394 additions & 0 deletions distance.cpp
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,394 @@
    #include <iostream>
    #include <cstdlib>
    #include <cassert>
    #include <vector>

    #include <wstp.h>
    #include <glm/glm.hpp>
    #include <glm/gtc/type_ptr.hpp>

    #define DEQUE_SIZE 7
    #define MAX_ITERATION 64
    #define EPSILON 1e-4

    static void error(WSLINK lp)
    {
    fprintf(stderr, "WSTP Error: %s.\n",
    WSErrorMessage(lp));
    exit(EXIT_FAILURE);
    }

    // discard resulting packages until RETURNPKT
    static void discard_pkt(WSLINK lp, bool keep_retpkt = false)
    {
    int pkt;
    while ((pkt = WSNextPacket(lp), pkt) && pkt != RETURNPKT)
    {
    WSNewPacket(lp);
    if (WSError(lp))
    error(lp);
    }
    if (!keep_retpkt)
    {
    WSNewPacket(lp);
    }
    }

    // plot n order bezier curve with n+1 control points in gj
    static void show_bezier_curve(WSLINK lp, glm::vec2 gj[], int n = 3)
    {
    //printf("gj = {{0.2, -0.1}, {-0.2, 0.4}, {0.2, 0.6}, {0.5, -0.1}};\n");
    printf("gj = {");
    for (int idx = 0; idx < n; idx++)
    {
    printf("{%2.9f, %2.9f}, ", gj[idx].x, gj[idx].y);
    }
    printf("{%2.9f, %2.9f}};\n", gj[n].x, gj[n].y);

    WSPutFunction(lp, "EvaluatePacket", 1L);
    WSPutFunction(lp, "Set", 2L);
    WSPutSymbol(lp, "gj");
    WSPutFunction(lp, "List", n + 1);
    for (int idx = 0; idx < n + 1; idx++)
    {
    WSPutReal32List(lp, glm::value_ptr(gj[idx]), 2L);
    }
    //WSPutString(lp, "{{0.2, -0.1}, {-0.2, 0.4}, {0.2, 0.6}, {0.5, -0.1}}");
    WSEndPacket(lp);
    discard_pkt(lp);

    printf("Show[Graphics[{Brown, BezierCurve[gj], Red, Point[gj], Green, Line[gj]}], Axes->True, AxesOrigin -> {0, 0}]\n");
    WSPutFunction(lp, "EvaluatePacket", 1L);
    WSPutFunction(lp, "ToExpression", 1L);
    WSPutString(lp, "Show[Graphics[{Brown, BezierCurve[gj], Red, Point[gj], Green, Line[gj]}], Axes->True, AxesOrigin -> {0, 0}]");
    WSEndPacket(lp);
    discard_pkt(lp);
    }

    // coefficients of distance constraint function for a given cubic bezier
    // it is explicit -- just a quintic Bernstein polynomial
    static void bezier5_hk(const glm::vec2 gj[], glm::vec2 thk[])
    {
    // numerator of coefficients of product of a cubic bezier function
    // and its 1st order derivative, which is a quadratic bezier function
    glm::vec2 g0g0 = gj[0] * gj[0];
    glm::vec2 g0g1 = gj[0] * gj[1];
    glm::vec2 g0g2 = gj[0] * gj[2];
    glm::vec2 g0g3 = gj[0] * gj[3];

    glm::vec2 g1g1 = gj[1] * gj[1];
    glm::vec2 g1g2 = gj[1] * gj[2];
    glm::vec2 g1g3 = gj[1] * gj[3];

    glm::vec2 g2g2 = gj[2] * gj[2];
    glm::vec2 g2g3 = gj[2] * gj[3];

    glm::vec2 g3g3 = gj[3] * gj[3];

    // only numerator p is left since denominator of hk is canceled
    // by the binomial coefficient of Bernstein polynomial
    glm::vec2 hk_p[6];
    hk_p[0] = -3.f*g0g0 + 3.f*g0g1;
    hk_p[1] = -15.f*g0g1 + 6.f*g0g2 + 9.f*g1g1;
    hk_p[2] = -12.f*g0g2 + 3.f*g0g3 - 18.f*g1g1 + 27.f*g1g2;
    hk_p[3] = -3.f*g0g3 - 27.f*g1g2 + 12.f*g1g3 + 18.f*g2g2;
    hk_p[4] = -6.f*g1g3 - 9.f*g2g2 + 15.f*g2g3;
    hk_p[5] = -3.f*g2g3 + 3.f*g3g3;

    // apply the denominator of hk
    // 1 / {Binomial[5, 0], Binomial[5, 1], Binomial[5, 2], Binomial[5, 3], Binomial[5, 4], Binomial[5, 5]}
    float binomial_coeff_inv[] = { 1.f / 1.f, 1.f / 5.f, 1.f / 10.f, 1.f / 10.f, 1.f / 5.f, 1.f / 1.f };
    for (int idx = 0; idx < 6; idx++)
    {
    thk[idx].y = (hk_p[idx].x + hk_p[idx].y)*binomial_coeff_inv[idx];
    }
    }

    // t coordinates of control points of an n order explicit bezier function
    static void bezier_tk(glm::vec2 thk[], int n)
    {
    float offset = 1.f / n;
    thk[0].x = 0.f;
    for (int idx = 1; idx < n; idx++)
    {
    thk[idx].x = idx * offset;
    }
    thk[n].x = 1.f;
    }

    // distance from the origin to a cubic bezier curve g[t]
    static float bezier3_dist(WSLINK lp, const glm::vec2 gj[], float t)
    {
    printf("BezierFunction[gj][%2.9f];\n", t);

    WSPutFunction(lp, "EvaluatePacket", 1L);
    WSPutFunction(lp, "Set", 2L);
    WSPutSymbol(lp, "gj");
    WSPutFunction(lp, "List", 3 + 1);
    for (int idx = 0; idx < 3 + 1; idx++)
    {
    WSPutReal32List(lp, glm::value_ptr(gj[idx]), 2L);
    }
    WSEndPacket(lp);
    discard_pkt(lp);

    WSPutFunction(lp, "EvaluatePacket", 1L);
    WSPutFunction(lp, "ToExpression", 1L);
    WSPutString(lp, "g = BezierFunction[gj]");
    WSEndPacket(lp);
    discard_pkt(lp);

    WSPutFunction(lp, "EvaluatePacket", 1L);
    WSPutFunction(lp, "g", 1L);
    WSPutReal32(lp, t);
    WSEndPacket(lp);
    discard_pkt(lp, true);

    int len;
    if (!WSTestHead(lp, "List", &len) || len != 2)
    error(lp);
    glm::vec2 gt;
    WSGetReal32(lp, &(glm::value_ptr(gt)[0]));
    WSGetReal32(lp, &(glm::value_ptr(gt)[1]));
    printf("{%2.9f, %2.9f}\n", gt.x, gt.y);

    return glm::length(gt);
    }

    // step forward: pop bottom, push top
    static inline int deque_step1(int p) { return int(glm::mod(float(p + 1), float(DEQUE_SIZE))); }
    // step backward: push bottom, pop top
    static inline int deque_step0(int p) { return int(glm::mod(float(p - 1), float(DEQUE_SIZE))); }
    // no. of elements
    static inline int deque_size(int bm, int tp) { return int(glm::mod(float(tp - bm), float(DEQUE_SIZE))) + 1; }

    // signed area of triangle given p2 wrt. p0p1
    static inline float signed_area(const glm::vec2& p0, const glm::vec2& p1, const glm::vec2& p2)
    {
    glm::vec2 e01 = p1 - p0;
    glm::vec2 e02 = p2 - p0;
    return e01.x*e02.y - e02.x*e01.y;
    }

    // plot polyline of n+1 points and its convex hull being recorded in deque
    static void show_convex_hull(WSLINK lp, glm::vec2 thk[], int n, int Dk[], int bm, int tp)
    {
    WSPutFunction(lp, "EvaluatePacket", 1L);
    WSPutFunction(lp, "Set", 2L);
    WSPutSymbol(lp, "thk");
    WSPutFunction(lp, "List", n + 1);
    for (int idx = 0; idx <= n; idx++)
    {
    WSPutReal32List(lp, glm::value_ptr(thk[idx]), 2L);
    }
    WSEndPacket(lp);
    discard_pkt(lp);

    WSPutFunction(lp, "EvaluatePacket", 1L);
    WSPutFunction(lp, "Set", 2L);
    WSPutSymbol(lp, "thDk");
    WSPutFunction(lp, "List", deque_size(bm, tp));
    for (; bm != tp; bm = deque_step1(bm))
    {
    WSPutReal32List(lp, glm::value_ptr(thk[Dk[bm]]), 2L);
    }
    WSPutReal32List(lp, glm::value_ptr(thk[Dk[bm]]), 2L);
    WSEndPacket(lp);
    discard_pkt(lp);

    printf("Show[Graphics[{Red, Point[thk], Green, Line[thk], Blue, Dotted, Line[thDk]}], Axes->True, AxesOrigin -> {0, 0}]\n");
    WSPutFunction(lp, "EvaluatePacket", 1L);
    WSPutFunction(lp, "ToExpression", 1L);
    WSPutString(lp, "Show[Graphics[{Red, Point[thk], Green, Line[thk], Blue, Dotted, Line[thDk]}], Axes->True, AxesOrigin -> {0, 0}]");
    WSEndPacket(lp);
    discard_pkt(lp);
    }

    // distance.exe -linklaunch
    // http://support.wolfram.com/kb/12487
    // http://reference.wolfram.com/language/JLink/tutorial/WritingJavaProgramsThatUseTheWolframLanguage.html
    int main(int argc, char **argv)
    {
    WSENV ep = (WSENV)0;
    WSLINK lp = (WSLINK)0;

    // initializes the WSTP environment object and passes parameters in p
    ep = WSInitialize((WSParametersPointer)0);
    if (ep == (WSENV)0)
    exit(EXIT_FAILURE);

    int err;
    // opens a WSTP connection, taking parameters from command-line arguments
    lp = WSOpenArgcArgv(ep, argc, argv, &err);
    if (lp == (WSLINK)0)
    exit(EXIT_FAILURE);

    printf("<<JavaGraphics`\n");
    WSPutFunction(lp, "EvaluatePacket", 1L);
    WSPutFunction(lp, "ToExpression", 1L);
    WSPutString(lp, "<<JavaGraphics`");
    WSEndPacket(lp);
    discard_pkt(lp);

    //////////////////////////////////////////////////////////////////////////
    // a cubic bezier curve
    glm::vec2 gj[] = { { 0.2f, -0.1f }, { -0.2f, 0.4f }, { 0.2f, 0.6f }, { 0.5f, -0.1f } };
    show_bezier_curve(lp, gj);
    glm::vec2 thk[6];
    bezier_tk(thk, 5);
    bezier5_hk(gj, thk);

    show_bezier_curve(lp, thk, 5);

    int Dk[DEQUE_SIZE] = { 0L };// deque as circular buffer recording current convex hull
    // bottom ^^^^ top
    int order = 6L - 1L;// order of current bezier function
    float tl = 0.f, tr = 1.f;
    float dist;// = std::numeric_limits<float>::infinity();

    dist = bezier3_dist(lp, gj, 0.f);
    if (float dist1 = bezier3_dist(lp, gj, 1.f) < dist)
    {
    tl = 1.f;
    dist = dist1;
    }

    for (int n_iter = 0; n_iter < MAX_ITERATION; n_iter++)
    {
    // find an edge which is not collinear with the first point
    float area_0kk1 = 0.f;
    int k = 1;
    for (; k < order && area_0kk1 == 0.f; k++)
    area_0kk1 = signed_area(thk[0], thk[k], thk[k + 1]);

    if (area_0kk1 == 0.f)
    {
    printf("all control points are collinear\n");
    // there is an intersection with the t-axis
    if (thk[0].y * thk[order].y < 0.f)
    {
    tl = (thk[0].x*thk[order].y - thk[0].y*thk[order].x) / (thk[order].y - thk[0].y);
    dist = bezier3_dist(lp, gj, tl);
    }
    break;
    }

    // initial deque by a triangle as the convex hull
    Dk[0] = k;// bottom
    if (area_0kk1 < 0.f)
    {
    Dk[1] = 0;
    Dk[2] = k - 1;
    }
    else
    {
    Dk[1] = k - 1;
    Dk[2] = 0;
    }
    Dk[3] = k;// top
    int bm = 0, tp = 3;
    int bm1 = deque_step1(bm), tp1 = deque_step0(tp);
    //show_convex_hull(lp, thk, order, Dk, bm, tp);

    // Melkman convex hull algorithm for simple polyline (e.g. control polygon)
    // 0,...,k,(k+1)th points have been used for the initial hull of triangle
    for (k = k + 1; k <= order; k++)
    {
    // kth point is to the left of both Db+1,D_b and Dt,Dt-1
    if (signed_area(thk[Dk[bm1]], thk[Dk[bm]], thk[k]) >= 0.f &&
    signed_area(thk[Dk[tp]], thk[Dk[tp1]], thk[k]) >= 0.f)
    {
    printf("INFO point %d is interior.\n", k);
    continue;
    }
    // tangent to the bottom of hull
    while (signed_area(thk[Dk[bm1]], thk[Dk[bm]], thk[k]) <= 0.f)
    {
    bm = bm1;
    bm1 = deque_step1(bm);
    }
    bm1 = bm;
    bm = deque_step0(bm);
    Dk[bm] = k;
    // tangent to the top of hull
    while (signed_area(thk[Dk[tp]], thk[Dk[tp1]], thk[k]) <= 0.f)
    {
    tp = tp1;
    tp1 = deque_step0(tp);
    }
    tp1 = tp;
    tp = deque_step1(tp);
    Dk[tp] = k;
    }
    show_convex_hull(lp, thk, order, Dk, bm, tp);

    // intersect the resulting hull with the t-axis and find the leftmost intersection
    float t = 1.f;
    for (int idx = bm, idx1; idx != tp; idx = idx1)
    {
    idx1 = deque_step1(idx);
    glm::vec2 thD_idx = thk[Dk[idx]];
    glm::vec2 thD_idx1 = thk[Dk[idx1]];
    if (thD_idx.y * thD_idx1.y <= 0.f)
    {
    t = glm::min(t,
    (thD_idx.y * thD_idx1.x - thD_idx.x * thD_idx1.y) / (thD_idx.y - thD_idx1.y));
    }
    }

    if (t < 1.f)
    {
    printf("convex hull leftmost intersection t = %2.9f\n", t);
    // subdividing by de Casteljau algorithm at t
    tr = tr * (1.f - t);
    for (int o = order; o >= 1; o--)
    {
    for (int idx = 0; idx < o; idx++)
    {
    thk[idx].y = (1.f - t)*thk[idx].y + t*thk[idx + 1].y;
    }
    }
    show_bezier_curve(lp, thk, 5);
    }
    else
    {
    printf("convex hull has no more intersection\n", t);
    break;
    }

    if (t < EPSILON)
    {
    float tlt = 1.f - tr;
    float distt = bezier3_dist(lp, gj, tlt);
    printf("root launch detected t = %2.9f, d = %2.9f\n", tlt, distt);

    // a nearer root
    if (distt < dist)
    {
    tl = tlt;
    dist = distt;
    }

    // polynomial deflation
    for (int idx = 1; idx <= order; idx++)
    {
    thk[idx - 1].y = float(order) / float(idx) * thk[idx].y;
    }
    order -= 1;
    bezier_tk(thk, order);
    printf("polynomial deflation o = %d\n", order);
    show_bezier_curve(lp, thk, order);
    }

    assert(n_iter < MAX_ITERATION);
    }

    printf("projection of origin onto curve t = %2.9f, d = %2.9f\n", tl, dist);

    system("pause");
    WSPutFunction(lp, "Exit", 0);
    WSClose(lp);
    WSDeinitialize(ep);

    return 0;
    }