Chipmunk Geometric algorithm

/* Copyright (c) Scott Lembcke
*
* Permission is hereby granted, free of charge, to all person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* In the software without restriction, including without limitation the rights
* To use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* Copies of the software, and to permit persons to whom the software are
* Furnished to does so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall is included in
* All copies or substantial portions of the software.
*
* The software is provided ' as is ', without WARRANTY of any KIND, EXPRESS OR
* implied, including but not LIMITED to the warranties of merchantability,
* FITNESS for A particular PURPOSE and noninfringement. In NO EVENT shall the
* AUTHORS or COPYRIGHT holders be liable for any CLAIM, damages or other
* Liability, WHETHER in a ACTION of contract, TORT OR OTHERWISE, arising from,
* Out of OR in CONNECTION with the software or the use or other dealings in the
* Software.
*/

#include <stdio.h>
#include <string.h>
#include <stdarg.h>

#include "chipmunk_private.h"

void
Cpmessage (const char *condition, const char *file, int line, Cpbool isError, Cpbool isharderror, const char *message, ...)
{
fprintf (stderr, IsError? "Aborting due to Chipmunk error:": "Chipmunk Warning:"));

Va_list Vargs;
Va_start (vargs, message); {
vfprintf (stderr, message, Vargs);
fprintf (stderr, "\ n");
} va_end (Vargs);

fprintf (stderr, "\tfailed condition:%s\n", condition);
fprintf (stderr, "\tsource:%s:%d\n", file, line);

if (IsError) abort ();
}

#define STR (s) #s
#define XSTR (s) STR (s)

const char *cpversionstring = XSTR (cp_version_major) "." XSTR (Cp_version_minor) "." XSTR (cp_version_release);

void
Cpinitchipmunk (void)
{
Cpassertwarn (Cpfalse, "cpinitchipmunk is deprecated and no longer required. It'll be removed on the future. ");
}

Mark:misc Functions

Cpfloat
Cpmomentforcircle (cpfloat m, cpfloat R1, cpfloat R2, cpvect offset)
{
Return m* (0.5f* (r1*r1 + r2*r2) + cpvlengthsq (offset));
}

Cpfloat
Cpareaforcircle (cpfloat R1, cpfloat R2)
{
Return (cpfloat) m_pi*cpfabs (R1*R1-R2*R2);
}

Cpfloat
Cpmomentforsegment (cpfloat m, cpvect A, cpvect b)
{
Cpvect offset = Cpvmult (Cpvadd (A, B), 0.5f);
Return m* (CPVDISTSQ (b, a)/12.0f + cpvlengthsq (offset));
}

Cpfloat
Cpareaforsegment (Cpvect A, Cpvect B, Cpfloat R)
{
Return r* ((cpfloat) M_pi*r + 2.0f*cpvdist (A, b));
}

Cpfloat
Cpmomentforpoly (cpfloat m, const int numverts, const cpvect *verts, cpvect offset)
{
Cpfloat sum1 = 0.0f;
Cpfloat sum2 = 0.0f;
for (int i=0; i<numverts; i++) {
Cpvect v1 = Cpvadd (verts[i], offset);
Cpvect v2 = Cpvadd (verts[(i+1)%numverts], offset);

Cpfloat a = Cpvcross (v2, v1);
Cpfloat B = Cpvdot (v1, v1) + Cpvdot (v1, v2) + Cpvdot (v2, v2);

Sum1 + = A*b;
Sum2 + = A;
}

Return (M*SUM1)/(6.0F*SUM2);
}

Cpfloat
Cpareaforpoly (const int numverts, const cpvect *verts)
{
Cpfloat area = 0.0f;
for (int i=0; i<numverts; i++) {
Area + = Cpvcross (Verts[i], verts[(i+1)%numverts]);
}

return-area/2.0f;
}

Cpvect
Cpcentroidforpoly (const int numverts, const cpvect *verts)
{
Cpfloat sum = 0.0f;
Cpvect vsum = Cpvzero;

for (int i=0; i<numverts; i++) {
Cpvect v1 = verts[i];
Cpvect v2 = verts[(i+1)%numverts];
Cpfloat cross = Cpvcross (v1, v2);

sum + = Cross;
Vsum = Cpvadd (Vsum, Cpvmult (Cpvadd (v1, v2), cross));
}

Return Cpvmult (Vsum, 1.0f/(3.0f*sum));
}

void
Cprecenterpoly (const int numverts, Cpvect *verts) {
Cpvect centroid = Cpcentroidforpoly (Numverts, Verts);

for (int i=0; i<numverts; i++) {
Verts[i] = Cpvsub (Verts[i], centroid);
}
}

Cpfloat
Cpmomentforbox (cpfloat m, cpfloat width, cpfloat height)
{
Return m* (Width*width + height*height)/12.0f;
}

Cpfloat
CpMomentForBox2 (cpfloat m, CPBB Box)
{
Cpfloat width = box.r-box.l;
Cpfloat height = box.t-box.b;
Cpvect offset = Cpvmult (CPV (box.l + BOX.R, box.b + box.t), 0.5f);

TODO NaN when offset is 0 and M are INFINITY
Return Cpmomentforbox (m, width, height) + m*cpvlengthsq (offset);
}

Mark:quick Hull

void
Cploopindexes (cpvect *verts, int count, int *start, int *end)
{
(*start) = (*end) = 0;
Cpvect min = verts[0];
Cpvect max = min;

for (int i=1; i<count; i++) {
Cpvect v = verts[i];

if (v.x < min.x | | (v.x = = min.x && v.y < MIN.Y)) {
min = v;
(*start) = i;
} else if (v.x > max.x | | (v.x = = max.x && v.y > Max.y)) {
max = V;
(*end) = i;
}
}
}

#define SWAP (__a__, __b__) {cpvect __tmp__ = __a__; __a__ = __b__; __b__ = __tmp__;}

Static int
Qhullpartition (cpvect *verts, int count, cpvect A, cpvect B, cpfloat tol)
{
if (count = = 0) return 0 ;

Cpfloat max = 0;
int pivot = 0;

Cpvect delta = cpvsub (b, a);
Cpfloat valuetol = tol*cpvlength (delta);

int head = 0;
for (int tail = count-1; head <= tail;) {
Cpfloat value = Cpvcross (Delta, Cpvsub (Verts[head], a));
if (value > Valuetol) {
if (value > Max) {
Max = value;
Pivot = head;
}

head++;
} else {
SWAP (Verts[head], verts[tail]);
tail--;
}
}

//Move the new pivot to the front if it's not already there.
if (pivot! = 0) SWAP (verts[0], Verts[pivot]);
return head;
}

static int
Qhullreduce (cpfloat tol, cpvect *verts, int count, cpvect A, cpvect pivot, Cpvect B, Cpvect *result)
{
if (Count < 0) {
return 0;
} else if (count = = 0) {
RESULT[0] = pivot;
return 1;
} else {
int left_count = Qhullpartition (Verts, Count, a, pivot, tol);
int index = Qhullreduce (tol, Verts + 1, left_count-1, A, verts[0], pivot, result);

result[index++] = pivot;

int right_count = qhullpartition (verts + left_count, Count-left_count, pivot, B, tol);
return index + qhullreduce (tol, verts + Left_count + 1, right_count-1, Pivot, Verts[left_count], B, result + index);
}
}

Quickhull seemed like a neat algorithm, and efficient-ish for large input sets.
My implementation performs an on place reduction using the result array as scratch space.
Int
Cpconvexhull (int count, Cpvect *verts, cpvect *result, int *first, cpfloat tol)
{
if (result) {
Copy the line vertexes into the empty part of the "result polyline" to use as a scratch buffer.
memcpy (result, Verts, count*sizeof (Cpvect));
} else {
If a result array is not specified, reduce the input instead.
result = Verts;
}

Degenerate case, all poins is the same.
int start, end;
Cploopindexes (Verts, Count, &start, &end);
if (start = = end) {
if (first) (*first) = 0;
return 1;
}

SWAP (Result[0], Result[start]);
SWAP (result[1], result[end = = 0? Start:end]);

Cpvect a = result[0];
Cpvect B = result[1];

if (first) (*first) = start;
int resultcount = Qhullreduce (tol, result + 2, count-2, A, B, a, result + 1) + 1;
Cpassertsoft (Cppolyvalidate (result, Resultcount),
"Internal Error:cpconvexhull () and Cppolyvalidate () did not agree."
"Please report the error with as much info as you can.");
return resultcount;
}

Mark:alternate Block iterators

#if defined (__has_extension)
#if __has_extension (Blocks)

static void Iteratorfunc (void *ptr, void (^block) (void *ptr)) {block (PTR);}

void Cpspaceeachbody_b (Cpspace *space, Void (^block) (Cpbody *body)) {
Cpspaceeachbody (space, (Cpspacebodyiteratorfunc) iteratorfunc, block);
}

void Cpspaceeachshape_b (Cpspace *space, Void (^block) (Cpshape *shape)) {
Cpspaceeachshape (space, (Cpspaceshapeiteratorfunc) iteratorfunc, block);
}

void Cpspaceeachconstraint_b (Cpspace *space, Void (^block) (Cpconstraint *constraint)) {
Cpspaceeachconstraint (space, (Cpspaceconstraintiteratorfunc) iteratorfunc, block);
}

static void Bodyiteratorfunc (Cpbody *body, void *ptr, void (^block) (void *ptr)) {block (PTR);}

void Cpbodyeachshape_b (Cpbody *body, Void (^block) (Cpshape *shape)) {
Cpbodyeachshape (body, (Cpbodyshapeiteratorfunc) bodyiteratorfunc, block);
}

void Cpbodyeachconstraint_b (Cpbody *body, Void (^block) (Cpconstraint *constraint)) {
Cpbodyeachconstraint (body, (Cpbodyconstraintiteratorfunc) bodyiteratorfunc, block);
}

void Cpbodyeacharbiter_b (Cpbody *body, Void (^block) (Cparbiter *arbiter)) {
Cpbodyeacharbiter (body, (Cpbodyarbiteriteratorfunc) bodyiteratorfunc, block);
}

static void Nearestpointqueryiteratorfunc (Cpshape *shape, cpfloat distance, Cpvect Point, Cpspacenearestpointqueryblock block) {block (shape, distance, point);}
void Cpspacenearestpointquery_b (Cpspace *space, Cpvect Point, Cpfloat maxdistance, cplayers layers, Cpgroup Group, CpSpac Enearestpointqueryblock block) {
Cpspacenearestpointquery (space, point, maxdistance, Layers, group, (Cpspacenearestpointqueryfunc) NEARESTPOINTQUERYITERATORFUNC, block);
}

static void Segmentqueryiteratorfunc (Cpshape *shape, Cpfloat T, Cpvect N, cpspacesegmentqueryblock block) {block (shape, t , n);}
void Cpspacesegmentquery_b (Cpspace *space, cpvect start, Cpvect end, cplayers layers, Cpgroup Group, Cpspacesegmentqueryb Lock block) {
Cpspacesegmentquery (space, start, end, layers, group, (Cpspacesegmentqueryfunc) segmentqueryiteratorfunc, block);
}

void Cpspacebbquery_b (cpspace *space, Cpbb bb, cplayers layers, Cpgroup Group, cpspacebbqueryblock block) {
Cpspacebbquery (space, BB, Layers, Group, (Cpspacebbqueryfunc) iteratorfunc, block);
}

static void Shapequeryiteratorfunc (Cpshape *shape, Cpcontactpointset *points, Cpspaceshapequeryblock block) {block ( shape, points);}
Cpbool Cpspaceshapequery_b (cpspace *space, Cpshape *shape, cpspaceshapequeryblock block) {
Return Cpspaceshapequery (Space, Shape, (cpspaceshapequeryfunc) shapequeryiteratorfunc, block);
}

#endif
#endif

#include "chipmunk_ffi.h"

Chipmunk Geometric algorithm