Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
An iterative point to cubic Bézier curve distance solver.
#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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment