#include <math.h>
#include <stdbool.h>
+#ifdef CMATH3D_ASSERTS
+#include <assert.h>
+#endif
+
#ifndef M_PI_F
#define M_PI_F (3.14159265358979323846f)
#define M_1_PI_F (0.31830988618379067154f)
static inline struct vec vzero(void) {
return vrepeat(0.0f);
}
+// construct the i'th basis vector, i.e. vbasis(0) == (1, 0, 0).
+static inline struct vec vbasis(int i) {
+ float a[3] = {0.0f, 0.0f, 0.0f};
+ a[i] = 1.0f;
+ return mkvec(a[0], a[1], a[2]);
+}
//
// operators
static inline struct vec vnormalize(struct vec v) {
return vdiv(v, vmag(v));
}
+// clamp the Euclidean norm of a vector if it exceeds given maximum.
+static inline struct vec vclampnorm(struct vec v, float maxnorm) {
+ float const norm = vmag(v);
+ if (norm > maxnorm) {
+ return vscl(maxnorm / norm, v);
+ }
+ return v;
+}
// vector cross product.
static inline struct vec vcross(struct vec a, struct vec b) {
return mkvec(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x);
f[0] = q.x; f[1] = q.y; f[2] = q.z; f[3] = q.w;
}
+
+// ------------------------ convex sets in R^3 ---------------------------
+
+// project v onto the halfspace H = {x : a^T x <= b}, where a is a unit vector.
+// If v lies in H, returns v. Otherwise, returns the closest point, which
+// minimizes |x - v|_2, and will satisfy a^T x = b.
+static inline struct vec vprojecthalfspace(struct vec x, struct vec a_unit, float b) {
+ float ax = vdot(a_unit, x);
+ if (ax <= b) {
+ return x;
+ }
+ return vadd(x, vscl(b - ax, a_unit));
+}
+
+// test if v lies in the convex polytope defined by linear inequalities Ax <= b.
+// A: n x 3 matrix, row-major. Each row must have L2 norm of 1.
+// b: n vector.
+// tolerance: Allow violations up to Ax <= b + tolerance.
+static inline bool vinpolytope(struct vec v, float const A[], float const b[], int n, float tolerance)
+{
+ for (int i = 0; i < n; ++i) {
+ struct vec a = vloadf(A + 3 * i);
+ if (vdot(a, v) > b[i] + tolerance) {
+ return false;
+ }
+ }
+ return true;
+}
+
+// Projects v onto the convex polytope defined by linear inequalities Ax <= b.
+// Returns argmin_{x: Ax <= b} |x - v|_2. Uses Diykstra's (not Dijkstra's!)
+// projection algorithm [1].
+//
+// Args:
+// v: vector to project into the polytope.
+// A: n x 3 matrix, row-major. Each row must have L2 norm of 1.
+// b: n vector.
+// work: n x 3 matrix. will be overwritten. input values are not used.
+// tolerance: Halt once iterates satisfy |x_{k + 1} - x_k|_2 < tolerance.
+// Roughly, but not actually, equivalent to allowing constraint violations
+// up to Ax >= b + tolerance.
+//
+// Returns:
+// The projection of v into the polytope.
+//
+// References:
+// [1] Boyle, J. P., and Dykstra, R. L. (1986). A Method for Finding
+// Projections onto the Intersection of Convex Sets in Hilbert Spaces.
+// Lecture Notes in Statistics, 28–47. doi:10.1007/978-1-4613-9940-7_3
+//
+static inline struct vec vprojectpolytope(struct vec v, float const A[], float const b[], float work[], int n, float tolerance)
+{
+ // early bailout.
+ if (vinpolytope(v, A, b, n, tolerance)) {
+ return v;
+ }
+
+ #ifdef CMATH3D_ASSERTS
+ // check for normalized input.
+ for (int i = 0; i < n; ++i) {
+ struct vec a = vloadf(A + 3 * i);
+ assert(fabsf(vmag2(a) - 1.0f) < 1e-6f);
+ }
+ #endif
+
+ float *z = work;
+ for (int i = 0; i < 3 * n; ++i) {
+ z[i] = 0.0f;
+ }
+
+ float const tolerance2 = fsqr(tolerance);
+ struct vec x = v;
+
+ while (true) {
+ struct vec x_last_iter = x;
+ for (int i = 0; i < n; ++i) {
+ struct vec x_old = x;
+ struct vec ai = vloadf(A + 3 * i);
+ struct vec zi = vloadf(z + 3 * i);
+ x = vprojecthalfspace(vadd(x_old, zi), ai, b[i]);
+ struct vec zi_new = vadd3(x_old, zi, vneg(x));
+ vstoref(zi_new, z + 3 * i);
+ }
+ if (vdist2(x_last_iter, x) < tolerance2) {
+ return x;
+ }
+ }
+}
+
+
// Overall TODO: lines? segments? planes? axis-aligned boxes? spheres?
printf("%s passed\n", __func__);
}
+void test_polytope()
+{
+ srand(1); // deterministic
+
+ // For each polytope, generate a few points, project them,
+ // and check that the projections are closer than other points.
+ int const N_POLYTOPES = 100;
+ int const N_PROJECTIONS = 10;
+ int const N_OTHERS = 1000;
+
+ int const MAX_FACES = 30;
+ float *A = malloc(sizeof(float) * MAX_FACES * 3);
+ float *b = malloc(sizeof(float) * MAX_FACES);
+ float *work = malloc(sizeof(float) * MAX_FACES * 3);
+
+
+ for (int trial = 0; trial < N_POLYTOPES; ++trial) {
+
+ // Random number of polytope faces.
+ int n = rand() % MAX_FACES + 1;
+
+ // If we generate random Ax <= b with b always positive, we ensure that
+ // x = 0 is always a solution; hence the polytope is non-empty. However,
+ // we also want to test nonempty polytopes that don't contain x = 0.
+ // Therefore, we use A(x + shift) <= b with b positive, which is
+ // equivalent to Ax <= b - Ashift.
+ struct vec shift = randsphere();
+
+ for (int i = 0; i < n; ++i) {
+ struct vec a = randsphere();
+ vstoref(a, A + 3 * i);
+ b[i] = randu(0.01, 10) - vdot(a, shift);
+ }
+
+ for (int point = 0; point < N_PROJECTIONS; ++point) {
+ struct vec x = randsphere();
+ struct vec xp = vprojectpolytope(x, A, b, work, n, 1e-6);
+
+ // Feasibility check: projection is inside polytope.
+ // The tolerance is looser than the vprojectpolytope tolerance
+ // because that tolerance doesn't actually guarantee a rigid bound
+ // on constraint violations.
+ assert(vinpolytope(xp, A, b, n, 1e-5));
+
+ // Optimality check: projection is closer than other random points
+ // to query point. Very large N_OTHERS would be more thorough...
+ for (int other = 0; other < N_OTHERS; ++other) {
+ struct vec other = vadd(randsphere(), shift);
+ if (vinpolytope(other, A, b, n, 0.0f)) {
+ assert (vdist2(xp, x) <= vdist2(other, x));
+ }
+ }
+ }
+ }
+
+ free(A);
+ free(b);
+ free(work);
+
+ printf("%s passed\n", __func__);
+}
+
// micro test framework
typedef void (*voidvoid_fn)(void);
voidvoid_fn test_fns[] = {
test_quat_rpy_conversions,
test_quat_mat_conversions,
test_qvectovec,
+ test_polytope,
};
static int i_test = -1;
projects / forks / cmath3d.git / commitdiff