Jérôme Leclercq
parent
498d318a27
commit
26049ebc0e
|
|
@ -2,5 +2,5 @@ MODULE.Name = "Physics2D"
|
|||
|
||||
MODULE.Libraries = {
|
||||
"NazaraCore",
|
||||
"chipmunk-s"
|
||||
"chipmunk"
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,9 @@
|
|||
LIBRARY.Name = "chipmunk"
|
||||
|
||||
LIBRARY.Language = "C++"
|
||||
|
||||
LIBRARY.Files = {
|
||||
"../thirdparty/include/chipmunk/*.h",
|
||||
"../thirdparty/src/chipmunk/*.h",
|
||||
"../thirdparty/src/chipmunk/*.c",
|
||||
}
|
||||
|
|
@ -125,10 +125,10 @@ typedef struct cpSpace cpSpace;
|
|||
|
||||
#include "cpSpace.h"
|
||||
|
||||
// Chipmunk 7.0.1
|
||||
// Chipmunk 7.0.2
|
||||
#define CP_VERSION_MAJOR 7
|
||||
#define CP_VERSION_MINOR 0
|
||||
#define CP_VERSION_RELEASE 1
|
||||
#define CP_VERSION_RELEASE 2
|
||||
|
||||
/// Version string.
|
||||
CP_EXPORT extern const char *cpVersionString;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,331 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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>
|
||||
#if defined(ANDROID)
|
||||
# include <android/log.h>
|
||||
#endif
|
||||
|
||||
#include "chipmunk/chipmunk_private.h"
|
||||
|
||||
void
|
||||
cpMessage(const char *condition, const char *file, int line, int isError, int isHardError, const char *message, ...)
|
||||
{
|
||||
fprintf(stderr, (isError ? "Aborting due to Chipmunk error: " : "Chipmunk warning: "));
|
||||
|
||||
va_list vargs;
|
||||
va_start(vargs, message); {
|
||||
#if defined(ANDROID)
|
||||
__android_log_print( ANDROID_LOG_INFO, "Chipmunk", "%s(%d)", file, line );
|
||||
__android_log_print( ANDROID_LOG_INFO, "Chipmunk", message, vargs );
|
||||
#else
|
||||
vfprintf(stderr, message, vargs);
|
||||
fprintf(stderr, "\n");
|
||||
#endif
|
||||
} va_end(vargs);
|
||||
|
||||
#if defined(ANDROID)
|
||||
__android_log_print(ANDROID_LOG_INFO, "Chipmunk", "\tFailed condition: %s\n", condition);
|
||||
__android_log_print(ANDROID_LOG_INFO, "Chipmunk", "\tSource:%s:%d\n", file, line);
|
||||
#else
|
||||
fprintf(stderr, "\tFailed condition: %s\n", condition);
|
||||
fprintf(stderr, "\tSource:%s:%d\n", file, line);
|
||||
#endif
|
||||
}
|
||||
|
||||
#define STR(s) #s
|
||||
#define XSTR(s) STR(s)
|
||||
|
||||
const char *cpVersionString = XSTR(CP_VERSION_MAJOR) "." XSTR(CP_VERSION_MINOR) "." XSTR(CP_VERSION_RELEASE);
|
||||
|
||||
//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)CP_PI*cpfabs(r1*r1 - r2*r2);
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpMomentForSegment(cpFloat m, cpVect a, cpVect b, cpFloat r)
|
||||
{
|
||||
cpVect offset = cpvlerp(a, b, 0.5f);
|
||||
|
||||
// This approximates the shape as a box for rounded segments, but it's quite close.
|
||||
cpFloat length = cpvdist(b, a) + 2.0f*r;
|
||||
return m*((length*length + 4.0f*r*r)/12.0f + cpvlengthsq(offset));
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpAreaForSegment(cpVect a, cpVect b, cpFloat r)
|
||||
{
|
||||
return r*((cpFloat)CP_PI*r + 2.0f*cpvdist(a, b));
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpMomentForPoly(cpFloat m, const int count, const cpVect *verts, cpVect offset, cpFloat r)
|
||||
{
|
||||
// TODO account for radius.
|
||||
if(count == 2) return cpMomentForSegment(m, verts[0], verts[1], 0.0f);
|
||||
|
||||
cpFloat sum1 = 0.0f;
|
||||
cpFloat sum2 = 0.0f;
|
||||
for(int i=0; i<count; i++){
|
||||
cpVect v1 = cpvadd(verts[i], offset);
|
||||
cpVect v2 = cpvadd(verts[(i+1)%count], 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 count, const cpVect *verts, cpFloat r)
|
||||
{
|
||||
cpFloat area = 0.0f;
|
||||
cpFloat perimeter = 0.0f;
|
||||
for(int i=0; i<count; i++){
|
||||
cpVect v1 = verts[i];
|
||||
cpVect v2 = verts[(i+1)%count];
|
||||
|
||||
area += cpvcross(v1, v2);
|
||||
perimeter += cpvdist(v1, v2);
|
||||
}
|
||||
|
||||
return r*(CP_PI*cpfabs(r) + perimeter) + area/2.0f;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpCentroidForPoly(const int count, const cpVect *verts)
|
||||
{
|
||||
cpFloat sum = 0.0f;
|
||||
cpVect vsum = cpvzero;
|
||||
|
||||
for(int i=0; i<count; i++){
|
||||
cpVect v1 = verts[i];
|
||||
cpVect v2 = verts[(i+1)%count];
|
||||
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 count, cpVect *verts){
|
||||
// cpVect centroid = cpCentroidForPoly(count, verts);
|
||||
//
|
||||
// for(int i=0; i<count; 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 is INFINITY
|
||||
return cpMomentForBox(m, width, height) + m*cpvlengthsq(offset);
|
||||
}
|
||||
|
||||
//MARK: Quick Hull
|
||||
|
||||
void
|
||||
cpLoopIndexes(const 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(cpvsub(verts[head], a), delta);
|
||||
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 in place reduction using the result array as scratch space.
|
||||
int
|
||||
cpConvexHull(int count, const cpVect *verts, cpVect *result, int *first, cpFloat tol)
|
||||
{
|
||||
if(verts != 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));
|
||||
}
|
||||
|
||||
// Degenerate case, all points are 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;
|
||||
return QHullReduce(tol, result + 2, count - 2, a, b, a, result + 1) + 1;
|
||||
}
|
||||
|
||||
//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 PointQueryIteratorFunc(cpShape *shape, cpVect p, cpFloat d, cpVect g, cpSpacePointQueryBlock block){block(shape, p, d, g);}
|
||||
void cpSpacePointQuery_b(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryBlock block){
|
||||
cpSpacePointQuery(space, point, maxDistance, filter, (cpSpacePointQueryFunc)PointQueryIteratorFunc, block);
|
||||
}
|
||||
|
||||
static void SegmentQueryIteratorFunc(cpShape *shape, cpVect p, cpVect n, cpFloat t, cpSpaceSegmentQueryBlock block){block(shape, p, n, t);}
|
||||
void cpSpaceSegmentQuery_b(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryBlock block){
|
||||
cpSpaceSegmentQuery(space, start, end, radius, filter, (cpSpaceSegmentQueryFunc)SegmentQueryIteratorFunc, block);
|
||||
}
|
||||
|
||||
void cpSpaceBBQuery_b(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryBlock block){
|
||||
cpSpaceBBQuery(space, bb, filter, (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/chipmunk_ffi.h"
|
||||
|
|
@ -0,0 +1,496 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
// TODO: make this generic so I can reuse it for constraints also.
|
||||
static inline void
|
||||
unthreadHelper(cpArbiter *arb, cpBody *body)
|
||||
{
|
||||
struct cpArbiterThread *thread = cpArbiterThreadForBody(arb, body);
|
||||
cpArbiter *prev = thread->prev;
|
||||
cpArbiter *next = thread->next;
|
||||
|
||||
if(prev){
|
||||
cpArbiterThreadForBody(prev, body)->next = next;
|
||||
} else if(body->arbiterList == arb) {
|
||||
// IFF prev is NULL and body->arbiterList == arb, is arb at the head of the list.
|
||||
// This function may be called for an arbiter that was never in a list.
|
||||
// In that case, we need to protect it from wiping out the body->arbiterList pointer.
|
||||
body->arbiterList = next;
|
||||
}
|
||||
|
||||
if(next) cpArbiterThreadForBody(next, body)->prev = prev;
|
||||
|
||||
thread->prev = NULL;
|
||||
thread->next = NULL;
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterUnthread(cpArbiter *arb)
|
||||
{
|
||||
unthreadHelper(arb, arb->body_a);
|
||||
unthreadHelper(arb, arb->body_b);
|
||||
}
|
||||
|
||||
cpBool cpArbiterIsFirstContact(const cpArbiter *arb)
|
||||
{
|
||||
return arb->state == CP_ARBITER_STATE_FIRST_COLLISION;
|
||||
}
|
||||
|
||||
cpBool cpArbiterIsRemoval(const cpArbiter *arb)
|
||||
{
|
||||
return arb->state == CP_ARBITER_STATE_INVALIDATED;
|
||||
}
|
||||
|
||||
int cpArbiterGetCount(const cpArbiter *arb)
|
||||
{
|
||||
// Return 0 contacts if we are in a separate callback.
|
||||
return (arb->state < CP_ARBITER_STATE_CACHED ? arb->count : 0);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpArbiterGetNormal(const cpArbiter *arb)
|
||||
{
|
||||
return cpvmult(arb->n, arb->swapped ? -1.0f : 1.0);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpArbiterGetPointA(const cpArbiter *arb, int i)
|
||||
{
|
||||
cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
|
||||
return cpvadd(arb->body_a->p, arb->contacts[i].r1);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpArbiterGetPointB(const cpArbiter *arb, int i)
|
||||
{
|
||||
cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
|
||||
return cpvadd(arb->body_b->p, arb->contacts[i].r2);
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpArbiterGetDepth(const cpArbiter *arb, int i)
|
||||
{
|
||||
cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
|
||||
|
||||
struct cpContact *con = &arb->contacts[i];
|
||||
return cpvdot(cpvadd(cpvsub(con->r2, con->r1), cpvsub(arb->body_b->p, arb->body_a->p)), arb->n);
|
||||
}
|
||||
|
||||
cpContactPointSet
|
||||
cpArbiterGetContactPointSet(const cpArbiter *arb)
|
||||
{
|
||||
cpContactPointSet set;
|
||||
set.count = cpArbiterGetCount(arb);
|
||||
|
||||
cpBool swapped = arb->swapped;
|
||||
cpVect n = arb->n;
|
||||
set.normal = (swapped ? cpvneg(n) : n);
|
||||
|
||||
for(int i=0; i<set.count; i++){
|
||||
// Contact points are relative to body CoGs;
|
||||
cpVect p1 = cpvadd(arb->body_a->p, arb->contacts[i].r1);
|
||||
cpVect p2 = cpvadd(arb->body_b->p, arb->contacts[i].r2);
|
||||
|
||||
set.points[i].pointA = (swapped ? p2 : p1);
|
||||
set.points[i].pointB = (swapped ? p1 : p2);
|
||||
set.points[i].distance = cpvdot(cpvsub(p2, p1), n);
|
||||
}
|
||||
|
||||
return set;
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterSetContactPointSet(cpArbiter *arb, cpContactPointSet *set)
|
||||
{
|
||||
int count = set->count;
|
||||
cpAssertHard(count == arb->count, "The number of contact points cannot be changed.");
|
||||
|
||||
cpBool swapped = arb->swapped;
|
||||
arb->n = (swapped ? cpvneg(set->normal) : set->normal);
|
||||
|
||||
for(int i=0; i<count; i++){
|
||||
// Convert back to CoG relative offsets.
|
||||
cpVect p1 = set->points[i].pointA;
|
||||
cpVect p2 = set->points[i].pointB;
|
||||
|
||||
arb->contacts[i].r1 = cpvsub(swapped ? p2 : p1, arb->body_a->p);
|
||||
arb->contacts[i].r2 = cpvsub(swapped ? p1 : p2, arb->body_b->p);
|
||||
}
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpArbiterTotalImpulse(const cpArbiter *arb)
|
||||
{
|
||||
struct cpContact *contacts = arb->contacts;
|
||||
cpVect n = arb->n;
|
||||
cpVect sum = cpvzero;
|
||||
|
||||
for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
|
||||
struct cpContact *con = &contacts[i];
|
||||
sum = cpvadd(sum, cpvrotate(n, cpv(con->jnAcc, con->jtAcc)));
|
||||
}
|
||||
|
||||
return (arb->swapped ? sum : cpvneg(sum));
|
||||
return cpvzero;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpArbiterTotalKE(const cpArbiter *arb)
|
||||
{
|
||||
cpFloat eCoef = (1 - arb->e)/(1 + arb->e);
|
||||
cpFloat sum = 0.0;
|
||||
|
||||
struct cpContact *contacts = arb->contacts;
|
||||
for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
|
||||
struct cpContact *con = &contacts[i];
|
||||
cpFloat jnAcc = con->jnAcc;
|
||||
cpFloat jtAcc = con->jtAcc;
|
||||
|
||||
sum += eCoef*jnAcc*jnAcc/con->nMass + jtAcc*jtAcc/con->tMass;
|
||||
}
|
||||
|
||||
return sum;
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpArbiterIgnore(cpArbiter *arb)
|
||||
{
|
||||
arb->state = CP_ARBITER_STATE_IGNORE;
|
||||
return cpFalse;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpArbiterGetRestitution(const cpArbiter *arb)
|
||||
{
|
||||
return arb->e;
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterSetRestitution(cpArbiter *arb, cpFloat restitution)
|
||||
{
|
||||
arb->e = restitution;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpArbiterGetFriction(const cpArbiter *arb)
|
||||
{
|
||||
return arb->u;
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterSetFriction(cpArbiter *arb, cpFloat friction)
|
||||
{
|
||||
arb->u = friction;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpArbiterGetSurfaceVelocity(cpArbiter *arb)
|
||||
{
|
||||
return cpvmult(arb->surface_vr, arb->swapped ? -1.0f : 1.0);
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterSetSurfaceVelocity(cpArbiter *arb, cpVect vr)
|
||||
{
|
||||
arb->surface_vr = cpvmult(vr, arb->swapped ? -1.0f : 1.0);
|
||||
}
|
||||
|
||||
cpDataPointer
|
||||
cpArbiterGetUserData(const cpArbiter *arb)
|
||||
{
|
||||
return arb->data;
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterSetUserData(cpArbiter *arb, cpDataPointer userData)
|
||||
{
|
||||
arb->data = userData;
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b)
|
||||
{
|
||||
if(arb->swapped){
|
||||
(*a) = (cpShape *)arb->b, (*b) = (cpShape *)arb->a;
|
||||
} else {
|
||||
(*a) = (cpShape *)arb->a, (*b) = (cpShape *)arb->b;
|
||||
}
|
||||
}
|
||||
|
||||
void cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b)
|
||||
{
|
||||
CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b);
|
||||
(*a) = shape_a->body;
|
||||
(*b) = shape_b->body;
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpArbiterCallWildcardBeginA(cpArbiter *arb, cpSpace *space)
|
||||
{
|
||||
cpCollisionHandler *handler = arb->handlerA;
|
||||
return handler->beginFunc(arb, space, handler->userData);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpArbiterCallWildcardBeginB(cpArbiter *arb, cpSpace *space)
|
||||
{
|
||||
cpCollisionHandler *handler = arb->handlerB;
|
||||
arb->swapped = !arb->swapped;
|
||||
cpBool retval = handler->beginFunc(arb, space, handler->userData);
|
||||
arb->swapped = !arb->swapped;
|
||||
return retval;
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpArbiterCallWildcardPreSolveA(cpArbiter *arb, cpSpace *space)
|
||||
{
|
||||
cpCollisionHandler *handler = arb->handlerA;
|
||||
return handler->preSolveFunc(arb, space, handler->userData);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpArbiterCallWildcardPreSolveB(cpArbiter *arb, cpSpace *space)
|
||||
{
|
||||
cpCollisionHandler *handler = arb->handlerB;
|
||||
arb->swapped = !arb->swapped;
|
||||
cpBool retval = handler->preSolveFunc(arb, space, handler->userData);
|
||||
arb->swapped = !arb->swapped;
|
||||
return retval;
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterCallWildcardPostSolveA(cpArbiter *arb, cpSpace *space)
|
||||
{
|
||||
cpCollisionHandler *handler = arb->handlerA;
|
||||
handler->postSolveFunc(arb, space, handler->userData);
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterCallWildcardPostSolveB(cpArbiter *arb, cpSpace *space)
|
||||
{
|
||||
cpCollisionHandler *handler = arb->handlerB;
|
||||
arb->swapped = !arb->swapped;
|
||||
handler->postSolveFunc(arb, space, handler->userData);
|
||||
arb->swapped = !arb->swapped;
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterCallWildcardSeparateA(cpArbiter *arb, cpSpace *space)
|
||||
{
|
||||
cpCollisionHandler *handler = arb->handlerA;
|
||||
handler->separateFunc(arb, space, handler->userData);
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterCallWildcardSeparateB(cpArbiter *arb, cpSpace *space)
|
||||
{
|
||||
cpCollisionHandler *handler = arb->handlerB;
|
||||
arb->swapped = !arb->swapped;
|
||||
handler->separateFunc(arb, space, handler->userData);
|
||||
arb->swapped = !arb->swapped;
|
||||
}
|
||||
|
||||
cpArbiter*
|
||||
cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b)
|
||||
{
|
||||
arb->handler = NULL;
|
||||
arb->swapped = cpFalse;
|
||||
|
||||
arb->handler = NULL;
|
||||
arb->handlerA = NULL;
|
||||
arb->handlerB = NULL;
|
||||
|
||||
arb->e = 0.0f;
|
||||
arb->u = 0.0f;
|
||||
arb->surface_vr = cpvzero;
|
||||
|
||||
arb->count = 0;
|
||||
arb->contacts = NULL;
|
||||
|
||||
arb->a = a; arb->body_a = a->body;
|
||||
arb->b = b; arb->body_b = b->body;
|
||||
|
||||
arb->thread_a.next = NULL;
|
||||
arb->thread_b.next = NULL;
|
||||
arb->thread_a.prev = NULL;
|
||||
arb->thread_b.prev = NULL;
|
||||
|
||||
arb->stamp = 0;
|
||||
arb->state = CP_ARBITER_STATE_FIRST_COLLISION;
|
||||
|
||||
arb->data = NULL;
|
||||
|
||||
return arb;
|
||||
}
|
||||
|
||||
static inline cpCollisionHandler *
|
||||
cpSpaceLookupHandler(cpSpace *space, cpCollisionType a, cpCollisionType b, cpCollisionHandler *defaultValue)
|
||||
{
|
||||
cpCollisionType types[] = {a, b};
|
||||
cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collisionHandlers, CP_HASH_PAIR(a, b), types);
|
||||
return (handler ? handler : defaultValue);
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterUpdate(cpArbiter *arb, struct cpCollisionInfo *info, cpSpace *space)
|
||||
{
|
||||
const cpShape *a = info->a, *b = info->b;
|
||||
|
||||
// For collisions between two similar primitive types, the order could have been swapped since the last frame.
|
||||
arb->a = a; arb->body_a = a->body;
|
||||
arb->b = b; arb->body_b = b->body;
|
||||
|
||||
// Iterate over the possible pairs to look for hash value matches.
|
||||
for(int i=0; i<info->count; i++){
|
||||
struct cpContact *con = &info->arr[i];
|
||||
|
||||
// r1 and r2 store absolute offsets at init time.
|
||||
// Need to convert them to relative offsets.
|
||||
con->r1 = cpvsub(con->r1, a->body->p);
|
||||
con->r2 = cpvsub(con->r2, b->body->p);
|
||||
|
||||
// Cached impulses are not zeroed at init time.
|
||||
con->jnAcc = con->jtAcc = 0.0f;
|
||||
|
||||
for(int j=0; j<arb->count; j++){
|
||||
struct cpContact *old = &arb->contacts[j];
|
||||
|
||||
// This could trigger false positives, but is fairly unlikely nor serious if it does.
|
||||
if(con->hash == old->hash){
|
||||
// Copy the persistant contact information.
|
||||
con->jnAcc = old->jnAcc;
|
||||
con->jtAcc = old->jtAcc;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
arb->contacts = info->arr;
|
||||
arb->count = info->count;
|
||||
arb->n = info->n;
|
||||
|
||||
arb->e = a->e * b->e;
|
||||
arb->u = a->u * b->u;
|
||||
|
||||
cpVect surface_vr = cpvsub(b->surfaceV, a->surfaceV);
|
||||
arb->surface_vr = cpvsub(surface_vr, cpvmult(info->n, cpvdot(surface_vr, info->n)));
|
||||
|
||||
cpCollisionType typeA = info->a->type, typeB = info->b->type;
|
||||
cpCollisionHandler *defaultHandler = &space->defaultHandler;
|
||||
cpCollisionHandler *handler = arb->handler = cpSpaceLookupHandler(space, typeA, typeB, defaultHandler);
|
||||
|
||||
// Check if the types match, but don't swap for a default handler which use the wildcard for type A.
|
||||
cpBool swapped = arb->swapped = (typeA != handler->typeA && handler->typeA != CP_WILDCARD_COLLISION_TYPE);
|
||||
|
||||
if(handler != defaultHandler || space->usesWildcards){
|
||||
// The order of the main handler swaps the wildcard handlers too. Uffda.
|
||||
arb->handlerA = cpSpaceLookupHandler(space, (swapped ? typeB : typeA), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing);
|
||||
arb->handlerB = cpSpaceLookupHandler(space, (swapped ? typeA : typeB), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing);
|
||||
}
|
||||
|
||||
// mark it as new if it's been cached
|
||||
if(arb->state == CP_ARBITER_STATE_CACHED) arb->state = CP_ARBITER_STATE_FIRST_COLLISION;
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
|
||||
{
|
||||
cpBody *a = arb->body_a;
|
||||
cpBody *b = arb->body_b;
|
||||
cpVect n = arb->n;
|
||||
cpVect body_delta = cpvsub(b->p, a->p);
|
||||
|
||||
for(int i=0; i<arb->count; i++){
|
||||
struct cpContact *con = &arb->contacts[i];
|
||||
|
||||
// Calculate the mass normal and mass tangent.
|
||||
con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, n);
|
||||
con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(n));
|
||||
|
||||
// Calculate the target bias velocity.
|
||||
cpFloat dist = cpvdot(cpvadd(cpvsub(con->r2, con->r1), body_delta), n);
|
||||
con->bias = -bias*cpfmin(0.0f, dist + slop)/dt;
|
||||
con->jBias = 0.0f;
|
||||
|
||||
// Calculate the target bounce velocity.
|
||||
con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, n)*arb->e;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef)
|
||||
{
|
||||
if(cpArbiterIsFirstContact(arb)) return;
|
||||
|
||||
cpBody *a = arb->body_a;
|
||||
cpBody *b = arb->body_b;
|
||||
cpVect n = arb->n;
|
||||
|
||||
for(int i=0; i<arb->count; i++){
|
||||
struct cpContact *con = &arb->contacts[i];
|
||||
cpVect j = cpvrotate(n, cpv(con->jnAcc, con->jtAcc));
|
||||
apply_impulses(a, b, con->r1, con->r2, cpvmult(j, dt_coef));
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: is it worth splitting velocity/position correction?
|
||||
|
||||
void
|
||||
cpArbiterApplyImpulse(cpArbiter *arb)
|
||||
{
|
||||
cpBody *a = arb->body_a;
|
||||
cpBody *b = arb->body_b;
|
||||
cpVect n = arb->n;
|
||||
cpVect surface_vr = arb->surface_vr;
|
||||
cpFloat friction = arb->u;
|
||||
|
||||
for(int i=0; i<arb->count; i++){
|
||||
struct cpContact *con = &arb->contacts[i];
|
||||
cpFloat nMass = con->nMass;
|
||||
cpVect r1 = con->r1;
|
||||
cpVect r2 = con->r2;
|
||||
|
||||
cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
|
||||
cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
|
||||
cpVect vr = cpvadd(relative_velocity(a, b, r1, r2), surface_vr);
|
||||
|
||||
cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
|
||||
cpFloat vrn = cpvdot(vr, n);
|
||||
cpFloat vrt = cpvdot(vr, cpvperp(n));
|
||||
|
||||
cpFloat jbn = (con->bias - vbn)*nMass;
|
||||
cpFloat jbnOld = con->jBias;
|
||||
con->jBias = cpfmax(jbnOld + jbn, 0.0f);
|
||||
|
||||
cpFloat jn = -(con->bounce + vrn)*nMass;
|
||||
cpFloat jnOld = con->jnAcc;
|
||||
con->jnAcc = cpfmax(jnOld + jn, 0.0f);
|
||||
|
||||
cpFloat jtMax = friction*con->jnAcc;
|
||||
cpFloat jt = -vrt*con->tMass;
|
||||
cpFloat jtOld = con->jtAcc;
|
||||
con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
|
||||
|
||||
apply_bias_impulses(a, b, r1, r2, cpvmult(n, con->jBias - jbnOld));
|
||||
apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(con->jnAcc - jnOld, con->jtAcc - jtOld)));
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 <string.h>
|
||||
|
||||
#include "chipmunk/chipmunk_private.h"
|
||||
|
||||
|
||||
cpArray *
|
||||
cpArrayNew(int size)
|
||||
{
|
||||
cpArray *arr = (cpArray *)cpcalloc(1, sizeof(cpArray));
|
||||
|
||||
arr->num = 0;
|
||||
arr->max = (size ? size : 4);
|
||||
arr->arr = (void **)cpcalloc(arr->max, sizeof(void*));
|
||||
|
||||
return arr;
|
||||
}
|
||||
|
||||
void
|
||||
cpArrayFree(cpArray *arr)
|
||||
{
|
||||
if(arr){
|
||||
cpfree(arr->arr);
|
||||
arr->arr = NULL;
|
||||
|
||||
cpfree(arr);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpArrayPush(cpArray *arr, void *object)
|
||||
{
|
||||
if(arr->num == arr->max){
|
||||
arr->max = 3*(arr->max + 1)/2;
|
||||
arr->arr = (void **)cprealloc(arr->arr, arr->max*sizeof(void*));
|
||||
}
|
||||
|
||||
arr->arr[arr->num] = object;
|
||||
arr->num++;
|
||||
}
|
||||
|
||||
void *
|
||||
cpArrayPop(cpArray *arr)
|
||||
{
|
||||
arr->num--;
|
||||
|
||||
void *value = arr->arr[arr->num];
|
||||
arr->arr[arr->num] = NULL;
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
void
|
||||
cpArrayDeleteObj(cpArray *arr, void *obj)
|
||||
{
|
||||
for(int i=0; i<arr->num; i++){
|
||||
if(arr->arr[i] == obj){
|
||||
arr->num--;
|
||||
|
||||
arr->arr[i] = arr->arr[arr->num];
|
||||
arr->arr[arr->num] = NULL;
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*))
|
||||
{
|
||||
for(int i=0; i<arr->num; i++) freeFunc(arr->arr[i]);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpArrayContains(cpArray *arr, void *ptr)
|
||||
{
|
||||
for(int i=0; i<arr->num; i++)
|
||||
if(arr->arr[i] == ptr) return cpTrue;
|
||||
|
||||
return cpFalse;
|
||||
}
|
||||
|
|
@ -0,0 +1,896 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "stdlib.h"
|
||||
#include "stdio.h"
|
||||
|
||||
#include "chipmunk/chipmunk_private.h"
|
||||
|
||||
static inline cpSpatialIndexClass *Klass();
|
||||
|
||||
typedef struct Node Node;
|
||||
typedef struct Pair Pair;
|
||||
|
||||
struct cpBBTree {
|
||||
cpSpatialIndex spatialIndex;
|
||||
cpBBTreeVelocityFunc velocityFunc;
|
||||
|
||||
cpHashSet *leaves;
|
||||
Node *root;
|
||||
|
||||
Node *pooledNodes;
|
||||
Pair *pooledPairs;
|
||||
cpArray *allocatedBuffers;
|
||||
|
||||
cpTimestamp stamp;
|
||||
};
|
||||
|
||||
struct Node {
|
||||
void *obj;
|
||||
cpBB bb;
|
||||
Node *parent;
|
||||
|
||||
union {
|
||||
// Internal nodes
|
||||
struct { Node *a, *b; } children;
|
||||
|
||||
// Leaves
|
||||
struct {
|
||||
cpTimestamp stamp;
|
||||
Pair *pairs;
|
||||
} leaf;
|
||||
} node;
|
||||
};
|
||||
|
||||
// Can't use anonymous unions and still get good x-compiler compatability
|
||||
#define A node.children.a
|
||||
#define B node.children.b
|
||||
#define STAMP node.leaf.stamp
|
||||
#define PAIRS node.leaf.pairs
|
||||
|
||||
typedef struct Thread {
|
||||
Pair *prev;
|
||||
Node *leaf;
|
||||
Pair *next;
|
||||
} Thread;
|
||||
|
||||
struct Pair {
|
||||
Thread a, b;
|
||||
cpCollisionID id;
|
||||
};
|
||||
|
||||
//MARK: Misc Functions
|
||||
|
||||
static inline cpBB
|
||||
GetBB(cpBBTree *tree, void *obj)
|
||||
{
|
||||
cpBB bb = tree->spatialIndex.bbfunc(obj);
|
||||
|
||||
cpBBTreeVelocityFunc velocityFunc = tree->velocityFunc;
|
||||
if(velocityFunc){
|
||||
cpFloat coef = 0.1f;
|
||||
cpFloat x = (bb.r - bb.l)*coef;
|
||||
cpFloat y = (bb.t - bb.b)*coef;
|
||||
|
||||
cpVect v = cpvmult(velocityFunc(obj), 0.1f);
|
||||
return cpBBNew(bb.l + cpfmin(-x, v.x), bb.b + cpfmin(-y, v.y), bb.r + cpfmax(x, v.x), bb.t + cpfmax(y, v.y));
|
||||
} else {
|
||||
return bb;
|
||||
}
|
||||
}
|
||||
|
||||
static inline cpBBTree *
|
||||
GetTree(cpSpatialIndex *index)
|
||||
{
|
||||
return (index && index->klass == Klass() ? (cpBBTree *)index : NULL);
|
||||
}
|
||||
|
||||
static inline Node *
|
||||
GetRootIfTree(cpSpatialIndex *index){
|
||||
return (index && index->klass == Klass() ? ((cpBBTree *)index)->root : NULL);
|
||||
}
|
||||
|
||||
static inline cpBBTree *
|
||||
GetMasterTree(cpBBTree *tree)
|
||||
{
|
||||
cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex);
|
||||
return (dynamicTree ? dynamicTree : tree);
|
||||
}
|
||||
|
||||
static inline void
|
||||
IncrementStamp(cpBBTree *tree)
|
||||
{
|
||||
cpBBTree *dynamicTree = GetTree(tree->spatialIndex.dynamicIndex);
|
||||
if(dynamicTree){
|
||||
dynamicTree->stamp++;
|
||||
} else {
|
||||
tree->stamp++;
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Pair/Thread Functions
|
||||
|
||||
static void
|
||||
PairRecycle(cpBBTree *tree, Pair *pair)
|
||||
{
|
||||
// Share the pool of the master tree.
|
||||
// TODO: would be lovely to move the pairs stuff into an external data structure.
|
||||
tree = GetMasterTree(tree);
|
||||
|
||||
pair->a.next = tree->pooledPairs;
|
||||
tree->pooledPairs = pair;
|
||||
}
|
||||
|
||||
static Pair *
|
||||
PairFromPool(cpBBTree *tree)
|
||||
{
|
||||
// Share the pool of the master tree.
|
||||
// TODO: would be lovely to move the pairs stuff into an external data structure.
|
||||
tree = GetMasterTree(tree);
|
||||
|
||||
Pair *pair = tree->pooledPairs;
|
||||
|
||||
if(pair){
|
||||
tree->pooledPairs = pair->a.next;
|
||||
return pair;
|
||||
} else {
|
||||
// Pool is exhausted, make more
|
||||
int count = CP_BUFFER_BYTES/sizeof(Pair);
|
||||
cpAssertHard(count, "Internal Error: Buffer size is too small.");
|
||||
|
||||
Pair *buffer = (Pair *)cpcalloc(1, CP_BUFFER_BYTES);
|
||||
cpArrayPush(tree->allocatedBuffers, buffer);
|
||||
|
||||
// push all but the first one, return the first instead
|
||||
for(int i=1; i<count; i++) PairRecycle(tree, buffer + i);
|
||||
return buffer;
|
||||
}
|
||||
}
|
||||
|
||||
static inline void
|
||||
ThreadUnlink(Thread thread)
|
||||
{
|
||||
Pair *next = thread.next;
|
||||
Pair *prev = thread.prev;
|
||||
|
||||
if(next){
|
||||
if(next->a.leaf == thread.leaf) next->a.prev = prev; else next->b.prev = prev;
|
||||
}
|
||||
|
||||
if(prev){
|
||||
if(prev->a.leaf == thread.leaf) prev->a.next = next; else prev->b.next = next;
|
||||
} else {
|
||||
thread.leaf->PAIRS = next;
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
PairsClear(Node *leaf, cpBBTree *tree)
|
||||
{
|
||||
Pair *pair = leaf->PAIRS;
|
||||
leaf->PAIRS = NULL;
|
||||
|
||||
while(pair){
|
||||
if(pair->a.leaf == leaf){
|
||||
Pair *next = pair->a.next;
|
||||
ThreadUnlink(pair->b);
|
||||
PairRecycle(tree, pair);
|
||||
pair = next;
|
||||
} else {
|
||||
Pair *next = pair->b.next;
|
||||
ThreadUnlink(pair->a);
|
||||
PairRecycle(tree, pair);
|
||||
pair = next;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
PairInsert(Node *a, Node *b, cpBBTree *tree)
|
||||
{
|
||||
Pair *nextA = a->PAIRS, *nextB = b->PAIRS;
|
||||
Pair *pair = PairFromPool(tree);
|
||||
Pair temp = {{NULL, a, nextA},{NULL, b, nextB}, 0};
|
||||
|
||||
a->PAIRS = b->PAIRS = pair;
|
||||
*pair = temp;
|
||||
|
||||
if(nextA){
|
||||
if(nextA->a.leaf == a) nextA->a.prev = pair; else nextA->b.prev = pair;
|
||||
}
|
||||
|
||||
if(nextB){
|
||||
if(nextB->a.leaf == b) nextB->a.prev = pair; else nextB->b.prev = pair;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//MARK: Node Functions
|
||||
|
||||
static void
|
||||
NodeRecycle(cpBBTree *tree, Node *node)
|
||||
{
|
||||
node->parent = tree->pooledNodes;
|
||||
tree->pooledNodes = node;
|
||||
}
|
||||
|
||||
static Node *
|
||||
NodeFromPool(cpBBTree *tree)
|
||||
{
|
||||
Node *node = tree->pooledNodes;
|
||||
|
||||
if(node){
|
||||
tree->pooledNodes = node->parent;
|
||||
return node;
|
||||
} else {
|
||||
// Pool is exhausted, make more
|
||||
int count = CP_BUFFER_BYTES/sizeof(Node);
|
||||
cpAssertHard(count, "Internal Error: Buffer size is too small.");
|
||||
|
||||
Node *buffer = (Node *)cpcalloc(1, CP_BUFFER_BYTES);
|
||||
cpArrayPush(tree->allocatedBuffers, buffer);
|
||||
|
||||
// push all but the first one, return the first instead
|
||||
for(int i=1; i<count; i++) NodeRecycle(tree, buffer + i);
|
||||
return buffer;
|
||||
}
|
||||
}
|
||||
|
||||
static inline void
|
||||
NodeSetA(Node *node, Node *value)
|
||||
{
|
||||
node->A = value;
|
||||
value->parent = node;
|
||||
}
|
||||
|
||||
static inline void
|
||||
NodeSetB(Node *node, Node *value)
|
||||
{
|
||||
node->B = value;
|
||||
value->parent = node;
|
||||
}
|
||||
|
||||
static Node *
|
||||
NodeNew(cpBBTree *tree, Node *a, Node *b)
|
||||
{
|
||||
Node *node = NodeFromPool(tree);
|
||||
|
||||
node->obj = NULL;
|
||||
node->bb = cpBBMerge(a->bb, b->bb);
|
||||
node->parent = NULL;
|
||||
|
||||
NodeSetA(node, a);
|
||||
NodeSetB(node, b);
|
||||
|
||||
return node;
|
||||
}
|
||||
|
||||
static inline cpBool
|
||||
NodeIsLeaf(Node *node)
|
||||
{
|
||||
return (node->obj != NULL);
|
||||
}
|
||||
|
||||
static inline Node *
|
||||
NodeOther(Node *node, Node *child)
|
||||
{
|
||||
return (node->A == child ? node->B : node->A);
|
||||
}
|
||||
|
||||
static inline void
|
||||
NodeReplaceChild(Node *parent, Node *child, Node *value, cpBBTree *tree)
|
||||
{
|
||||
cpAssertSoft(!NodeIsLeaf(parent), "Internal Error: Cannot replace child of a leaf.");
|
||||
cpAssertSoft(child == parent->A || child == parent->B, "Internal Error: Node is not a child of parent.");
|
||||
|
||||
if(parent->A == child){
|
||||
NodeRecycle(tree, parent->A);
|
||||
NodeSetA(parent, value);
|
||||
} else {
|
||||
NodeRecycle(tree, parent->B);
|
||||
NodeSetB(parent, value);
|
||||
}
|
||||
|
||||
for(Node *node=parent; node; node = node->parent){
|
||||
node->bb = cpBBMerge(node->A->bb, node->B->bb);
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Subtree Functions
|
||||
|
||||
static inline cpFloat
|
||||
cpBBProximity(cpBB a, cpBB b)
|
||||
{
|
||||
return cpfabs(a.l + a.r - b.l - b.r) + cpfabs(a.b + a.t - b.b - b.t);
|
||||
}
|
||||
|
||||
static Node *
|
||||
SubtreeInsert(Node *subtree, Node *leaf, cpBBTree *tree)
|
||||
{
|
||||
if(subtree == NULL){
|
||||
return leaf;
|
||||
} else if(NodeIsLeaf(subtree)){
|
||||
return NodeNew(tree, leaf, subtree);
|
||||
} else {
|
||||
cpFloat cost_a = cpBBArea(subtree->B->bb) + cpBBMergedArea(subtree->A->bb, leaf->bb);
|
||||
cpFloat cost_b = cpBBArea(subtree->A->bb) + cpBBMergedArea(subtree->B->bb, leaf->bb);
|
||||
|
||||
if(cost_a == cost_b){
|
||||
cost_a = cpBBProximity(subtree->A->bb, leaf->bb);
|
||||
cost_b = cpBBProximity(subtree->B->bb, leaf->bb);
|
||||
}
|
||||
|
||||
if(cost_b < cost_a){
|
||||
NodeSetB(subtree, SubtreeInsert(subtree->B, leaf, tree));
|
||||
} else {
|
||||
NodeSetA(subtree, SubtreeInsert(subtree->A, leaf, tree));
|
||||
}
|
||||
|
||||
subtree->bb = cpBBMerge(subtree->bb, leaf->bb);
|
||||
return subtree;
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
SubtreeQuery(Node *subtree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data)
|
||||
{
|
||||
if(cpBBIntersects(subtree->bb, bb)){
|
||||
if(NodeIsLeaf(subtree)){
|
||||
func(obj, subtree->obj, 0, data);
|
||||
} else {
|
||||
SubtreeQuery(subtree->A, obj, bb, func, data);
|
||||
SubtreeQuery(subtree->B, obj, bb, func, data);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static cpFloat
|
||||
SubtreeSegmentQuery(Node *subtree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
|
||||
{
|
||||
if(NodeIsLeaf(subtree)){
|
||||
return func(obj, subtree->obj, data);
|
||||
} else {
|
||||
cpFloat t_a = cpBBSegmentQuery(subtree->A->bb, a, b);
|
||||
cpFloat t_b = cpBBSegmentQuery(subtree->B->bb, a, b);
|
||||
|
||||
if(t_a < t_b){
|
||||
if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data));
|
||||
if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data));
|
||||
} else {
|
||||
if(t_b < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->B, obj, a, b, t_exit, func, data));
|
||||
if(t_a < t_exit) t_exit = cpfmin(t_exit, SubtreeSegmentQuery(subtree->A, obj, a, b, t_exit, func, data));
|
||||
}
|
||||
|
||||
return t_exit;
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
SubtreeRecycle(cpBBTree *tree, Node *node)
|
||||
{
|
||||
if(!NodeIsLeaf(node)){
|
||||
SubtreeRecycle(tree, node->A);
|
||||
SubtreeRecycle(tree, node->B);
|
||||
NodeRecycle(tree, node);
|
||||
}
|
||||
}
|
||||
|
||||
static inline Node *
|
||||
SubtreeRemove(Node *subtree, Node *leaf, cpBBTree *tree)
|
||||
{
|
||||
if(leaf == subtree){
|
||||
return NULL;
|
||||
} else {
|
||||
Node *parent = leaf->parent;
|
||||
if(parent == subtree){
|
||||
Node *other = NodeOther(subtree, leaf);
|
||||
other->parent = subtree->parent;
|
||||
NodeRecycle(tree, subtree);
|
||||
return other;
|
||||
} else {
|
||||
NodeReplaceChild(parent->parent, parent, NodeOther(parent, leaf), tree);
|
||||
return subtree;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Marking Functions
|
||||
|
||||
typedef struct MarkContext {
|
||||
cpBBTree *tree;
|
||||
Node *staticRoot;
|
||||
cpSpatialIndexQueryFunc func;
|
||||
void *data;
|
||||
} MarkContext;
|
||||
|
||||
static void
|
||||
MarkLeafQuery(Node *subtree, Node *leaf, cpBool left, MarkContext *context)
|
||||
{
|
||||
if(cpBBIntersects(leaf->bb, subtree->bb)){
|
||||
if(NodeIsLeaf(subtree)){
|
||||
if(left){
|
||||
PairInsert(leaf, subtree, context->tree);
|
||||
} else {
|
||||
if(subtree->STAMP < leaf->STAMP) PairInsert(subtree, leaf, context->tree);
|
||||
context->func(leaf->obj, subtree->obj, 0, context->data);
|
||||
}
|
||||
} else {
|
||||
MarkLeafQuery(subtree->A, leaf, left, context);
|
||||
MarkLeafQuery(subtree->B, leaf, left, context);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
MarkLeaf(Node *leaf, MarkContext *context)
|
||||
{
|
||||
cpBBTree *tree = context->tree;
|
||||
if(leaf->STAMP == GetMasterTree(tree)->stamp){
|
||||
Node *staticRoot = context->staticRoot;
|
||||
if(staticRoot) MarkLeafQuery(staticRoot, leaf, cpFalse, context);
|
||||
|
||||
for(Node *node = leaf; node->parent; node = node->parent){
|
||||
if(node == node->parent->A){
|
||||
MarkLeafQuery(node->parent->B, leaf, cpTrue, context);
|
||||
} else {
|
||||
MarkLeafQuery(node->parent->A, leaf, cpFalse, context);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
Pair *pair = leaf->PAIRS;
|
||||
while(pair){
|
||||
if(leaf == pair->b.leaf){
|
||||
pair->id = context->func(pair->a.leaf->obj, leaf->obj, pair->id, context->data);
|
||||
pair = pair->b.next;
|
||||
} else {
|
||||
pair = pair->a.next;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
MarkSubtree(Node *subtree, MarkContext *context)
|
||||
{
|
||||
if(NodeIsLeaf(subtree)){
|
||||
MarkLeaf(subtree, context);
|
||||
} else {
|
||||
MarkSubtree(subtree->A, context);
|
||||
MarkSubtree(subtree->B, context); // TODO: Force TCO here?
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Leaf Functions
|
||||
|
||||
static Node *
|
||||
LeafNew(cpBBTree *tree, void *obj, cpBB bb)
|
||||
{
|
||||
Node *node = NodeFromPool(tree);
|
||||
node->obj = obj;
|
||||
node->bb = GetBB(tree, obj);
|
||||
|
||||
node->parent = NULL;
|
||||
node->STAMP = 0;
|
||||
node->PAIRS = NULL;
|
||||
|
||||
return node;
|
||||
}
|
||||
|
||||
static cpBool
|
||||
LeafUpdate(Node *leaf, cpBBTree *tree)
|
||||
{
|
||||
Node *root = tree->root;
|
||||
cpBB bb = tree->spatialIndex.bbfunc(leaf->obj);
|
||||
|
||||
if(!cpBBContainsBB(leaf->bb, bb)){
|
||||
leaf->bb = GetBB(tree, leaf->obj);
|
||||
|
||||
root = SubtreeRemove(root, leaf, tree);
|
||||
tree->root = SubtreeInsert(root, leaf, tree);
|
||||
|
||||
PairsClear(leaf, tree);
|
||||
leaf->STAMP = GetMasterTree(tree)->stamp;
|
||||
|
||||
return cpTrue;
|
||||
} else {
|
||||
return cpFalse;
|
||||
}
|
||||
}
|
||||
|
||||
static cpCollisionID VoidQueryFunc(void *obj1, void *obj2, cpCollisionID id, void *data){return id;}
|
||||
|
||||
static void
|
||||
LeafAddPairs(Node *leaf, cpBBTree *tree)
|
||||
{
|
||||
cpSpatialIndex *dynamicIndex = tree->spatialIndex.dynamicIndex;
|
||||
if(dynamicIndex){
|
||||
Node *dynamicRoot = GetRootIfTree(dynamicIndex);
|
||||
if(dynamicRoot){
|
||||
cpBBTree *dynamicTree = GetTree(dynamicIndex);
|
||||
MarkContext context = {dynamicTree, NULL, NULL, NULL};
|
||||
MarkLeafQuery(dynamicRoot, leaf, cpTrue, &context);
|
||||
}
|
||||
} else {
|
||||
Node *staticRoot = GetRootIfTree(tree->spatialIndex.staticIndex);
|
||||
MarkContext context = {tree, staticRoot, VoidQueryFunc, NULL};
|
||||
MarkLeaf(leaf, &context);
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Memory Management Functions
|
||||
|
||||
cpBBTree *
|
||||
cpBBTreeAlloc(void)
|
||||
{
|
||||
return (cpBBTree *)cpcalloc(1, sizeof(cpBBTree));
|
||||
}
|
||||
|
||||
static int
|
||||
leafSetEql(void *obj, Node *node)
|
||||
{
|
||||
return (obj == node->obj);
|
||||
}
|
||||
|
||||
static void *
|
||||
leafSetTrans(void *obj, cpBBTree *tree)
|
||||
{
|
||||
return LeafNew(tree, obj, tree->spatialIndex.bbfunc(obj));
|
||||
}
|
||||
|
||||
cpSpatialIndex *
|
||||
cpBBTreeInit(cpBBTree *tree, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
|
||||
{
|
||||
cpSpatialIndexInit((cpSpatialIndex *)tree, Klass(), bbfunc, staticIndex);
|
||||
|
||||
tree->velocityFunc = NULL;
|
||||
|
||||
tree->leaves = cpHashSetNew(0, (cpHashSetEqlFunc)leafSetEql);
|
||||
tree->root = NULL;
|
||||
|
||||
tree->pooledNodes = NULL;
|
||||
tree->allocatedBuffers = cpArrayNew(0);
|
||||
|
||||
tree->stamp = 0;
|
||||
|
||||
return (cpSpatialIndex *)tree;
|
||||
}
|
||||
|
||||
void
|
||||
cpBBTreeSetVelocityFunc(cpSpatialIndex *index, cpBBTreeVelocityFunc func)
|
||||
{
|
||||
if(index->klass != Klass()){
|
||||
cpAssertWarn(cpFalse, "Ignoring cpBBTreeSetVelocityFunc() call to non-tree spatial index.");
|
||||
return;
|
||||
}
|
||||
|
||||
((cpBBTree *)index)->velocityFunc = func;
|
||||
}
|
||||
|
||||
cpSpatialIndex *
|
||||
cpBBTreeNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
|
||||
{
|
||||
return cpBBTreeInit(cpBBTreeAlloc(), bbfunc, staticIndex);
|
||||
}
|
||||
|
||||
static void
|
||||
cpBBTreeDestroy(cpBBTree *tree)
|
||||
{
|
||||
cpHashSetFree(tree->leaves);
|
||||
|
||||
if(tree->allocatedBuffers) cpArrayFreeEach(tree->allocatedBuffers, cpfree);
|
||||
cpArrayFree(tree->allocatedBuffers);
|
||||
}
|
||||
|
||||
//MARK: Insert/Remove
|
||||
|
||||
static void
|
||||
cpBBTreeInsert(cpBBTree *tree, void *obj, cpHashValue hashid)
|
||||
{
|
||||
Node *leaf = (Node *)cpHashSetInsert(tree->leaves, hashid, obj, (cpHashSetTransFunc)leafSetTrans, tree);
|
||||
|
||||
Node *root = tree->root;
|
||||
tree->root = SubtreeInsert(root, leaf, tree);
|
||||
|
||||
leaf->STAMP = GetMasterTree(tree)->stamp;
|
||||
LeafAddPairs(leaf, tree);
|
||||
IncrementStamp(tree);
|
||||
}
|
||||
|
||||
static void
|
||||
cpBBTreeRemove(cpBBTree *tree, void *obj, cpHashValue hashid)
|
||||
{
|
||||
Node *leaf = (Node *)cpHashSetRemove(tree->leaves, hashid, obj);
|
||||
|
||||
tree->root = SubtreeRemove(tree->root, leaf, tree);
|
||||
PairsClear(leaf, tree);
|
||||
NodeRecycle(tree, leaf);
|
||||
}
|
||||
|
||||
static cpBool
|
||||
cpBBTreeContains(cpBBTree *tree, void *obj, cpHashValue hashid)
|
||||
{
|
||||
return (cpHashSetFind(tree->leaves, hashid, obj) != NULL);
|
||||
}
|
||||
|
||||
//MARK: Reindex
|
||||
|
||||
static void LeafUpdateWrap(Node *leaf, cpBBTree *tree) {LeafUpdate(leaf, tree);}
|
||||
|
||||
static void
|
||||
cpBBTreeReindexQuery(cpBBTree *tree, cpSpatialIndexQueryFunc func, void *data)
|
||||
{
|
||||
if(!tree->root) return;
|
||||
|
||||
// LeafUpdate() may modify tree->root. Don't cache it.
|
||||
cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)LeafUpdateWrap, tree);
|
||||
|
||||
cpSpatialIndex *staticIndex = tree->spatialIndex.staticIndex;
|
||||
Node *staticRoot = (staticIndex && staticIndex->klass == Klass() ? ((cpBBTree *)staticIndex)->root : NULL);
|
||||
|
||||
MarkContext context = {tree, staticRoot, func, data};
|
||||
MarkSubtree(tree->root, &context);
|
||||
if(staticIndex && !staticRoot) cpSpatialIndexCollideStatic((cpSpatialIndex *)tree, staticIndex, func, data);
|
||||
|
||||
IncrementStamp(tree);
|
||||
}
|
||||
|
||||
static void
|
||||
cpBBTreeReindex(cpBBTree *tree)
|
||||
{
|
||||
cpBBTreeReindexQuery(tree, VoidQueryFunc, NULL);
|
||||
}
|
||||
|
||||
static void
|
||||
cpBBTreeReindexObject(cpBBTree *tree, void *obj, cpHashValue hashid)
|
||||
{
|
||||
Node *leaf = (Node *)cpHashSetFind(tree->leaves, hashid, obj);
|
||||
if(leaf){
|
||||
if(LeafUpdate(leaf, tree)) LeafAddPairs(leaf, tree);
|
||||
IncrementStamp(tree);
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Query
|
||||
|
||||
static void
|
||||
cpBBTreeSegmentQuery(cpBBTree *tree, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
|
||||
{
|
||||
Node *root = tree->root;
|
||||
if(root) SubtreeSegmentQuery(root, obj, a, b, t_exit, func, data);
|
||||
}
|
||||
|
||||
static void
|
||||
cpBBTreeQuery(cpBBTree *tree, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data)
|
||||
{
|
||||
if(tree->root) SubtreeQuery(tree->root, obj, bb, func, data);
|
||||
}
|
||||
|
||||
//MARK: Misc
|
||||
|
||||
static int
|
||||
cpBBTreeCount(cpBBTree *tree)
|
||||
{
|
||||
return cpHashSetCount(tree->leaves);
|
||||
}
|
||||
|
||||
typedef struct eachContext {
|
||||
cpSpatialIndexIteratorFunc func;
|
||||
void *data;
|
||||
} eachContext;
|
||||
|
||||
static void each_helper(Node *node, eachContext *context){context->func(node->obj, context->data);}
|
||||
|
||||
static void
|
||||
cpBBTreeEach(cpBBTree *tree, cpSpatialIndexIteratorFunc func, void *data)
|
||||
{
|
||||
eachContext context = {func, data};
|
||||
cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)each_helper, &context);
|
||||
}
|
||||
|
||||
static cpSpatialIndexClass klass = {
|
||||
(cpSpatialIndexDestroyImpl)cpBBTreeDestroy,
|
||||
|
||||
(cpSpatialIndexCountImpl)cpBBTreeCount,
|
||||
(cpSpatialIndexEachImpl)cpBBTreeEach,
|
||||
|
||||
(cpSpatialIndexContainsImpl)cpBBTreeContains,
|
||||
(cpSpatialIndexInsertImpl)cpBBTreeInsert,
|
||||
(cpSpatialIndexRemoveImpl)cpBBTreeRemove,
|
||||
|
||||
(cpSpatialIndexReindexImpl)cpBBTreeReindex,
|
||||
(cpSpatialIndexReindexObjectImpl)cpBBTreeReindexObject,
|
||||
(cpSpatialIndexReindexQueryImpl)cpBBTreeReindexQuery,
|
||||
|
||||
(cpSpatialIndexQueryImpl)cpBBTreeQuery,
|
||||
(cpSpatialIndexSegmentQueryImpl)cpBBTreeSegmentQuery,
|
||||
};
|
||||
|
||||
static inline cpSpatialIndexClass *Klass(){return &klass;}
|
||||
|
||||
|
||||
//MARK: Tree Optimization
|
||||
|
||||
static int
|
||||
cpfcompare(const cpFloat *a, const cpFloat *b){
|
||||
return (*a < *b ? -1 : (*b < *a ? 1 : 0));
|
||||
}
|
||||
|
||||
static void
|
||||
fillNodeArray(Node *node, Node ***cursor){
|
||||
(**cursor) = node;
|
||||
(*cursor)++;
|
||||
}
|
||||
|
||||
static Node *
|
||||
partitionNodes(cpBBTree *tree, Node **nodes, int count)
|
||||
{
|
||||
if(count == 1){
|
||||
return nodes[0];
|
||||
} else if(count == 2) {
|
||||
return NodeNew(tree, nodes[0], nodes[1]);
|
||||
}
|
||||
|
||||
// Find the AABB for these nodes
|
||||
cpBB bb = nodes[0]->bb;
|
||||
for(int i=1; i<count; i++) bb = cpBBMerge(bb, nodes[i]->bb);
|
||||
|
||||
// Split it on it's longest axis
|
||||
cpBool splitWidth = (bb.r - bb.l > bb.t - bb.b);
|
||||
|
||||
// Sort the bounds and use the median as the splitting point
|
||||
cpFloat *bounds = (cpFloat *)cpcalloc(count*2, sizeof(cpFloat));
|
||||
if(splitWidth){
|
||||
for(int i=0; i<count; i++){
|
||||
bounds[2*i + 0] = nodes[i]->bb.l;
|
||||
bounds[2*i + 1] = nodes[i]->bb.r;
|
||||
}
|
||||
} else {
|
||||
for(int i=0; i<count; i++){
|
||||
bounds[2*i + 0] = nodes[i]->bb.b;
|
||||
bounds[2*i + 1] = nodes[i]->bb.t;
|
||||
}
|
||||
}
|
||||
|
||||
qsort(bounds, count*2, sizeof(cpFloat), (int (*)(const void *, const void *))cpfcompare);
|
||||
cpFloat split = (bounds[count - 1] + bounds[count])*0.5f; // use the medain as the split
|
||||
cpfree(bounds);
|
||||
|
||||
// Generate the child BBs
|
||||
cpBB a = bb, b = bb;
|
||||
if(splitWidth) a.r = b.l = split; else a.t = b.b = split;
|
||||
|
||||
// Partition the nodes
|
||||
int right = count;
|
||||
for(int left=0; left < right;){
|
||||
Node *node = nodes[left];
|
||||
if(cpBBMergedArea(node->bb, b) < cpBBMergedArea(node->bb, a)){
|
||||
// if(cpBBProximity(node->bb, b) < cpBBProximity(node->bb, a)){
|
||||
right--;
|
||||
nodes[left] = nodes[right];
|
||||
nodes[right] = node;
|
||||
} else {
|
||||
left++;
|
||||
}
|
||||
}
|
||||
|
||||
if(right == count){
|
||||
Node *node = NULL;
|
||||
for(int i=0; i<count; i++) node = SubtreeInsert(node, nodes[i], tree);
|
||||
return node;
|
||||
}
|
||||
|
||||
// Recurse and build the node!
|
||||
return NodeNew(tree,
|
||||
partitionNodes(tree, nodes, right),
|
||||
partitionNodes(tree, nodes + right, count - right)
|
||||
);
|
||||
}
|
||||
|
||||
//static void
|
||||
//cpBBTreeOptimizeIncremental(cpBBTree *tree, int passes)
|
||||
//{
|
||||
// for(int i=0; i<passes; i++){
|
||||
// Node *root = tree->root;
|
||||
// Node *node = root;
|
||||
// int bit = 0;
|
||||
// unsigned int path = tree->opath;
|
||||
//
|
||||
// while(!NodeIsLeaf(node)){
|
||||
// node = (path&(1<<bit) ? node->a : node->b);
|
||||
// bit = (bit + 1)&(sizeof(unsigned int)*8 - 1);
|
||||
// }
|
||||
//
|
||||
// root = subtreeRemove(root, node, tree);
|
||||
// tree->root = subtreeInsert(root, node, tree);
|
||||
// }
|
||||
//}
|
||||
|
||||
void
|
||||
cpBBTreeOptimize(cpSpatialIndex *index)
|
||||
{
|
||||
if(index->klass != &klass){
|
||||
cpAssertWarn(cpFalse, "Ignoring cpBBTreeOptimize() call to non-tree spatial index.");
|
||||
return;
|
||||
}
|
||||
|
||||
cpBBTree *tree = (cpBBTree *)index;
|
||||
Node *root = tree->root;
|
||||
if(!root) return;
|
||||
|
||||
int count = cpBBTreeCount(tree);
|
||||
Node **nodes = (Node **)cpcalloc(count, sizeof(Node *));
|
||||
Node **cursor = nodes;
|
||||
|
||||
cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)fillNodeArray, &cursor);
|
||||
|
||||
SubtreeRecycle(tree, root);
|
||||
tree->root = partitionNodes(tree, nodes, count);
|
||||
cpfree(nodes);
|
||||
}
|
||||
|
||||
//MARK: Debug Draw
|
||||
|
||||
//#define CP_BBTREE_DEBUG_DRAW
|
||||
#ifdef CP_BBTREE_DEBUG_DRAW
|
||||
#include "OpenGL/gl.h"
|
||||
#include "OpenGL/glu.h"
|
||||
#include <GLUT/glut.h>
|
||||
|
||||
static void
|
||||
NodeRender(Node *node, int depth)
|
||||
{
|
||||
if(!NodeIsLeaf(node) && depth <= 10){
|
||||
NodeRender(node->a, depth + 1);
|
||||
NodeRender(node->b, depth + 1);
|
||||
}
|
||||
|
||||
cpBB bb = node->bb;
|
||||
|
||||
// GLfloat v = depth/2.0f;
|
||||
// glColor3f(1.0f - v, v, 0.0f);
|
||||
glLineWidth(cpfmax(5.0f - depth, 1.0f));
|
||||
glBegin(GL_LINES); {
|
||||
glVertex2f(bb.l, bb.b);
|
||||
glVertex2f(bb.l, bb.t);
|
||||
|
||||
glVertex2f(bb.l, bb.t);
|
||||
glVertex2f(bb.r, bb.t);
|
||||
|
||||
glVertex2f(bb.r, bb.t);
|
||||
glVertex2f(bb.r, bb.b);
|
||||
|
||||
glVertex2f(bb.r, bb.b);
|
||||
glVertex2f(bb.l, bb.b);
|
||||
}; glEnd();
|
||||
}
|
||||
|
||||
void
|
||||
cpBBTreeRenderDebug(cpSpatialIndex *index){
|
||||
if(index->klass != &klass){
|
||||
cpAssertWarn(cpFalse, "Ignoring cpBBTreeRenderDebug() call to non-tree spatial index.");
|
||||
return;
|
||||
}
|
||||
|
||||
cpBBTree *tree = (cpBBTree *)index;
|
||||
if(tree->root) NodeRender(tree->root, 0);
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,626 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 <float.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
#include "chipmunk/chipmunk_private.h"
|
||||
|
||||
cpBody*
|
||||
cpBodyAlloc(void)
|
||||
{
|
||||
return (cpBody *)cpcalloc(1, sizeof(cpBody));
|
||||
}
|
||||
|
||||
cpBody *
|
||||
cpBodyInit(cpBody *body, cpFloat mass, cpFloat moment)
|
||||
{
|
||||
body->space = NULL;
|
||||
body->shapeList = NULL;
|
||||
body->arbiterList = NULL;
|
||||
body->constraintList = NULL;
|
||||
|
||||
body->velocity_func = cpBodyUpdateVelocity;
|
||||
body->position_func = cpBodyUpdatePosition;
|
||||
|
||||
body->sleeping.root = NULL;
|
||||
body->sleeping.next = NULL;
|
||||
body->sleeping.idleTime = 0.0f;
|
||||
|
||||
body->p = cpvzero;
|
||||
body->v = cpvzero;
|
||||
body->f = cpvzero;
|
||||
|
||||
body->w = 0.0f;
|
||||
body->t = 0.0f;
|
||||
|
||||
body->v_bias = cpvzero;
|
||||
body->w_bias = 0.0f;
|
||||
|
||||
body->userData = NULL;
|
||||
|
||||
// Setters must be called after full initialization so the sanity checks don't assert on garbage data.
|
||||
cpBodySetMass(body, mass);
|
||||
cpBodySetMoment(body, moment);
|
||||
cpBodySetAngle(body, 0.0f);
|
||||
|
||||
return body;
|
||||
}
|
||||
|
||||
cpBody*
|
||||
cpBodyNew(cpFloat mass, cpFloat moment)
|
||||
{
|
||||
return cpBodyInit(cpBodyAlloc(), mass, moment);
|
||||
}
|
||||
|
||||
cpBody*
|
||||
cpBodyNewKinematic()
|
||||
{
|
||||
cpBody *body = cpBodyNew(0.0f, 0.0f);
|
||||
cpBodySetType(body, CP_BODY_TYPE_KINEMATIC);
|
||||
|
||||
return body;
|
||||
}
|
||||
|
||||
cpBody*
|
||||
cpBodyNewStatic()
|
||||
{
|
||||
cpBody *body = cpBodyNew(0.0f, 0.0f);
|
||||
cpBodySetType(body, CP_BODY_TYPE_STATIC);
|
||||
|
||||
return body;
|
||||
}
|
||||
|
||||
void cpBodyDestroy(cpBody *body){}
|
||||
|
||||
void
|
||||
cpBodyFree(cpBody *body)
|
||||
{
|
||||
if(body){
|
||||
cpBodyDestroy(body);
|
||||
cpfree(body);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef NDEBUG
|
||||
#define cpAssertSaneBody(body)
|
||||
#else
|
||||
static void cpv_assert_nan(cpVect v, char *message){cpAssertHard(v.x == v.x && v.y == v.y, message);}
|
||||
static void cpv_assert_infinite(cpVect v, char *message){cpAssertHard(cpfabs(v.x) != INFINITY && cpfabs(v.y) != INFINITY, message);}
|
||||
static void cpv_assert_sane(cpVect v, char *message){cpv_assert_nan(v, message); cpv_assert_infinite(v, message);}
|
||||
|
||||
static void
|
||||
cpBodySanityCheck(const cpBody *body)
|
||||
{
|
||||
cpAssertHard(body->m == body->m && body->m_inv == body->m_inv, "Body's mass is NaN.");
|
||||
cpAssertHard(body->i == body->i && body->i_inv == body->i_inv, "Body's moment is NaN.");
|
||||
cpAssertHard(body->m >= 0.0f, "Body's mass is negative.");
|
||||
cpAssertHard(body->i >= 0.0f, "Body's moment is negative.");
|
||||
|
||||
cpv_assert_sane(body->p, "Body's position is invalid.");
|
||||
cpv_assert_sane(body->v, "Body's velocity is invalid.");
|
||||
cpv_assert_sane(body->f, "Body's force is invalid.");
|
||||
|
||||
cpAssertHard(body->a == body->a && cpfabs(body->a) != INFINITY, "Body's angle is invalid.");
|
||||
cpAssertHard(body->w == body->w && cpfabs(body->w) != INFINITY, "Body's angular velocity is invalid.");
|
||||
cpAssertHard(body->t == body->t && cpfabs(body->t) != INFINITY, "Body's torque is invalid.");
|
||||
}
|
||||
|
||||
#define cpAssertSaneBody(body) cpBodySanityCheck(body)
|
||||
#endif
|
||||
|
||||
cpBool
|
||||
cpBodyIsSleeping(const cpBody *body)
|
||||
{
|
||||
return (body->sleeping.root != ((cpBody*)0));
|
||||
}
|
||||
|
||||
cpBodyType
|
||||
cpBodyGetType(cpBody *body)
|
||||
{
|
||||
if(body->sleeping.idleTime == INFINITY){
|
||||
return CP_BODY_TYPE_STATIC;
|
||||
} else if(body->m == INFINITY){
|
||||
return CP_BODY_TYPE_KINEMATIC;
|
||||
} else {
|
||||
return CP_BODY_TYPE_DYNAMIC;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetType(cpBody *body, cpBodyType type)
|
||||
{
|
||||
cpBodyType oldType = cpBodyGetType(body);
|
||||
if(oldType == type) return;
|
||||
|
||||
// Static bodies have their idle timers set to infinity.
|
||||
// Non-static bodies should have their idle timer reset.
|
||||
body->sleeping.idleTime = (type == CP_BODY_TYPE_STATIC ? INFINITY : 0.0f);
|
||||
|
||||
if(type == CP_BODY_TYPE_DYNAMIC){
|
||||
body->m = body->i = 0.0f;
|
||||
body->m_inv = body->i_inv = INFINITY;
|
||||
|
||||
cpBodyAccumulateMassFromShapes(body);
|
||||
} else {
|
||||
body->m = body->i = INFINITY;
|
||||
body->m_inv = body->i_inv = 0.0f;
|
||||
|
||||
body->v = cpvzero;
|
||||
body->w = 0.0f;
|
||||
}
|
||||
|
||||
// If the body is added to a space already, we'll need to update some space data structures.
|
||||
cpSpace *space = cpBodyGetSpace(body);
|
||||
if(space != NULL){
|
||||
cpAssertSpaceUnlocked(space);
|
||||
|
||||
if(oldType == CP_BODY_TYPE_STATIC){
|
||||
// TODO This is probably not necessary
|
||||
// cpBodyActivateStatic(body, NULL);
|
||||
} else {
|
||||
cpBodyActivate(body);
|
||||
}
|
||||
|
||||
// Move the bodies to the correct array.
|
||||
cpArray *fromArray = cpSpaceArrayForBodyType(space, oldType);
|
||||
cpArray *toArray = cpSpaceArrayForBodyType(space, type);
|
||||
if(fromArray != toArray){
|
||||
cpArrayDeleteObj(fromArray, body);
|
||||
cpArrayPush(toArray, body);
|
||||
}
|
||||
|
||||
// Move the body's shapes to the correct spatial index.
|
||||
cpSpatialIndex *fromIndex = (oldType == CP_BODY_TYPE_STATIC ? space->staticShapes : space->dynamicShapes);
|
||||
cpSpatialIndex *toIndex = (type == CP_BODY_TYPE_STATIC ? space->staticShapes : space->dynamicShapes);
|
||||
if(fromIndex != toIndex){
|
||||
CP_BODY_FOREACH_SHAPE(body, shape){
|
||||
cpSpatialIndexRemove(fromIndex, shape, shape->hashid);
|
||||
cpSpatialIndexInsert(toIndex, shape, shape->hashid);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Should *only* be called when shapes with mass info are modified, added or removed.
|
||||
void
|
||||
cpBodyAccumulateMassFromShapes(cpBody *body)
|
||||
{
|
||||
if(body == NULL || cpBodyGetType(body) != CP_BODY_TYPE_DYNAMIC) return;
|
||||
|
||||
// Reset the body's mass data.
|
||||
body->m = body->i = 0.0f;
|
||||
body->cog = cpvzero;
|
||||
|
||||
// Cache the position to realign it at the end.
|
||||
cpVect pos = cpBodyGetPosition(body);
|
||||
|
||||
// Accumulate mass from shapes.
|
||||
CP_BODY_FOREACH_SHAPE(body, shape){
|
||||
struct cpShapeMassInfo *info = &shape->massInfo;
|
||||
cpFloat m = info->m;
|
||||
|
||||
if(m > 0.0f){
|
||||
cpFloat msum = body->m + m;
|
||||
|
||||
body->i += m*info->i + cpvdistsq(body->cog, info->cog)*(m*body->m)/msum;
|
||||
body->cog = cpvlerp(body->cog, info->cog, m/msum);
|
||||
body->m = msum;
|
||||
}
|
||||
}
|
||||
|
||||
// Recalculate the inverses.
|
||||
body->m_inv = 1.0f/body->m;
|
||||
body->i_inv = 1.0f/body->i;
|
||||
|
||||
// Realign the body since the CoG has probably moved.
|
||||
cpBodySetPosition(body, pos);
|
||||
cpAssertSaneBody(body);
|
||||
}
|
||||
|
||||
cpSpace *
|
||||
cpBodyGetSpace(const cpBody *body)
|
||||
{
|
||||
return body->space;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpBodyGetMass(const cpBody *body)
|
||||
{
|
||||
return body->m;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetMass(cpBody *body, cpFloat mass)
|
||||
{
|
||||
cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "You cannot set the mass of kinematic or static bodies.");
|
||||
cpAssertHard(0.0f <= mass && mass < INFINITY, "Mass must be positive and finite.");
|
||||
|
||||
cpBodyActivate(body);
|
||||
body->m = mass;
|
||||
body->m_inv = 1.0f/mass;
|
||||
cpAssertSaneBody(body);
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpBodyGetMoment(const cpBody *body)
|
||||
{
|
||||
return body->i;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetMoment(cpBody *body, cpFloat moment)
|
||||
{
|
||||
cpAssertHard(moment >= 0.0f, "Moment of Inertia must be positive.");
|
||||
|
||||
cpBodyActivate(body);
|
||||
body->i = moment;
|
||||
body->i_inv = 1.0f/moment;
|
||||
cpAssertSaneBody(body);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpBodyGetRotation(const cpBody *body)
|
||||
{
|
||||
return cpv(body->transform.a, body->transform.b);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyAddShape(cpBody *body, cpShape *shape)
|
||||
{
|
||||
cpShape *next = body->shapeList;
|
||||
if(next) next->prev = shape;
|
||||
|
||||
shape->next = next;
|
||||
body->shapeList = shape;
|
||||
|
||||
if(shape->massInfo.m > 0.0f){
|
||||
cpBodyAccumulateMassFromShapes(body);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyRemoveShape(cpBody *body, cpShape *shape)
|
||||
{
|
||||
cpShape *prev = shape->prev;
|
||||
cpShape *next = shape->next;
|
||||
|
||||
if(prev){
|
||||
prev->next = next;
|
||||
} else {
|
||||
body->shapeList = next;
|
||||
}
|
||||
|
||||
if(next){
|
||||
next->prev = prev;
|
||||
}
|
||||
|
||||
shape->prev = NULL;
|
||||
shape->next = NULL;
|
||||
|
||||
if(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC && shape->massInfo.m > 0.0f){
|
||||
cpBodyAccumulateMassFromShapes(body);
|
||||
}
|
||||
}
|
||||
|
||||
static cpConstraint *
|
||||
filterConstraints(cpConstraint *node, cpBody *body, cpConstraint *filter)
|
||||
{
|
||||
if(node == filter){
|
||||
return cpConstraintNext(node, body);
|
||||
} else if(node->a == body){
|
||||
node->next_a = filterConstraints(node->next_a, body, filter);
|
||||
} else {
|
||||
node->next_b = filterConstraints(node->next_b, body, filter);
|
||||
}
|
||||
|
||||
return node;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint)
|
||||
{
|
||||
body->constraintList = filterConstraints(body->constraintList, body, constraint);
|
||||
}
|
||||
|
||||
// 'p' is the position of the CoG
|
||||
static void
|
||||
SetTransform(cpBody *body, cpVect p, cpFloat a)
|
||||
{
|
||||
cpVect rot = cpvforangle(a);
|
||||
cpVect c = body->cog;
|
||||
|
||||
body->transform = cpTransformNewTranspose(
|
||||
rot.x, -rot.y, p.x - (c.x*rot.x - c.y*rot.y),
|
||||
rot.y, rot.x, p.y - (c.x*rot.y + c.y*rot.x)
|
||||
);
|
||||
}
|
||||
|
||||
static inline cpFloat
|
||||
SetAngle(cpBody *body, cpFloat a)
|
||||
{
|
||||
body->a = a;
|
||||
cpAssertSaneBody(body);
|
||||
|
||||
return a;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpBodyGetPosition(const cpBody *body)
|
||||
{
|
||||
return cpTransformPoint(body->transform, cpvzero);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetPosition(cpBody *body, cpVect position)
|
||||
{
|
||||
cpBodyActivate(body);
|
||||
cpVect p = body->p = cpvadd(cpTransformVect(body->transform, body->cog), position);
|
||||
cpAssertSaneBody(body);
|
||||
|
||||
SetTransform(body, p, body->a);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpBodyGetCenterOfGravity(const cpBody *body)
|
||||
{
|
||||
return body->cog;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetCenterOfGravity(cpBody *body, cpVect cog)
|
||||
{
|
||||
cpBodyActivate(body);
|
||||
body->cog = cog;
|
||||
cpAssertSaneBody(body);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpBodyGetVelocity(const cpBody *body)
|
||||
{
|
||||
return body->v;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetVelocity(cpBody *body, cpVect velocity)
|
||||
{
|
||||
cpBodyActivate(body);
|
||||
body->v = velocity;
|
||||
cpAssertSaneBody(body);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpBodyGetForce(const cpBody *body)
|
||||
{
|
||||
return body->f;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetForce(cpBody *body, cpVect force)
|
||||
{
|
||||
cpBodyActivate(body);
|
||||
body->f = force;
|
||||
cpAssertSaneBody(body);
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpBodyGetAngle(const cpBody *body)
|
||||
{
|
||||
return body->a;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetAngle(cpBody *body, cpFloat angle)
|
||||
{
|
||||
cpBodyActivate(body);
|
||||
SetAngle(body, angle);
|
||||
|
||||
SetTransform(body, body->p, angle);
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpBodyGetAngularVelocity(const cpBody *body)
|
||||
{
|
||||
return body->w;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetAngularVelocity(cpBody *body, cpFloat angularVelocity)
|
||||
{
|
||||
cpBodyActivate(body);
|
||||
body->w = angularVelocity;
|
||||
cpAssertSaneBody(body);
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpBodyGetTorque(const cpBody *body)
|
||||
{
|
||||
return body->t;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetTorque(cpBody *body, cpFloat torque)
|
||||
{
|
||||
cpBodyActivate(body);
|
||||
body->t = torque;
|
||||
cpAssertSaneBody(body);
|
||||
}
|
||||
|
||||
cpDataPointer
|
||||
cpBodyGetUserData(const cpBody *body)
|
||||
{
|
||||
return body->userData;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetUserData(cpBody *body, cpDataPointer userData)
|
||||
{
|
||||
body->userData = userData;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetVelocityUpdateFunc(cpBody *body, cpBodyVelocityFunc velocityFunc)
|
||||
{
|
||||
body->velocity_func = velocityFunc;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySetPositionUpdateFunc(cpBody *body, cpBodyPositionFunc positionFunc)
|
||||
{
|
||||
body->position_func = positionFunc;
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
|
||||
{
|
||||
// Skip kinematic bodies.
|
||||
if(cpBodyGetType(body) == CP_BODY_TYPE_KINEMATIC) return;
|
||||
|
||||
cpAssertSoft(body->m > 0.0f && body->i > 0.0f, "Body's mass and moment must be positive to simulate. (Mass: %f Moment: %f)", body->m, body->i);
|
||||
|
||||
body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt));
|
||||
body->w = body->w*damping + body->t*body->i_inv*dt;
|
||||
|
||||
// Reset forces.
|
||||
body->f = cpvzero;
|
||||
body->t = 0.0f;
|
||||
|
||||
cpAssertSaneBody(body);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyUpdatePosition(cpBody *body, cpFloat dt)
|
||||
{
|
||||
cpVect p = body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt));
|
||||
cpFloat a = SetAngle(body, body->a + (body->w + body->w_bias)*dt);
|
||||
SetTransform(body, p, a);
|
||||
|
||||
body->v_bias = cpvzero;
|
||||
body->w_bias = 0.0f;
|
||||
|
||||
cpAssertSaneBody(body);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpBodyLocalToWorld(const cpBody *body, const cpVect point)
|
||||
{
|
||||
return cpTransformPoint(body->transform, point);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpBodyWorldToLocal(const cpBody *body, const cpVect point)
|
||||
{
|
||||
return cpTransformPoint(cpTransformRigidInverse(body->transform), point);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyApplyForceAtWorldPoint(cpBody *body, cpVect force, cpVect point)
|
||||
{
|
||||
cpBodyActivate(body);
|
||||
body->f = cpvadd(body->f, force);
|
||||
|
||||
cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog));
|
||||
body->t += cpvcross(r, force);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyApplyForceAtLocalPoint(cpBody *body, cpVect force, cpVect point)
|
||||
{
|
||||
cpBodyApplyForceAtWorldPoint(body, cpTransformVect(body->transform, force), cpTransformPoint(body->transform, point));
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyApplyImpulseAtWorldPoint(cpBody *body, cpVect impulse, cpVect point)
|
||||
{
|
||||
cpBodyActivate(body);
|
||||
|
||||
cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog));
|
||||
apply_impulse(body, impulse, r);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyApplyImpulseAtLocalPoint(cpBody *body, cpVect impulse, cpVect point)
|
||||
{
|
||||
cpBodyApplyImpulseAtWorldPoint(body, cpTransformVect(body->transform, impulse), cpTransformPoint(body->transform, point));
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpBodyGetVelocityAtLocalPoint(const cpBody *body, cpVect point)
|
||||
{
|
||||
cpVect r = cpTransformVect(body->transform, cpvsub(point, body->cog));
|
||||
return cpvadd(body->v, cpvmult(cpvperp(r), body->w));
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpBodyGetVelocityAtWorldPoint(const cpBody *body, cpVect point)
|
||||
{
|
||||
cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog));
|
||||
return cpvadd(body->v, cpvmult(cpvperp(r), body->w));
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpBodyKineticEnergy(const cpBody *body)
|
||||
{
|
||||
// Need to do some fudging to avoid NaNs
|
||||
cpFloat vsq = cpvdot(body->v, body->v);
|
||||
cpFloat wsq = body->w*body->w;
|
||||
return (vsq ? vsq*body->m : 0.0f) + (wsq ? wsq*body->i : 0.0f);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data)
|
||||
{
|
||||
cpShape *shape = body->shapeList;
|
||||
while(shape){
|
||||
cpShape *next = shape->next;
|
||||
func(body, shape, data);
|
||||
shape = next;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyEachConstraint(cpBody *body, cpBodyConstraintIteratorFunc func, void *data)
|
||||
{
|
||||
cpConstraint *constraint = body->constraintList;
|
||||
while(constraint){
|
||||
cpConstraint *next = cpConstraintNext(constraint, body);
|
||||
func(body, constraint, data);
|
||||
constraint = next;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyEachArbiter(cpBody *body, cpBodyArbiterIteratorFunc func, void *data)
|
||||
{
|
||||
cpArbiter *arb = body->arbiterList;
|
||||
while(arb){
|
||||
cpArbiter *next = cpArbiterNext(arb, body);
|
||||
|
||||
cpBool swapped = arb->swapped; {
|
||||
arb->swapped = (body == arb->body_b);
|
||||
func(body, arb, data);
|
||||
} arb->swapped = swapped;
|
||||
|
||||
arb = next;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,726 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
#include "chipmunk/cpRobust.h"
|
||||
|
||||
#if DEBUG && 0
|
||||
#include "ChipmunkDemo.h"
|
||||
#define DRAW_ALL 0
|
||||
#define DRAW_GJK (0 || DRAW_ALL)
|
||||
#define DRAW_EPA (0 || DRAW_ALL)
|
||||
#define DRAW_CLOSEST (0 || DRAW_ALL)
|
||||
#define DRAW_CLIP (0 || DRAW_ALL)
|
||||
|
||||
#define PRINT_LOG 0
|
||||
#endif
|
||||
|
||||
#define MAX_GJK_ITERATIONS 30
|
||||
#define MAX_EPA_ITERATIONS 30
|
||||
#define WARN_GJK_ITERATIONS 20
|
||||
#define WARN_EPA_ITERATIONS 20
|
||||
|
||||
static inline void
|
||||
cpCollisionInfoPushContact(struct cpCollisionInfo *info, cpVect p1, cpVect p2, cpHashValue hash)
|
||||
{
|
||||
cpAssertSoft(info->count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal error: Tried to push too many contacts.");
|
||||
|
||||
struct cpContact *con = &info->arr[info->count];
|
||||
con->r1 = p1;
|
||||
con->r2 = p2;
|
||||
con->hash = hash;
|
||||
|
||||
info->count++;
|
||||
}
|
||||
|
||||
//MARK: Support Points and Edges:
|
||||
|
||||
// Support points are the maximal points on a shape's perimeter along a certain axis.
|
||||
// The GJK and EPA algorithms use support points to iteratively sample the surface of the two shapes' minkowski difference.
|
||||
|
||||
static inline int
|
||||
PolySupportPointIndex(const int count, const struct cpSplittingPlane *planes, const cpVect n)
|
||||
{
|
||||
cpFloat max = -INFINITY;
|
||||
int index = 0;
|
||||
|
||||
for(int i=0; i<count; i++){
|
||||
cpVect v = planes[i].v0;
|
||||
cpFloat d = cpvdot(v, n);
|
||||
if(d > max){
|
||||
max = d;
|
||||
index = i;
|
||||
}
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
struct SupportPoint {
|
||||
cpVect p;
|
||||
// Save an index of the point so it can be cheaply looked up as a starting point for the next frame.
|
||||
cpCollisionID index;
|
||||
};
|
||||
|
||||
static inline struct SupportPoint
|
||||
SupportPointNew(cpVect p, cpCollisionID index)
|
||||
{
|
||||
struct SupportPoint point = {p, index};
|
||||
return point;
|
||||
}
|
||||
|
||||
typedef struct SupportPoint (*SupportPointFunc)(const cpShape *shape, const cpVect n);
|
||||
|
||||
static inline struct SupportPoint
|
||||
CircleSupportPoint(const cpCircleShape *circle, const cpVect n)
|
||||
{
|
||||
return SupportPointNew(circle->tc, 0);
|
||||
}
|
||||
|
||||
static inline struct SupportPoint
|
||||
SegmentSupportPoint(const cpSegmentShape *seg, const cpVect n)
|
||||
{
|
||||
if(cpvdot(seg->ta, n) > cpvdot(seg->tb, n)){
|
||||
return SupportPointNew(seg->ta, 0);
|
||||
} else {
|
||||
return SupportPointNew(seg->tb, 1);
|
||||
}
|
||||
}
|
||||
|
||||
static inline struct SupportPoint
|
||||
PolySupportPoint(const cpPolyShape *poly, const cpVect n)
|
||||
{
|
||||
const struct cpSplittingPlane *planes = poly->planes;
|
||||
int i = PolySupportPointIndex(poly->count, planes, n);
|
||||
return SupportPointNew(planes[i].v0, i);
|
||||
}
|
||||
|
||||
// A point on the surface of two shape's minkowski difference.
|
||||
struct MinkowskiPoint {
|
||||
// Cache the two original support points.
|
||||
cpVect a, b;
|
||||
// b - a
|
||||
cpVect ab;
|
||||
// Concatenate the two support point indexes.
|
||||
cpCollisionID id;
|
||||
};
|
||||
|
||||
static inline struct MinkowskiPoint
|
||||
MinkowskiPointNew(const struct SupportPoint a, const struct SupportPoint b)
|
||||
{
|
||||
struct MinkowskiPoint point = {a.p, b.p, cpvsub(b.p, a.p), (a.index & 0xFF)<<8 | (b.index & 0xFF)};
|
||||
return point;
|
||||
}
|
||||
|
||||
struct SupportContext {
|
||||
const cpShape *shape1, *shape2;
|
||||
SupportPointFunc func1, func2;
|
||||
};
|
||||
|
||||
// Calculate the maximal point on the minkowski difference of two shapes along a particular axis.
|
||||
static inline struct MinkowskiPoint
|
||||
Support(const struct SupportContext *ctx, const cpVect n)
|
||||
{
|
||||
struct SupportPoint a = ctx->func1(ctx->shape1, cpvneg(n));
|
||||
struct SupportPoint b = ctx->func2(ctx->shape2, n);
|
||||
return MinkowskiPointNew(a, b);
|
||||
}
|
||||
|
||||
struct EdgePoint {
|
||||
cpVect p;
|
||||
// Keep a hash value for Chipmunk's collision hashing mechanism.
|
||||
cpHashValue hash;
|
||||
};
|
||||
|
||||
// Support edges are the edges of a polygon or segment shape that are in contact.
|
||||
struct Edge {
|
||||
struct EdgePoint a, b;
|
||||
cpFloat r;
|
||||
cpVect n;
|
||||
};
|
||||
|
||||
static struct Edge
|
||||
SupportEdgeForPoly(const cpPolyShape *poly, const cpVect n)
|
||||
{
|
||||
int count = poly->count;
|
||||
int i1 = PolySupportPointIndex(poly->count, poly->planes, n);
|
||||
|
||||
// TODO: get rid of mod eventually, very expensive on ARM
|
||||
int i0 = (i1 - 1 + count)%count;
|
||||
int i2 = (i1 + 1)%count;
|
||||
|
||||
const struct cpSplittingPlane *planes = poly->planes;
|
||||
cpHashValue hashid = poly->shape.hashid;
|
||||
if(cpvdot(n, planes[i1].n) > cpvdot(n, planes[i2].n)){
|
||||
struct Edge edge = {{planes[i0].v0, CP_HASH_PAIR(hashid, i0)}, {planes[i1].v0, CP_HASH_PAIR(hashid, i1)}, poly->r, planes[i1].n};
|
||||
return edge;
|
||||
} else {
|
||||
struct Edge edge = {{planes[i1].v0, CP_HASH_PAIR(hashid, i1)}, {planes[i2].v0, CP_HASH_PAIR(hashid, i2)}, poly->r, planes[i2].n};
|
||||
return edge;
|
||||
}
|
||||
}
|
||||
|
||||
static struct Edge
|
||||
SupportEdgeForSegment(const cpSegmentShape *seg, const cpVect n)
|
||||
{
|
||||
cpHashValue hashid = seg->shape.hashid;
|
||||
if(cpvdot(seg->tn, n) > 0.0){
|
||||
struct Edge edge = {{seg->ta, CP_HASH_PAIR(hashid, 0)}, {seg->tb, CP_HASH_PAIR(hashid, 1)}, seg->r, seg->tn};
|
||||
return edge;
|
||||
} else {
|
||||
struct Edge edge = {{seg->tb, CP_HASH_PAIR(hashid, 1)}, {seg->ta, CP_HASH_PAIR(hashid, 0)}, seg->r, cpvneg(seg->tn)};
|
||||
return edge;
|
||||
}
|
||||
}
|
||||
|
||||
// Find the closest p(t) to (0, 0) where p(t) = a*(1-t)/2 + b*(1+t)/2
|
||||
// The range for t is [-1, 1] to avoid floating point issues if the parameters are swapped.
|
||||
static inline cpFloat
|
||||
ClosestT(const cpVect a, const cpVect b)
|
||||
{
|
||||
cpVect delta = cpvsub(b, a);
|
||||
return -cpfclamp(cpvdot(delta, cpvadd(a, b))/cpvlengthsq(delta), -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
// Basically the same as cpvlerp(), except t = [-1, 1]
|
||||
static inline cpVect
|
||||
LerpT(const cpVect a, const cpVect b, const cpFloat t)
|
||||
{
|
||||
cpFloat ht = 0.5f*t;
|
||||
return cpvadd(cpvmult(a, 0.5f - ht), cpvmult(b, 0.5f + ht));
|
||||
}
|
||||
|
||||
// Closest points on the surface of two shapes.
|
||||
struct ClosestPoints {
|
||||
// Surface points in absolute coordinates.
|
||||
cpVect a, b;
|
||||
// Minimum separating axis of the two shapes.
|
||||
cpVect n;
|
||||
// Signed distance between the points.
|
||||
cpFloat d;
|
||||
// Concatenation of the id's of the minkoski points.
|
||||
cpCollisionID id;
|
||||
};
|
||||
|
||||
// Calculate the closest points on two shapes given the closest edge on their minkowski difference to (0, 0)
|
||||
static inline struct ClosestPoints
|
||||
ClosestPointsNew(const struct MinkowskiPoint v0, const struct MinkowskiPoint v1)
|
||||
{
|
||||
// Find the closest p(t) on the minkowski difference to (0, 0)
|
||||
cpFloat t = ClosestT(v0.ab, v1.ab);
|
||||
cpVect p = LerpT(v0.ab, v1.ab, t);
|
||||
|
||||
// Interpolate the original support points using the same 't' value as above.
|
||||
// This gives you the closest surface points in absolute coordinates. NEAT!
|
||||
cpVect pa = LerpT(v0.a, v1.a, t);
|
||||
cpVect pb = LerpT(v0.b, v1.b, t);
|
||||
cpCollisionID id = (v0.id & 0xFFFF)<<16 | (v1.id & 0xFFFF);
|
||||
|
||||
// First try calculating the MSA from the minkowski difference edge.
|
||||
// This gives us a nice, accurate MSA when the surfaces are close together.
|
||||
cpVect delta = cpvsub(v1.ab, v0.ab);
|
||||
cpVect n = cpvnormalize(cpvrperp(delta));
|
||||
cpFloat d = cpvdot(n, p);
|
||||
|
||||
if(d <= 0.0f || (-1.0f < t && t < 1.0f)){
|
||||
// If the shapes are overlapping, or we have a regular vertex/edge collision, we are done.
|
||||
struct ClosestPoints points = {pa, pb, n, d, id};
|
||||
return points;
|
||||
} else {
|
||||
// Vertex/vertex collisions need special treatment since the MSA won't be shared with an axis of the minkowski difference.
|
||||
cpFloat d2 = cpvlength(p);
|
||||
cpVect n2 = cpvmult(p, 1.0f/(d2 + CPFLOAT_MIN));
|
||||
|
||||
struct ClosestPoints points = {pa, pb, n2, d2, id};
|
||||
return points;
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: EPA Functions
|
||||
|
||||
static inline cpFloat
|
||||
ClosestDist(const cpVect v0,const cpVect v1)
|
||||
{
|
||||
return cpvlengthsq(LerpT(v0, v1, ClosestT(v0, v1)));
|
||||
}
|
||||
|
||||
// Recursive implementation of the EPA loop.
|
||||
// Each recursion adds a point to the convex hull until it's known that we have the closest point on the surface.
|
||||
static struct ClosestPoints
|
||||
EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
|
||||
{
|
||||
int mini = 0;
|
||||
cpFloat minDist = INFINITY;
|
||||
|
||||
// TODO: precalculate this when building the hull and save a step.
|
||||
// Find the closest segment hull[i] and hull[i + 1] to (0, 0)
|
||||
for(int j=0, i=count-1; j<count; i=j, j++){
|
||||
cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
|
||||
if(d < minDist){
|
||||
minDist = d;
|
||||
mini = i;
|
||||
}
|
||||
}
|
||||
|
||||
struct MinkowskiPoint v0 = hull[mini];
|
||||
struct MinkowskiPoint v1 = hull[(mini + 1)%count];
|
||||
cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);
|
||||
|
||||
// Check if there is a point on the minkowski difference beyond this edge.
|
||||
struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));
|
||||
|
||||
#if DRAW_EPA
|
||||
cpVect verts[count];
|
||||
for(int i=0; i<count; i++) verts[i] = hull[i].ab;
|
||||
|
||||
ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
|
||||
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));
|
||||
|
||||
ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
|
||||
#endif
|
||||
|
||||
// The usual exit condition is a duplicated vertex.
|
||||
// Much faster to check the ids than to check the signed area.
|
||||
cpBool duplicate = (p.id == v0.id || p.id == v1.id);
|
||||
|
||||
if(!duplicate && cpCheckPointGreater(v0.ab, v1.ab, p.ab) && iteration < MAX_EPA_ITERATIONS){
|
||||
// Rebuild the convex hull by inserting p.
|
||||
struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
|
||||
int count2 = 1;
|
||||
hull2[0] = p;
|
||||
|
||||
for(int i=0; i<count; i++){
|
||||
int index = (mini + 1 + i)%count;
|
||||
|
||||
cpVect h0 = hull2[count2 - 1].ab;
|
||||
cpVect h1 = hull[index].ab;
|
||||
cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;
|
||||
|
||||
if(cpCheckPointGreater(h0, h2, h1)){
|
||||
hull2[count2] = hull[index];
|
||||
count2++;
|
||||
}
|
||||
}
|
||||
|
||||
return EPARecurse(ctx, count2, hull2, iteration + 1);
|
||||
} else {
|
||||
// Could not find a new point to insert, so we have found the closest edge of the minkowski difference.
|
||||
cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
|
||||
return ClosestPointsNew(v0, v1);
|
||||
}
|
||||
}
|
||||
|
||||
// Find the closest points on the surface of two overlapping shapes using the EPA algorithm.
|
||||
// EPA is called from GJK when two shapes overlap.
|
||||
// This is a moderately expensive step! Avoid it by adding radii to your shapes so their inner polygons won't overlap.
|
||||
static struct ClosestPoints
|
||||
EPA(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const struct MinkowskiPoint v2)
|
||||
{
|
||||
// TODO: allocate a NxM array here and do an in place convex hull reduction in EPARecurse?
|
||||
struct MinkowskiPoint hull[3] = {v0, v1, v2};
|
||||
return EPARecurse(ctx, 3, hull, 1);
|
||||
}
|
||||
|
||||
//MARK: GJK Functions.
|
||||
|
||||
// Recursive implementation of the GJK loop.
|
||||
static inline struct ClosestPoints
|
||||
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
|
||||
{
|
||||
if(iteration > MAX_GJK_ITERATIONS){
|
||||
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
|
||||
return ClosestPointsNew(v0, v1);
|
||||
}
|
||||
|
||||
if(cpCheckPointGreater(v1.ab, v0.ab, cpvzero)){
|
||||
// Origin is behind axis. Flip and try again.
|
||||
return GJKRecurse(ctx, v1, v0, iteration);
|
||||
} else {
|
||||
cpFloat t = ClosestT(v0.ab, v1.ab);
|
||||
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(cpvsub(v1.ab, v0.ab)) : cpvneg(LerpT(v0.ab, v1.ab, t)));
|
||||
struct MinkowskiPoint p = Support(ctx, n);
|
||||
|
||||
#if DRAW_GJK
|
||||
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
|
||||
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
|
||||
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
|
||||
|
||||
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
|
||||
#endif
|
||||
|
||||
if(cpCheckPointGreater(p.ab, v0.ab, cpvzero) && cpCheckPointGreater(v1.ab, p.ab, cpvzero)){
|
||||
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
|
||||
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
|
||||
return EPA(ctx, v0, p, v1);
|
||||
} else {
|
||||
if(cpCheckAxis(v0.ab, v1.ab, p.ab, n)){
|
||||
// The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer.
|
||||
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
|
||||
return ClosestPointsNew(v0, v1);
|
||||
} else {
|
||||
// p was closer to the origin than our existing edge.
|
||||
// Need to figure out which existing point to drop.
|
||||
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){
|
||||
return GJKRecurse(ctx, v0, p, iteration + 1);
|
||||
} else {
|
||||
return GJKRecurse(ctx, p, v1, iteration + 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Get a SupportPoint from a cached shape and index.
|
||||
static struct SupportPoint
|
||||
ShapePoint(const cpShape *shape, const int i)
|
||||
{
|
||||
switch(shape->klass->type){
|
||||
case CP_CIRCLE_SHAPE: {
|
||||
return SupportPointNew(((cpCircleShape *)shape)->tc, 0);
|
||||
} case CP_SEGMENT_SHAPE: {
|
||||
cpSegmentShape *seg = (cpSegmentShape *)shape;
|
||||
return SupportPointNew(i == 0 ? seg->ta : seg->tb, i);
|
||||
} case CP_POLY_SHAPE: {
|
||||
cpPolyShape *poly = (cpPolyShape *)shape;
|
||||
// Poly shapes may change vertex count.
|
||||
int index = (i < poly->count ? i : 0);
|
||||
return SupportPointNew(poly->planes[index].v0, index);
|
||||
} default: {
|
||||
return SupportPointNew(cpvzero, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Find the closest points between two shapes using the GJK algorithm.
|
||||
static struct ClosestPoints
|
||||
GJK(const struct SupportContext *ctx, cpCollisionID *id)
|
||||
{
|
||||
#if DRAW_GJK || DRAW_EPA
|
||||
int count1 = 1;
|
||||
int count2 = 1;
|
||||
|
||||
switch(ctx->shape1->klass->type){
|
||||
case CP_SEGMENT_SHAPE: count1 = 2; break;
|
||||
case CP_POLY_SHAPE: count1 = ((cpPolyShape *)ctx->shape1)->count; break;
|
||||
default: break;
|
||||
}
|
||||
|
||||
switch(ctx->shape2->klass->type){
|
||||
case CP_SEGMENT_SHAPE: count1 = 2; break;
|
||||
case CP_POLY_SHAPE: count2 = ((cpPolyShape *)ctx->shape2)->count; break;
|
||||
default: break;
|
||||
}
|
||||
|
||||
|
||||
// draw the minkowski difference origin
|
||||
cpVect origin = cpvzero;
|
||||
ChipmunkDebugDrawDot(5.0, origin, RGBAColor(1,0,0,1));
|
||||
|
||||
int mdiffCount = count1*count2;
|
||||
cpVect *mdiffVerts = alloca(mdiffCount*sizeof(cpVect));
|
||||
|
||||
for(int i=0; i<count1; i++){
|
||||
for(int j=0; j<count2; j++){
|
||||
cpVect v = cpvsub(ShapePoint(ctx->shape2, j).p, ShapePoint(ctx->shape1, i).p);
|
||||
mdiffVerts[i*count2 + j] = v;
|
||||
ChipmunkDebugDrawDot(2.0, v, RGBAColor(1, 0, 0, 1));
|
||||
}
|
||||
}
|
||||
|
||||
cpVect *hullVerts = alloca(mdiffCount*sizeof(cpVect));
|
||||
int hullCount = cpConvexHull(mdiffCount, mdiffVerts, hullVerts, NULL, 0.0);
|
||||
|
||||
ChipmunkDebugDrawPolygon(hullCount, hullVerts, 0.0, RGBAColor(1, 0, 0, 1), RGBAColor(1, 0, 0, 0.25));
|
||||
#endif
|
||||
|
||||
struct MinkowskiPoint v0, v1;
|
||||
if(*id){
|
||||
// Use the minkowski points from the last frame as a starting point using the cached indexes.
|
||||
v0 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>>24)&0xFF), ShapePoint(ctx->shape2, (*id>>16)&0xFF));
|
||||
v1 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>> 8)&0xFF), ShapePoint(ctx->shape2, (*id )&0xFF));
|
||||
} else {
|
||||
// No cached indexes, use the shapes' bounding box centers as a guess for a starting axis.
|
||||
cpVect axis = cpvperp(cpvsub(cpBBCenter(ctx->shape1->bb), cpBBCenter(ctx->shape2->bb)));
|
||||
v0 = Support(ctx, axis);
|
||||
v1 = Support(ctx, cpvneg(axis));
|
||||
}
|
||||
|
||||
struct ClosestPoints points = GJKRecurse(ctx, v0, v1, 1);
|
||||
*id = points.id;
|
||||
return points;
|
||||
}
|
||||
|
||||
//MARK: Contact Clipping
|
||||
|
||||
// Given two support edges, find contact point pairs on their surfaces.
|
||||
static inline void
|
||||
ContactPoints(const struct Edge e1, const struct Edge e2, const struct ClosestPoints points, struct cpCollisionInfo *info)
|
||||
{
|
||||
cpFloat mindist = e1.r + e2.r;
|
||||
if(points.d <= mindist){
|
||||
#ifdef DRAW_CLIP
|
||||
ChipmunkDebugDrawFatSegment(e1.a.p, e1.b.p, e1.r, RGBAColor(0, 1, 0, 1), LAColor(0, 0));
|
||||
ChipmunkDebugDrawFatSegment(e2.a.p, e2.b.p, e2.r, RGBAColor(1, 0, 0, 1), LAColor(0, 0));
|
||||
#endif
|
||||
cpVect n = info->n = points.n;
|
||||
|
||||
// Distances along the axis parallel to n
|
||||
cpFloat d_e1_a = cpvcross(e1.a.p, n);
|
||||
cpFloat d_e1_b = cpvcross(e1.b.p, n);
|
||||
cpFloat d_e2_a = cpvcross(e2.a.p, n);
|
||||
cpFloat d_e2_b = cpvcross(e2.b.p, n);
|
||||
|
||||
// TODO + min isn't a complete fix.
|
||||
cpFloat e1_denom = 1.0f/(d_e1_b - d_e1_a + CPFLOAT_MIN);
|
||||
cpFloat e2_denom = 1.0f/(d_e2_b - d_e2_a + CPFLOAT_MIN);
|
||||
|
||||
// Project the endpoints of the two edges onto the opposing edge, clamping them as necessary.
|
||||
// Compare the projected points to the collision normal to see if the shapes overlap there.
|
||||
{
|
||||
cpVect p1 = cpvadd(cpvmult(n, e1.r), cpvlerp(e1.a.p, e1.b.p, cpfclamp01((d_e2_b - d_e1_a)*e1_denom)));
|
||||
cpVect p2 = cpvadd(cpvmult(n, -e2.r), cpvlerp(e2.a.p, e2.b.p, cpfclamp01((d_e1_a - d_e2_a)*e2_denom)));
|
||||
cpFloat dist = cpvdot(cpvsub(p2, p1), n);
|
||||
if(dist <= 0.0f){
|
||||
cpHashValue hash_1a2b = CP_HASH_PAIR(e1.a.hash, e2.b.hash);
|
||||
cpCollisionInfoPushContact(info, p1, p2, hash_1a2b);
|
||||
}
|
||||
}{
|
||||
cpVect p1 = cpvadd(cpvmult(n, e1.r), cpvlerp(e1.a.p, e1.b.p, cpfclamp01((d_e2_a - d_e1_a)*e1_denom)));
|
||||
cpVect p2 = cpvadd(cpvmult(n, -e2.r), cpvlerp(e2.a.p, e2.b.p, cpfclamp01((d_e1_b - d_e2_a)*e2_denom)));
|
||||
cpFloat dist = cpvdot(cpvsub(p2, p1), n);
|
||||
if(dist <= 0.0f){
|
||||
cpHashValue hash_1b2a = CP_HASH_PAIR(e1.b.hash, e2.a.hash);
|
||||
cpCollisionInfoPushContact(info, p1, p2, hash_1b2a);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Collision Functions
|
||||
|
||||
typedef void (*CollisionFunc)(const cpShape *a, const cpShape *b, struct cpCollisionInfo *info);
|
||||
|
||||
// Collide circle shapes.
|
||||
static void
|
||||
CircleToCircle(const cpCircleShape *c1, const cpCircleShape *c2, struct cpCollisionInfo *info)
|
||||
{
|
||||
cpFloat mindist = c1->r + c2->r;
|
||||
cpVect delta = cpvsub(c2->tc, c1->tc);
|
||||
cpFloat distsq = cpvlengthsq(delta);
|
||||
|
||||
if(distsq < mindist*mindist){
|
||||
cpFloat dist = cpfsqrt(distsq);
|
||||
cpVect n = info->n = (dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f));
|
||||
cpCollisionInfoPushContact(info, cpvadd(c1->tc, cpvmult(n, c1->r)), cpvadd(c2->tc, cpvmult(n, -c2->r)), 0);
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
CircleToSegment(const cpCircleShape *circle, const cpSegmentShape *segment, struct cpCollisionInfo *info)
|
||||
{
|
||||
cpVect seg_a = segment->ta;
|
||||
cpVect seg_b = segment->tb;
|
||||
cpVect center = circle->tc;
|
||||
|
||||
// Find the closest point on the segment to the circle.
|
||||
cpVect seg_delta = cpvsub(seg_b, seg_a);
|
||||
cpFloat closest_t = cpfclamp01(cpvdot(seg_delta, cpvsub(center, seg_a))/cpvlengthsq(seg_delta));
|
||||
cpVect closest = cpvadd(seg_a, cpvmult(seg_delta, closest_t));
|
||||
|
||||
// Compare the radii of the two shapes to see if they are colliding.
|
||||
cpFloat mindist = circle->r + segment->r;
|
||||
cpVect delta = cpvsub(closest, center);
|
||||
cpFloat distsq = cpvlengthsq(delta);
|
||||
if(distsq < mindist*mindist){
|
||||
cpFloat dist = cpfsqrt(distsq);
|
||||
// Handle coincident shapes as gracefully as possible.
|
||||
cpVect n = info->n = (dist ? cpvmult(delta, 1.0f/dist) : segment->tn);
|
||||
|
||||
// Reject endcap collisions if tangents are provided.
|
||||
cpVect rot = cpBodyGetRotation(segment->shape.body);
|
||||
if(
|
||||
(closest_t != 0.0f || cpvdot(n, cpvrotate(segment->a_tangent, rot)) >= 0.0) &&
|
||||
(closest_t != 1.0f || cpvdot(n, cpvrotate(segment->b_tangent, rot)) >= 0.0)
|
||||
){
|
||||
cpCollisionInfoPushContact(info, cpvadd(center, cpvmult(n, circle->r)), cpvadd(closest, cpvmult(n, -segment->r)), 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
SegmentToSegment(const cpSegmentShape *seg1, const cpSegmentShape *seg2, struct cpCollisionInfo *info)
|
||||
{
|
||||
struct SupportContext context = {(cpShape *)seg1, (cpShape *)seg2, (SupportPointFunc)SegmentSupportPoint, (SupportPointFunc)SegmentSupportPoint};
|
||||
struct ClosestPoints points = GJK(&context, &info->id);
|
||||
|
||||
#if DRAW_CLOSEST
|
||||
#if PRINT_LOG
|
||||
// ChipmunkDemoPrintString("Distance: %.2f\n", points.d);
|
||||
#endif
|
||||
|
||||
ChipmunkDebugDrawDot(6.0, points.a, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawDot(6.0, points.b, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1));
|
||||
#endif
|
||||
|
||||
cpVect n = points.n;
|
||||
cpVect rot1 = cpBodyGetRotation(seg1->shape.body);
|
||||
cpVect rot2 = cpBodyGetRotation(seg2->shape.body);
|
||||
|
||||
// If the closest points are nearer than the sum of the radii...
|
||||
if(
|
||||
points.d <= (seg1->r + seg2->r) && (
|
||||
// Reject endcap collisions if tangents are provided.
|
||||
(!cpveql(points.a, seg1->ta) || cpvdot(n, cpvrotate(seg1->a_tangent, rot1)) <= 0.0) &&
|
||||
(!cpveql(points.a, seg1->tb) || cpvdot(n, cpvrotate(seg1->b_tangent, rot1)) <= 0.0) &&
|
||||
(!cpveql(points.b, seg2->ta) || cpvdot(n, cpvrotate(seg2->a_tangent, rot2)) >= 0.0) &&
|
||||
(!cpveql(points.b, seg2->tb) || cpvdot(n, cpvrotate(seg2->b_tangent, rot2)) >= 0.0)
|
||||
)
|
||||
){
|
||||
ContactPoints(SupportEdgeForSegment(seg1, n), SupportEdgeForSegment(seg2, cpvneg(n)), points, info);
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
PolyToPoly(const cpPolyShape *poly1, const cpPolyShape *poly2, struct cpCollisionInfo *info)
|
||||
{
|
||||
struct SupportContext context = {(cpShape *)poly1, (cpShape *)poly2, (SupportPointFunc)PolySupportPoint, (SupportPointFunc)PolySupportPoint};
|
||||
struct ClosestPoints points = GJK(&context, &info->id);
|
||||
|
||||
#if DRAW_CLOSEST
|
||||
#if PRINT_LOG
|
||||
// ChipmunkDemoPrintString("Distance: %.2f\n", points.d);
|
||||
#endif
|
||||
|
||||
ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1));
|
||||
#endif
|
||||
|
||||
// If the closest points are nearer than the sum of the radii...
|
||||
if(points.d - poly1->r - poly2->r <= 0.0){
|
||||
ContactPoints(SupportEdgeForPoly(poly1, points.n), SupportEdgeForPoly(poly2, cpvneg(points.n)), points, info);
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
SegmentToPoly(const cpSegmentShape *seg, const cpPolyShape *poly, struct cpCollisionInfo *info)
|
||||
{
|
||||
struct SupportContext context = {(cpShape *)seg, (cpShape *)poly, (SupportPointFunc)SegmentSupportPoint, (SupportPointFunc)PolySupportPoint};
|
||||
struct ClosestPoints points = GJK(&context, &info->id);
|
||||
|
||||
#if DRAW_CLOSEST
|
||||
#if PRINT_LOG
|
||||
// ChipmunkDemoPrintString("Distance: %.2f\n", points.d);
|
||||
#endif
|
||||
|
||||
ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1));
|
||||
#endif
|
||||
|
||||
cpVect n = points.n;
|
||||
cpVect rot = cpBodyGetRotation(seg->shape.body);
|
||||
|
||||
if(
|
||||
// If the closest points are nearer than the sum of the radii...
|
||||
points.d - seg->r - poly->r <= 0.0 && (
|
||||
// Reject endcap collisions if tangents are provided.
|
||||
(!cpveql(points.a, seg->ta) || cpvdot(n, cpvrotate(seg->a_tangent, rot)) <= 0.0) &&
|
||||
(!cpveql(points.a, seg->tb) || cpvdot(n, cpvrotate(seg->b_tangent, rot)) <= 0.0)
|
||||
)
|
||||
){
|
||||
ContactPoints(SupportEdgeForSegment(seg, n), SupportEdgeForPoly(poly, cpvneg(n)), points, info);
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
CircleToPoly(const cpCircleShape *circle, const cpPolyShape *poly, struct cpCollisionInfo *info)
|
||||
{
|
||||
struct SupportContext context = {(cpShape *)circle, (cpShape *)poly, (SupportPointFunc)CircleSupportPoint, (SupportPointFunc)PolySupportPoint};
|
||||
struct ClosestPoints points = GJK(&context, &info->id);
|
||||
|
||||
#if DRAW_CLOSEST
|
||||
ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1));
|
||||
ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1));
|
||||
#endif
|
||||
|
||||
// If the closest points are nearer than the sum of the radii...
|
||||
if(points.d <= circle->r + poly->r){
|
||||
cpVect n = info->n = points.n;
|
||||
cpCollisionInfoPushContact(info, cpvadd(points.a, cpvmult(n, circle->r)), cpvadd(points.b, cpvmult(n, poly->r)), 0);
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
CollisionError(const cpShape *circle, const cpShape *poly, struct cpCollisionInfo *info)
|
||||
{
|
||||
cpAssertHard(cpFalse, "Internal Error: Shape types are not sorted.");
|
||||
}
|
||||
|
||||
|
||||
static const CollisionFunc BuiltinCollisionFuncs[9] = {
|
||||
(CollisionFunc)CircleToCircle,
|
||||
CollisionError,
|
||||
CollisionError,
|
||||
(CollisionFunc)CircleToSegment,
|
||||
(CollisionFunc)SegmentToSegment,
|
||||
CollisionError,
|
||||
(CollisionFunc)CircleToPoly,
|
||||
(CollisionFunc)SegmentToPoly,
|
||||
(CollisionFunc)PolyToPoly,
|
||||
};
|
||||
static const CollisionFunc *CollisionFuncs = BuiltinCollisionFuncs;
|
||||
|
||||
struct cpCollisionInfo
|
||||
cpCollide(const cpShape *a, const cpShape *b, cpCollisionID id, struct cpContact *contacts)
|
||||
{
|
||||
struct cpCollisionInfo info = {a, b, id, cpvzero, 0, contacts};
|
||||
|
||||
// Make sure the shape types are in order.
|
||||
if(a->klass->type > b->klass->type){
|
||||
info.a = b;
|
||||
info.b = a;
|
||||
}
|
||||
|
||||
CollisionFuncs[info.a->klass->type + info.b->klass->type*CP_NUM_SHAPES](info.a, info.b, &info);
|
||||
|
||||
// if(0){
|
||||
// for(int i=0; i<info.count; i++){
|
||||
// cpVect r1 = info.arr[i].r1;
|
||||
// cpVect r2 = info.arr[i].r2;
|
||||
// cpVect mid = cpvlerp(r1, r2, 0.5f);
|
||||
//
|
||||
// ChipmunkDebugDrawSegment(r1, mid, RGBAColor(1, 0, 0, 1));
|
||||
// ChipmunkDebugDrawSegment(r2, mid, RGBAColor(0, 0, 1, 1));
|
||||
// }
|
||||
// }
|
||||
|
||||
return info;
|
||||
}
|
||||
|
|
@ -0,0 +1,173 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
// TODO: Comment me!
|
||||
|
||||
void cpConstraintDestroy(cpConstraint *constraint){}
|
||||
|
||||
void
|
||||
cpConstraintFree(cpConstraint *constraint)
|
||||
{
|
||||
if(constraint){
|
||||
cpConstraintDestroy(constraint);
|
||||
cpfree(constraint);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b)
|
||||
{
|
||||
constraint->klass = klass;
|
||||
|
||||
constraint->a = a;
|
||||
constraint->b = b;
|
||||
constraint->space = NULL;
|
||||
|
||||
constraint->next_a = NULL;
|
||||
constraint->next_b = NULL;
|
||||
|
||||
constraint->maxForce = (cpFloat)INFINITY;
|
||||
constraint->errorBias = cpfpow(1.0f - 0.1f, 60.0f);
|
||||
constraint->maxBias = (cpFloat)INFINITY;
|
||||
|
||||
constraint->collideBodies = cpTrue;
|
||||
|
||||
constraint->preSolve = NULL;
|
||||
constraint->postSolve = NULL;
|
||||
}
|
||||
|
||||
cpSpace *
|
||||
cpConstraintGetSpace(const cpConstraint *constraint)
|
||||
{
|
||||
return constraint->space;
|
||||
}
|
||||
|
||||
cpBody *
|
||||
cpConstraintGetBodyA(const cpConstraint *constraint)
|
||||
{
|
||||
return constraint->a;
|
||||
}
|
||||
|
||||
cpBody *
|
||||
cpConstraintGetBodyB(const cpConstraint *constraint)
|
||||
{
|
||||
return constraint->b;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpConstraintGetMaxForce(const cpConstraint *constraint)
|
||||
{
|
||||
return constraint->maxForce;
|
||||
}
|
||||
|
||||
void
|
||||
cpConstraintSetMaxForce(cpConstraint *constraint, cpFloat maxForce)
|
||||
{
|
||||
cpAssertHard(maxForce >= 0.0f, "maxForce must be positive.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
constraint->maxForce = maxForce;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpConstraintGetErrorBias(const cpConstraint *constraint)
|
||||
{
|
||||
return constraint->errorBias;
|
||||
}
|
||||
|
||||
void
|
||||
cpConstraintSetErrorBias(cpConstraint *constraint, cpFloat errorBias)
|
||||
{
|
||||
cpAssertHard(errorBias >= 0.0f, "errorBias must be positive.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
constraint->errorBias = errorBias;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpConstraintGetMaxBias(const cpConstraint *constraint)
|
||||
{
|
||||
return constraint->maxBias;
|
||||
}
|
||||
|
||||
void
|
||||
cpConstraintSetMaxBias(cpConstraint *constraint, cpFloat maxBias)
|
||||
{
|
||||
cpAssertHard(maxBias >= 0.0f, "maxBias must be positive.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
constraint->maxBias = maxBias;
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpConstraintGetCollideBodies(const cpConstraint *constraint)
|
||||
{
|
||||
return constraint->collideBodies;
|
||||
}
|
||||
|
||||
void
|
||||
cpConstraintSetCollideBodies(cpConstraint *constraint, cpBool collideBodies)
|
||||
{
|
||||
cpConstraintActivateBodies(constraint);
|
||||
constraint->collideBodies = collideBodies;
|
||||
}
|
||||
|
||||
cpConstraintPreSolveFunc
|
||||
cpConstraintGetPreSolveFunc(const cpConstraint *constraint)
|
||||
{
|
||||
return constraint->preSolve;
|
||||
}
|
||||
|
||||
void
|
||||
cpConstraintSetPreSolveFunc(cpConstraint *constraint, cpConstraintPreSolveFunc preSolveFunc)
|
||||
{
|
||||
constraint->preSolve = preSolveFunc;
|
||||
}
|
||||
|
||||
cpConstraintPostSolveFunc
|
||||
cpConstraintGetPostSolveFunc(const cpConstraint *constraint)
|
||||
{
|
||||
return constraint->postSolve;
|
||||
}
|
||||
|
||||
void
|
||||
cpConstraintSetPostSolveFunc(cpConstraint *constraint, cpConstraintPostSolveFunc postSolveFunc)
|
||||
{
|
||||
constraint->postSolve = postSolveFunc;
|
||||
}
|
||||
|
||||
cpDataPointer
|
||||
cpConstraintGetUserData(const cpConstraint *constraint)
|
||||
{
|
||||
return constraint->userData;
|
||||
}
|
||||
|
||||
void
|
||||
cpConstraintSetUserData(cpConstraint *constraint, cpDataPointer userData)
|
||||
{
|
||||
constraint->userData = userData;
|
||||
}
|
||||
|
||||
|
||||
cpFloat
|
||||
cpConstraintGetImpulse(cpConstraint *constraint)
|
||||
{
|
||||
return constraint->klass->getImpulse(constraint);
|
||||
}
|
||||
|
|
@ -0,0 +1,178 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
static cpFloat
|
||||
defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){
|
||||
return (relativeAngle - spring->restAngle)*spring->stiffness;
|
||||
}
|
||||
|
||||
static void
|
||||
preStep(cpDampedRotarySpring *spring, cpFloat dt)
|
||||
{
|
||||
cpBody *a = spring->constraint.a;
|
||||
cpBody *b = spring->constraint.b;
|
||||
|
||||
cpFloat moment = a->i_inv + b->i_inv;
|
||||
cpAssertSoft(moment != 0.0, "Unsolvable spring.");
|
||||
spring->iSum = 1.0f/moment;
|
||||
|
||||
spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment);
|
||||
spring->target_wrn = 0.0f;
|
||||
|
||||
// apply spring torque
|
||||
cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt;
|
||||
spring->jAcc = j_spring;
|
||||
|
||||
a->w -= j_spring*a->i_inv;
|
||||
b->w += j_spring*b->i_inv;
|
||||
}
|
||||
|
||||
static void applyCachedImpulse(cpDampedRotarySpring *spring, cpFloat dt_coef){}
|
||||
|
||||
static void
|
||||
applyImpulse(cpDampedRotarySpring *spring, cpFloat dt)
|
||||
{
|
||||
cpBody *a = spring->constraint.a;
|
||||
cpBody *b = spring->constraint.b;
|
||||
|
||||
// compute relative velocity
|
||||
cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
|
||||
|
||||
// compute velocity loss from drag
|
||||
// not 100% certain this is derived correctly, though it makes sense
|
||||
cpFloat w_damp = (spring->target_wrn - wrn)*spring->w_coef;
|
||||
spring->target_wrn = wrn + w_damp;
|
||||
|
||||
//apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
|
||||
cpFloat j_damp = w_damp*spring->iSum;
|
||||
spring->jAcc += j_damp;
|
||||
|
||||
a->w += j_damp*a->i_inv;
|
||||
b->w -= j_damp*b->i_inv;
|
||||
}
|
||||
|
||||
static cpFloat
|
||||
getImpulse(cpDampedRotarySpring *spring)
|
||||
{
|
||||
return spring->jAcc;
|
||||
}
|
||||
|
||||
static const cpConstraintClass klass = {
|
||||
(cpConstraintPreStepImpl)preStep,
|
||||
(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
|
||||
(cpConstraintApplyImpulseImpl)applyImpulse,
|
||||
(cpConstraintGetImpulseImpl)getImpulse,
|
||||
};
|
||||
|
||||
cpDampedRotarySpring *
|
||||
cpDampedRotarySpringAlloc(void)
|
||||
{
|
||||
return (cpDampedRotarySpring *)cpcalloc(1, sizeof(cpDampedRotarySpring));
|
||||
}
|
||||
|
||||
cpDampedRotarySpring *
|
||||
cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
|
||||
{
|
||||
cpConstraintInit((cpConstraint *)spring, &klass, a, b);
|
||||
|
||||
spring->restAngle = restAngle;
|
||||
spring->stiffness = stiffness;
|
||||
spring->damping = damping;
|
||||
spring->springTorqueFunc = (cpDampedRotarySpringTorqueFunc)defaultSpringTorque;
|
||||
|
||||
spring->jAcc = 0.0f;
|
||||
|
||||
return spring;
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
|
||||
{
|
||||
return (cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpConstraintIsDampedRotarySpring(const cpConstraint *constraint)
|
||||
{
|
||||
return (constraint->klass == &klass);
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpDampedRotarySpringGetRestAngle(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring.");
|
||||
return ((cpDampedRotarySpring *)constraint)->restAngle;
|
||||
}
|
||||
|
||||
void
|
||||
cpDampedRotarySpringSetRestAngle(cpConstraint *constraint, cpFloat restAngle)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpDampedRotarySpring *)constraint)->restAngle = restAngle;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpDampedRotarySpringGetStiffness(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring.");
|
||||
return ((cpDampedRotarySpring *)constraint)->stiffness;
|
||||
}
|
||||
|
||||
void
|
||||
cpDampedRotarySpringSetStiffness(cpConstraint *constraint, cpFloat stiffness)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpDampedRotarySpring *)constraint)->stiffness = stiffness;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpDampedRotarySpringGetDamping(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring.");
|
||||
return ((cpDampedRotarySpring *)constraint)->damping;
|
||||
}
|
||||
|
||||
void
|
||||
cpDampedRotarySpringSetDamping(cpConstraint *constraint, cpFloat damping)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpDampedRotarySpring *)constraint)->damping = damping;
|
||||
}
|
||||
|
||||
cpDampedRotarySpringTorqueFunc
|
||||
cpDampedRotarySpringGetSpringTorqueFunc(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring.");
|
||||
return ((cpDampedRotarySpring *)constraint)->springTorqueFunc;
|
||||
}
|
||||
|
||||
void
|
||||
cpDampedRotarySpringSetSpringTorqueFunc(cpConstraint *constraint, cpDampedRotarySpringTorqueFunc springTorqueFunc)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedRotarySpring(constraint), "Constraint is not a damped rotary spring.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpDampedRotarySpring *)constraint)->springTorqueFunc = springTorqueFunc;
|
||||
}
|
||||
|
|
@ -0,0 +1,216 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
static cpFloat
|
||||
defaultSpringForce(cpDampedSpring *spring, cpFloat dist){
|
||||
return (spring->restLength - dist)*spring->stiffness;
|
||||
}
|
||||
|
||||
static void
|
||||
preStep(cpDampedSpring *spring, cpFloat dt)
|
||||
{
|
||||
cpBody *a = spring->constraint.a;
|
||||
cpBody *b = spring->constraint.b;
|
||||
|
||||
spring->r1 = cpTransformVect(a->transform, cpvsub(spring->anchorA, a->cog));
|
||||
spring->r2 = cpTransformVect(b->transform, cpvsub(spring->anchorB, b->cog));
|
||||
|
||||
cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
|
||||
cpFloat dist = cpvlength(delta);
|
||||
spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
|
||||
|
||||
cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n);
|
||||
cpAssertSoft(k != 0.0, "Unsolvable spring.");
|
||||
spring->nMass = 1.0f/k;
|
||||
|
||||
spring->target_vrn = 0.0f;
|
||||
spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k);
|
||||
|
||||
// apply spring force
|
||||
cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
|
||||
cpFloat j_spring = spring->jAcc = f_spring*dt;
|
||||
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, j_spring));
|
||||
}
|
||||
|
||||
static void applyCachedImpulse(cpDampedSpring *spring, cpFloat dt_coef){}
|
||||
|
||||
static void
|
||||
applyImpulse(cpDampedSpring *spring, cpFloat dt)
|
||||
{
|
||||
cpBody *a = spring->constraint.a;
|
||||
cpBody *b = spring->constraint.b;
|
||||
|
||||
cpVect n = spring->n;
|
||||
cpVect r1 = spring->r1;
|
||||
cpVect r2 = spring->r2;
|
||||
|
||||
// compute relative velocity
|
||||
cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n);
|
||||
|
||||
// compute velocity loss from drag
|
||||
cpFloat v_damp = (spring->target_vrn - vrn)*spring->v_coef;
|
||||
spring->target_vrn = vrn + v_damp;
|
||||
|
||||
cpFloat j_damp = v_damp*spring->nMass;
|
||||
spring->jAcc += j_damp;
|
||||
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, j_damp));
|
||||
}
|
||||
|
||||
static cpFloat
|
||||
getImpulse(cpDampedSpring *spring)
|
||||
{
|
||||
return spring->jAcc;
|
||||
}
|
||||
|
||||
static const cpConstraintClass klass = {
|
||||
(cpConstraintPreStepImpl)preStep,
|
||||
(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
|
||||
(cpConstraintApplyImpulseImpl)applyImpulse,
|
||||
(cpConstraintGetImpulseImpl)getImpulse,
|
||||
};
|
||||
|
||||
cpDampedSpring *
|
||||
cpDampedSpringAlloc(void)
|
||||
{
|
||||
return (cpDampedSpring *)cpcalloc(1, sizeof(cpDampedSpring));
|
||||
}
|
||||
|
||||
cpDampedSpring *
|
||||
cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping)
|
||||
{
|
||||
cpConstraintInit((cpConstraint *)spring, &klass, a, b);
|
||||
|
||||
spring->anchorA = anchorA;
|
||||
spring->anchorB = anchorB;
|
||||
|
||||
spring->restLength = restLength;
|
||||
spring->stiffness = stiffness;
|
||||
spring->damping = damping;
|
||||
spring->springForceFunc = (cpDampedSpringForceFunc)defaultSpringForce;
|
||||
|
||||
spring->jAcc = 0.0f;
|
||||
|
||||
return spring;
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping)
|
||||
{
|
||||
return (cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchorA, anchorB, restLength, stiffness, damping);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpConstraintIsDampedSpring(const cpConstraint *constraint)
|
||||
{
|
||||
return (constraint->klass == &klass);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpDampedSpringGetAnchorA(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
return ((cpDampedSpring *)constraint)->anchorA;
|
||||
}
|
||||
|
||||
void
|
||||
cpDampedSpringSetAnchorA(cpConstraint *constraint, cpVect anchorA)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpDampedSpring *)constraint)->anchorA = anchorA;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpDampedSpringGetAnchorB(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
return ((cpDampedSpring *)constraint)->anchorB;
|
||||
}
|
||||
|
||||
void
|
||||
cpDampedSpringSetAnchorB(cpConstraint *constraint, cpVect anchorB)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpDampedSpring *)constraint)->anchorB = anchorB;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpDampedSpringGetRestLength(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
return ((cpDampedSpring *)constraint)->restLength;
|
||||
}
|
||||
|
||||
void
|
||||
cpDampedSpringSetRestLength(cpConstraint *constraint, cpFloat restLength)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpDampedSpring *)constraint)->restLength = restLength;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpDampedSpringGetStiffness(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
return ((cpDampedSpring *)constraint)->stiffness;
|
||||
}
|
||||
|
||||
void
|
||||
cpDampedSpringSetStiffness(cpConstraint *constraint, cpFloat stiffness)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpDampedSpring *)constraint)->stiffness = stiffness;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpDampedSpringGetDamping(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
return ((cpDampedSpring *)constraint)->damping;
|
||||
}
|
||||
|
||||
void
|
||||
cpDampedSpringSetDamping(cpConstraint *constraint, cpFloat damping)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpDampedSpring *)constraint)->damping = damping;
|
||||
}
|
||||
|
||||
cpDampedSpringForceFunc
|
||||
cpDampedSpringGetSpringForceFunc(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
return ((cpDampedSpring *)constraint)->springForceFunc;
|
||||
}
|
||||
|
||||
void
|
||||
cpDampedSpringSetSpringForceFunc(cpConstraint *constraint, cpDampedSpringForceFunc springForceFunc)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpDampedSpring *)constraint)->springForceFunc = springForceFunc;
|
||||
}
|
||||
|
|
@ -0,0 +1,145 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
static void
|
||||
preStep(cpGearJoint *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
// calculate moment of inertia coefficient.
|
||||
joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv);
|
||||
|
||||
// calculate bias velocity
|
||||
cpFloat maxBias = joint->constraint.maxBias;
|
||||
joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(b->a*joint->ratio - a->a - joint->phase)/dt, -maxBias, maxBias);
|
||||
}
|
||||
|
||||
static void
|
||||
applyCachedImpulse(cpGearJoint *joint, cpFloat dt_coef)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
cpFloat j = joint->jAcc*dt_coef;
|
||||
a->w -= j*a->i_inv*joint->ratio_inv;
|
||||
b->w += j*b->i_inv;
|
||||
}
|
||||
|
||||
static void
|
||||
applyImpulse(cpGearJoint *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
// compute relative rotational velocity
|
||||
cpFloat wr = b->w*joint->ratio - a->w;
|
||||
|
||||
cpFloat jMax = joint->constraint.maxForce*dt;
|
||||
|
||||
// compute normal impulse
|
||||
cpFloat j = (joint->bias - wr)*joint->iSum;
|
||||
cpFloat jOld = joint->jAcc;
|
||||
joint->jAcc = cpfclamp(jOld + j, -jMax, jMax);
|
||||
j = joint->jAcc - jOld;
|
||||
|
||||
// apply impulse
|
||||
a->w -= j*a->i_inv*joint->ratio_inv;
|
||||
b->w += j*b->i_inv;
|
||||
}
|
||||
|
||||
static cpFloat
|
||||
getImpulse(cpGearJoint *joint)
|
||||
{
|
||||
return cpfabs(joint->jAcc);
|
||||
}
|
||||
|
||||
static const cpConstraintClass klass = {
|
||||
(cpConstraintPreStepImpl)preStep,
|
||||
(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
|
||||
(cpConstraintApplyImpulseImpl)applyImpulse,
|
||||
(cpConstraintGetImpulseImpl)getImpulse,
|
||||
};
|
||||
|
||||
cpGearJoint *
|
||||
cpGearJointAlloc(void)
|
||||
{
|
||||
return (cpGearJoint *)cpcalloc(1, sizeof(cpGearJoint));
|
||||
}
|
||||
|
||||
cpGearJoint *
|
||||
cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio)
|
||||
{
|
||||
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
||||
|
||||
joint->phase = phase;
|
||||
joint->ratio = ratio;
|
||||
joint->ratio_inv = 1.0f/ratio;
|
||||
|
||||
joint->jAcc = 0.0f;
|
||||
|
||||
return joint;
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio)
|
||||
{
|
||||
return (cpConstraint *)cpGearJointInit(cpGearJointAlloc(), a, b, phase, ratio);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpConstraintIsGearJoint(const cpConstraint *constraint)
|
||||
{
|
||||
return (constraint->klass == &klass);
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpGearJointGetPhase(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint.");
|
||||
return ((cpGearJoint *)constraint)->phase;
|
||||
}
|
||||
|
||||
void
|
||||
cpGearJointSetPhase(cpConstraint *constraint, cpFloat phase)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpGearJoint *)constraint)->phase = phase;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpGearJointGetRatio(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint.");
|
||||
return ((cpGearJoint *)constraint)->ratio;
|
||||
}
|
||||
|
||||
void
|
||||
cpGearJointSetRatio(cpConstraint *constraint, cpFloat ratio)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsGearJoint(constraint), "Constraint is not a ratchet joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpGearJoint *)constraint)->ratio = ratio;
|
||||
((cpGearJoint *)constraint)->ratio_inv = 1.0f/ratio;
|
||||
}
|
||||
|
|
@ -0,0 +1,197 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
static void
|
||||
preStep(cpGrooveJoint *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
// calculate endpoints in worldspace
|
||||
cpVect ta = cpTransformPoint(a->transform, joint->grv_a);
|
||||
cpVect tb = cpTransformPoint(a->transform, joint->grv_b);
|
||||
|
||||
// calculate axis
|
||||
cpVect n = cpTransformVect(a->transform, joint->grv_n);
|
||||
cpFloat d = cpvdot(ta, n);
|
||||
|
||||
joint->grv_tn = n;
|
||||
joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
|
||||
|
||||
// calculate tangential distance along the axis of r2
|
||||
cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
|
||||
// calculate clamping factor and r2
|
||||
if(td <= cpvcross(ta, n)){
|
||||
joint->clamp = 1.0f;
|
||||
joint->r1 = cpvsub(ta, a->p);
|
||||
} else if(td >= cpvcross(tb, n)){
|
||||
joint->clamp = -1.0f;
|
||||
joint->r1 = cpvsub(tb, a->p);
|
||||
} else {
|
||||
joint->clamp = 0.0f;
|
||||
joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
|
||||
}
|
||||
|
||||
// Calculate mass tensor
|
||||
joint->k = k_tensor(a, b, joint->r1, joint->r2);
|
||||
|
||||
// calculate bias velocity
|
||||
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
|
||||
joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
|
||||
}
|
||||
|
||||
static void
|
||||
applyCachedImpulse(cpGrooveJoint *joint, cpFloat dt_coef)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef));
|
||||
}
|
||||
|
||||
static inline cpVect
|
||||
grooveConstrain(cpGrooveJoint *joint, cpVect j, cpFloat dt){
|
||||
cpVect n = joint->grv_tn;
|
||||
cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n);
|
||||
return cpvclamp(jClamp, joint->constraint.maxForce*dt);
|
||||
}
|
||||
|
||||
static void
|
||||
applyImpulse(cpGrooveJoint *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
cpVect r1 = joint->r1;
|
||||
cpVect r2 = joint->r2;
|
||||
|
||||
// compute impulse
|
||||
cpVect vr = relative_velocity(a, b, r1, r2);
|
||||
|
||||
cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr));
|
||||
cpVect jOld = joint->jAcc;
|
||||
joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j), dt);
|
||||
j = cpvsub(joint->jAcc, jOld);
|
||||
|
||||
// apply impulse
|
||||
apply_impulses(a, b, joint->r1, joint->r2, j);
|
||||
}
|
||||
|
||||
static cpFloat
|
||||
getImpulse(cpGrooveJoint *joint)
|
||||
{
|
||||
return cpvlength(joint->jAcc);
|
||||
}
|
||||
|
||||
static const cpConstraintClass klass = {
|
||||
(cpConstraintPreStepImpl)preStep,
|
||||
(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
|
||||
(cpConstraintApplyImpulseImpl)applyImpulse,
|
||||
(cpConstraintGetImpulseImpl)getImpulse,
|
||||
};
|
||||
|
||||
cpGrooveJoint *
|
||||
cpGrooveJointAlloc(void)
|
||||
{
|
||||
return (cpGrooveJoint *)cpcalloc(1, sizeof(cpGrooveJoint));
|
||||
}
|
||||
|
||||
cpGrooveJoint *
|
||||
cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB)
|
||||
{
|
||||
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
||||
|
||||
joint->grv_a = groove_a;
|
||||
joint->grv_b = groove_b;
|
||||
joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a)));
|
||||
joint->anchorB = anchorB;
|
||||
|
||||
joint->jAcc = cpvzero;
|
||||
|
||||
return joint;
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB)
|
||||
{
|
||||
return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchorB);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpConstraintIsGrooveJoint(const cpConstraint *constraint)
|
||||
{
|
||||
return (constraint->klass == &klass);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpGrooveJointGetGrooveA(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
|
||||
return ((cpGrooveJoint *)constraint)->grv_a;
|
||||
}
|
||||
|
||||
void
|
||||
cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
|
||||
cpGrooveJoint *g = (cpGrooveJoint *)constraint;
|
||||
|
||||
g->grv_a = value;
|
||||
g->grv_n = cpvperp(cpvnormalize(cpvsub(g->grv_b, value)));
|
||||
|
||||
cpConstraintActivateBodies(constraint);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpGrooveJointGetGrooveB(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
|
||||
return ((cpGrooveJoint *)constraint)->grv_b;
|
||||
}
|
||||
|
||||
void
|
||||
cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
|
||||
cpGrooveJoint *g = (cpGrooveJoint *)constraint;
|
||||
|
||||
g->grv_b = value;
|
||||
g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a)));
|
||||
|
||||
cpConstraintActivateBodies(constraint);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpGrooveJointGetAnchorB(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
|
||||
return ((cpGrooveJoint *)constraint)->anchorB;
|
||||
}
|
||||
|
||||
void
|
||||
cpGrooveJointSetAnchorB(cpConstraint *constraint, cpVect anchorB)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpGrooveJoint *)constraint)->anchorB = anchorB;
|
||||
}
|
||||
|
|
@ -0,0 +1,253 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
#include "prime.h"
|
||||
|
||||
typedef struct cpHashSetBin {
|
||||
void *elt;
|
||||
cpHashValue hash;
|
||||
struct cpHashSetBin *next;
|
||||
} cpHashSetBin;
|
||||
|
||||
struct cpHashSet {
|
||||
unsigned int entries, size;
|
||||
|
||||
cpHashSetEqlFunc eql;
|
||||
void *default_value;
|
||||
|
||||
cpHashSetBin **table;
|
||||
cpHashSetBin *pooledBins;
|
||||
|
||||
cpArray *allocatedBuffers;
|
||||
};
|
||||
|
||||
void
|
||||
cpHashSetFree(cpHashSet *set)
|
||||
{
|
||||
if(set){
|
||||
cpfree(set->table);
|
||||
|
||||
cpArrayFreeEach(set->allocatedBuffers, cpfree);
|
||||
cpArrayFree(set->allocatedBuffers);
|
||||
|
||||
cpfree(set);
|
||||
}
|
||||
}
|
||||
|
||||
cpHashSet *
|
||||
cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc)
|
||||
{
|
||||
cpHashSet *set = (cpHashSet *)cpcalloc(1, sizeof(cpHashSet));
|
||||
|
||||
set->size = next_prime(size);
|
||||
set->entries = 0;
|
||||
|
||||
set->eql = eqlFunc;
|
||||
set->default_value = NULL;
|
||||
|
||||
set->table = (cpHashSetBin **)cpcalloc(set->size, sizeof(cpHashSetBin *));
|
||||
set->pooledBins = NULL;
|
||||
|
||||
set->allocatedBuffers = cpArrayNew(0);
|
||||
|
||||
return set;
|
||||
}
|
||||
|
||||
void
|
||||
cpHashSetSetDefaultValue(cpHashSet *set, void *default_value)
|
||||
{
|
||||
set->default_value = default_value;
|
||||
}
|
||||
|
||||
static int
|
||||
setIsFull(cpHashSet *set)
|
||||
{
|
||||
return (set->entries >= set->size);
|
||||
}
|
||||
|
||||
static void
|
||||
cpHashSetResize(cpHashSet *set)
|
||||
{
|
||||
// Get the next approximate doubled prime.
|
||||
unsigned int newSize = next_prime(set->size + 1);
|
||||
// Allocate a new table.
|
||||
cpHashSetBin **newTable = (cpHashSetBin **)cpcalloc(newSize, sizeof(cpHashSetBin *));
|
||||
|
||||
// Iterate over the chains.
|
||||
for(unsigned int i=0; i<set->size; i++){
|
||||
// Rehash the bins into the new table.
|
||||
cpHashSetBin *bin = set->table[i];
|
||||
while(bin){
|
||||
cpHashSetBin *next = bin->next;
|
||||
|
||||
cpHashValue idx = bin->hash%newSize;
|
||||
bin->next = newTable[idx];
|
||||
newTable[idx] = bin;
|
||||
|
||||
bin = next;
|
||||
}
|
||||
}
|
||||
|
||||
cpfree(set->table);
|
||||
|
||||
set->table = newTable;
|
||||
set->size = newSize;
|
||||
}
|
||||
|
||||
static inline void
|
||||
recycleBin(cpHashSet *set, cpHashSetBin *bin)
|
||||
{
|
||||
bin->next = set->pooledBins;
|
||||
set->pooledBins = bin;
|
||||
bin->elt = NULL;
|
||||
}
|
||||
|
||||
static cpHashSetBin *
|
||||
getUnusedBin(cpHashSet *set)
|
||||
{
|
||||
cpHashSetBin *bin = set->pooledBins;
|
||||
|
||||
if(bin){
|
||||
set->pooledBins = bin->next;
|
||||
return bin;
|
||||
} else {
|
||||
// Pool is exhausted, make more
|
||||
int count = CP_BUFFER_BYTES/sizeof(cpHashSetBin);
|
||||
cpAssertHard(count, "Internal Error: Buffer size is too small.");
|
||||
|
||||
cpHashSetBin *buffer = (cpHashSetBin *)cpcalloc(1, CP_BUFFER_BYTES);
|
||||
cpArrayPush(set->allocatedBuffers, buffer);
|
||||
|
||||
// push all but the first one, return it instead
|
||||
for(int i=1; i<count; i++) recycleBin(set, buffer + i);
|
||||
return buffer;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
cpHashSetCount(cpHashSet *set)
|
||||
{
|
||||
return set->entries;
|
||||
}
|
||||
|
||||
void *
|
||||
cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, cpHashSetTransFunc trans, void *data)
|
||||
{
|
||||
cpHashValue idx = hash%set->size;
|
||||
|
||||
// Find the bin with the matching element.
|
||||
cpHashSetBin *bin = set->table[idx];
|
||||
while(bin && !set->eql(ptr, bin->elt))
|
||||
bin = bin->next;
|
||||
|
||||
// Create it if necessary.
|
||||
if(!bin){
|
||||
bin = getUnusedBin(set);
|
||||
bin->hash = hash;
|
||||
bin->elt = (trans ? trans(ptr, data) : data);
|
||||
|
||||
bin->next = set->table[idx];
|
||||
set->table[idx] = bin;
|
||||
|
||||
set->entries++;
|
||||
if(setIsFull(set)) cpHashSetResize(set);
|
||||
}
|
||||
|
||||
return bin->elt;
|
||||
}
|
||||
|
||||
void *
|
||||
cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr)
|
||||
{
|
||||
cpHashValue idx = hash%set->size;
|
||||
|
||||
cpHashSetBin **prev_ptr = &set->table[idx];
|
||||
cpHashSetBin *bin = set->table[idx];
|
||||
|
||||
// Find the bin
|
||||
while(bin && !set->eql(ptr, bin->elt)){
|
||||
prev_ptr = &bin->next;
|
||||
bin = bin->next;
|
||||
}
|
||||
|
||||
// Remove it if it exists.
|
||||
if(bin){
|
||||
// Update the previous linked list pointer
|
||||
(*prev_ptr) = bin->next;
|
||||
set->entries--;
|
||||
|
||||
void *elt = bin->elt;
|
||||
recycleBin(set, bin);
|
||||
|
||||
return elt;
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void *
|
||||
cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr)
|
||||
{
|
||||
cpHashValue idx = hash%set->size;
|
||||
cpHashSetBin *bin = set->table[idx];
|
||||
while(bin && !set->eql(ptr, bin->elt))
|
||||
bin = bin->next;
|
||||
|
||||
return (bin ? bin->elt : set->default_value);
|
||||
}
|
||||
|
||||
void
|
||||
cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data)
|
||||
{
|
||||
for(unsigned int i=0; i<set->size; i++){
|
||||
cpHashSetBin *bin = set->table[i];
|
||||
while(bin){
|
||||
cpHashSetBin *next = bin->next;
|
||||
func(bin->elt, data);
|
||||
bin = next;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data)
|
||||
{
|
||||
for(unsigned int i=0; i<set->size; i++){
|
||||
// The rest works similarly to cpHashSetRemove() above.
|
||||
cpHashSetBin **prev_ptr = &set->table[i];
|
||||
cpHashSetBin *bin = set->table[i];
|
||||
while(bin){
|
||||
cpHashSetBin *next = bin->next;
|
||||
|
||||
if(func(bin->elt, data)){
|
||||
prev_ptr = &bin->next;
|
||||
} else {
|
||||
(*prev_ptr) = next;
|
||||
|
||||
set->entries--;
|
||||
recycleBin(set, bin);
|
||||
}
|
||||
|
||||
bin = next;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,700 @@
|
|||
// Copyright 2013 Howling Moon Software. All rights reserved.
|
||||
// See http://chipmunk2d.net/legal.php for more information.
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
//TODO: Move all the thread stuff to another file
|
||||
|
||||
//#include <sys/param.h >
|
||||
#ifndef _WIN32
|
||||
#include <sys/sysctl.h>
|
||||
#include <pthread.h>
|
||||
#else
|
||||
#ifndef WIN32_LEAN_AND_MEAN
|
||||
#define WIN32_LEAN_AND_MEAN
|
||||
#endif
|
||||
|
||||
#ifndef NOMINMAX
|
||||
#define NOMINMAX
|
||||
#endif
|
||||
|
||||
#include <process.h> // _beginthreadex
|
||||
#include <windows.h>
|
||||
|
||||
#ifndef ETIMEDOUT
|
||||
#define ETIMEDOUT 1
|
||||
#endif
|
||||
|
||||
// Simple pthread implementation for Windows
|
||||
// Made from scratch to avoid the LGPL licence from pthread-win32
|
||||
enum {
|
||||
SIGNAL = 0,
|
||||
BROADCAST = 1,
|
||||
MAX_EVENTS = 2
|
||||
};
|
||||
|
||||
typedef HANDLE pthread_t;
|
||||
typedef struct
|
||||
{
|
||||
// Based on http://www.cs.wustl.edu/~schmidt/win32-cv-1.html since Windows has no condition variable until NT6
|
||||
UINT waiters_count;
|
||||
// Count of the number of waiters.
|
||||
|
||||
CRITICAL_SECTION waiters_count_lock;
|
||||
// Serialize access to <waiters_count_>.
|
||||
|
||||
HANDLE events[MAX_EVENTS];
|
||||
} pthread_cond_t;
|
||||
typedef CRITICAL_SECTION pthread_mutex_t;
|
||||
|
||||
typedef struct {
|
||||
int dummy;
|
||||
} pthread_condattr_t; // Dummy;
|
||||
|
||||
int pthread_cond_destroy(pthread_cond_t* cv)
|
||||
{
|
||||
CloseHandle(cv->events[BROADCAST]);
|
||||
CloseHandle(cv->events[SIGNAL]);
|
||||
|
||||
DeleteCriticalSection(&cv->waiters_count_lock);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int pthread_cond_init(pthread_cond_t* cv, const pthread_condattr_t* attr)
|
||||
{
|
||||
// Initialize the count to 0.
|
||||
cv->waiters_count = 0;
|
||||
|
||||
// Create an auto-reset event.
|
||||
cv->events[SIGNAL] = CreateEvent(NULL, // no security
|
||||
FALSE, // auto-reset event
|
||||
FALSE, // non-signaled initially
|
||||
NULL); // unnamed
|
||||
|
||||
// Create a manual-reset event.
|
||||
cv->events[BROADCAST] = CreateEvent(NULL, // no security
|
||||
TRUE, // manual-reset
|
||||
FALSE, // non-signaled initially
|
||||
NULL); // unnamed
|
||||
|
||||
InitializeCriticalSection(&cv->waiters_count_lock);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int pthread_cond_broadcast(pthread_cond_t *cv)
|
||||
{
|
||||
// Avoid race conditions.
|
||||
EnterCriticalSection(&cv->waiters_count_lock);
|
||||
int have_waiters = cv->waiters_count > 0;
|
||||
LeaveCriticalSection(&cv->waiters_count_lock);
|
||||
|
||||
if (have_waiters)
|
||||
SetEvent(cv->events[BROADCAST]);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int pthread_cond_signal(pthread_cond_t* cv)
|
||||
{
|
||||
// Avoid race conditions.
|
||||
EnterCriticalSection(&cv->waiters_count_lock);
|
||||
int have_waiters = cv->waiters_count > 0;
|
||||
LeaveCriticalSection(&cv->waiters_count_lock);
|
||||
|
||||
if (have_waiters)
|
||||
SetEvent(cv->events[SIGNAL]);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int pthread_cond_wait(pthread_cond_t* cv, pthread_mutex_t* external_mutex)
|
||||
{
|
||||
// Avoid race conditions.
|
||||
EnterCriticalSection(&cv->waiters_count_lock);
|
||||
cv->waiters_count++;
|
||||
LeaveCriticalSection(&cv->waiters_count_lock);
|
||||
|
||||
// It's ok to release the <external_mutex> here since Win32
|
||||
// manual-reset events maintain state when used with
|
||||
// <SetEvent>. This avoids the "lost wakeup" bug...
|
||||
LeaveCriticalSection(external_mutex);
|
||||
|
||||
// Wait for either event to become signaled due to <pthread_cond_signal>
|
||||
// being called or <pthread_cond_broadcast> being called.
|
||||
int result = WaitForMultipleObjects(2, cv->events, FALSE, INFINITE);
|
||||
|
||||
EnterCriticalSection(&cv->waiters_count_lock);
|
||||
cv->waiters_count--;
|
||||
int last_waiter =
|
||||
result == WAIT_OBJECT_0 + BROADCAST
|
||||
&& cv->waiters_count == 0;
|
||||
LeaveCriticalSection(&cv->waiters_count_lock);
|
||||
|
||||
// Some thread called <pthread_cond_broadcast>.
|
||||
if (last_waiter)
|
||||
// We're the last waiter to be notified or to stop waiting, so
|
||||
// reset the manual event.
|
||||
ResetEvent(cv->events[BROADCAST]);
|
||||
|
||||
// Reacquire the <external_mutex>.
|
||||
EnterCriticalSection(external_mutex);
|
||||
|
||||
return result == WAIT_TIMEOUT ? ETIMEDOUT : 0;
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
int dummy;
|
||||
} pthread_mutexattr_t; //< Dummy
|
||||
|
||||
int pthread_mutex_init(pthread_mutex_t* mutex, const pthread_mutexattr_t* attr)
|
||||
{
|
||||
InitializeCriticalSection(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int pthread_mutex_destroy(pthread_mutex_t* mutex)
|
||||
{
|
||||
DeleteCriticalSection(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int pthread_mutex_lock(pthread_mutex_t* mutex)
|
||||
{
|
||||
EnterCriticalSection(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int pthread_mutex_unlock(pthread_mutex_t* mutex)
|
||||
{
|
||||
LeaveCriticalSection(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
int dummy;
|
||||
} pthread_attr_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
void *(*start_routine) (void *);
|
||||
void* arg;
|
||||
} pthread_internal_thread;
|
||||
|
||||
unsigned int __stdcall ThreadProc(void* userdata)
|
||||
{
|
||||
pthread_internal_thread* ud = (pthread_internal_thread*) userdata;
|
||||
ud->start_routine(ud->arg);
|
||||
|
||||
free(ud);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int pthread_create(pthread_t* thread, const pthread_attr_t* attr, void *(*start_routine) (void *), void *arg)
|
||||
{
|
||||
pthread_internal_thread* ud = (pthread_internal_thread*) malloc(sizeof(pthread_internal_thread));
|
||||
ud->start_routine = start_routine;
|
||||
ud->arg = arg;
|
||||
|
||||
*thread = (HANDLE) (_beginthreadex(NULL, 0, &ThreadProc, ud, 0, NULL));
|
||||
if (!*thread)
|
||||
return 1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int pthread_join(pthread_t thread, void **value_ptr)
|
||||
{
|
||||
WaitForSingleObject(thread, INFINITE);
|
||||
CloseHandle(thread);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#include "chipmunk/chipmunk_private.h"
|
||||
#include "chipmunk/cpHastySpace.h"
|
||||
|
||||
|
||||
//MARK: ARM NEON Solver
|
||||
|
||||
#if __ARM_NEON__
|
||||
#include <arm_neon.h>
|
||||
|
||||
// Tested and known to work fine with Clang 3.0 and GCC 4.2
|
||||
// Doesn't work with Clang 1.6, and I have no idea why.
|
||||
#if defined(__clang_major__) && __clang_major__ < 3
|
||||
#error Compiler not supported.
|
||||
#endif
|
||||
|
||||
#if CP_USE_DOUBLES
|
||||
#if !__arm64
|
||||
#error Cannot use CP_USE_DOUBLES on 32 bit ARM.
|
||||
#endif
|
||||
|
||||
typedef float64_t cpFloat_t;
|
||||
typedef float64x2_t cpFloatx2_t;
|
||||
#define vld vld1q_f64
|
||||
#define vdup_n vdupq_n_f64
|
||||
#define vst vst1q_f64
|
||||
#define vst_lane vst1q_lane_f64
|
||||
#define vadd vaddq_f64
|
||||
#define vsub vsubq_f64
|
||||
#define vpadd vpaddq_f64
|
||||
#define vmul vmulq_f64
|
||||
#define vmul_n vmulq_n_f64
|
||||
#define vneg vnegq_f64
|
||||
#define vget_lane vgetq_lane_f64
|
||||
#define vset_lane vsetq_lane_f64
|
||||
#define vmin vminq_f64
|
||||
#define vmax vmaxq_f64
|
||||
#define vrev(__a) __builtin_shufflevector(__a, __a, 1, 0)
|
||||
#else
|
||||
typedef float32_t cpFloat_t;
|
||||
typedef float32x2_t cpFloatx2_t;
|
||||
#define vld vld1_f32
|
||||
#define vdup_n vdup_n_f32
|
||||
#define vst vst1_f32
|
||||
#define vst_lane vst1_lane_f32
|
||||
#define vadd vadd_f32
|
||||
#define vsub vsub_f32
|
||||
#define vpadd vpadd_f32
|
||||
#define vmul vmul_f32
|
||||
#define vmul_n vmul_n_f32
|
||||
#define vneg vneg_f32
|
||||
#define vget_lane vget_lane_f32
|
||||
#define vset_lane vset_lane_f32
|
||||
#define vmin vmin_f32
|
||||
#define vmax vmax_f32
|
||||
#define vrev vrev64_f32
|
||||
#endif
|
||||
|
||||
// TODO could probably do better here, maybe using vcreate?
|
||||
// especially for the constants
|
||||
// Maybe use the {} notation for GCC/Clang?
|
||||
static inline cpFloatx2_t
|
||||
vmake(cpFloat_t x, cpFloat_t y)
|
||||
{
|
||||
// cpFloatx2_t v = {};
|
||||
// v = vset_lane(x, v, 0);
|
||||
// v = vset_lane(y, v, 1);
|
||||
//
|
||||
// return v;
|
||||
|
||||
// This might not be super compatible, but all the NEON headers use it...
|
||||
return (cpFloatx2_t){x, y};
|
||||
}
|
||||
|
||||
static void
|
||||
cpArbiterApplyImpulse_NEON(cpArbiter *arb)
|
||||
{
|
||||
cpBody *a = arb->body_a;
|
||||
cpBody *b = arb->body_b;
|
||||
cpFloatx2_t surface_vr = vld((cpFloat_t *)&arb->surface_vr);
|
||||
cpFloatx2_t n = vld((cpFloat_t *)&arb->n);
|
||||
cpFloat_t friction = arb->u;
|
||||
|
||||
int numContacts = arb->count;
|
||||
struct cpContact *contacts = arb->contacts;
|
||||
for(int i=0; i<numContacts; i++){
|
||||
struct cpContact *con = contacts + i;
|
||||
cpFloatx2_t r1 = vld((cpFloat_t *)&con->r1);
|
||||
cpFloatx2_t r2 = vld((cpFloat_t *)&con->r2);
|
||||
|
||||
cpFloatx2_t perp = vmake(-1.0, 1.0);
|
||||
cpFloatx2_t r1p = vmul(vrev(r1), perp);
|
||||
cpFloatx2_t r2p = vmul(vrev(r2), perp);
|
||||
|
||||
cpFloatx2_t vBias_a = vld((cpFloat_t *)&a->v_bias);
|
||||
cpFloatx2_t vBias_b = vld((cpFloat_t *)&b->v_bias);
|
||||
cpFloatx2_t wBias = vmake(a->w_bias, b->w_bias);
|
||||
|
||||
cpFloatx2_t vb1 = vadd(vBias_a, vmul_n(r1p, vget_lane(wBias, 0)));
|
||||
cpFloatx2_t vb2 = vadd(vBias_b, vmul_n(r2p, vget_lane(wBias, 1)));
|
||||
cpFloatx2_t vbr = vsub(vb2, vb1);
|
||||
|
||||
cpFloatx2_t v_a = vld((cpFloat_t *)&a->v);
|
||||
cpFloatx2_t v_b = vld((cpFloat_t *)&b->v);
|
||||
cpFloatx2_t w = vmake(a->w, b->w);
|
||||
cpFloatx2_t v1 = vadd(v_a, vmul_n(r1p, vget_lane(w, 0)));
|
||||
cpFloatx2_t v2 = vadd(v_b, vmul_n(r2p, vget_lane(w, 1)));
|
||||
cpFloatx2_t vr = vsub(v2, v1);
|
||||
|
||||
cpFloatx2_t vbn_vrn = vpadd(vmul(vbr, n), vmul(vr, n));
|
||||
|
||||
cpFloatx2_t v_offset = vmake(con->bias, -con->bounce);
|
||||
cpFloatx2_t jOld = vmake(con->jBias, con->jnAcc);
|
||||
cpFloatx2_t jbn_jn = vmul_n(vsub(v_offset, vbn_vrn), con->nMass);
|
||||
jbn_jn = vmax(vadd(jOld, jbn_jn), vdup_n(0.0));
|
||||
cpFloatx2_t jApply = vsub(jbn_jn, jOld);
|
||||
|
||||
cpFloatx2_t t = vmul(vrev(n), perp);
|
||||
cpFloatx2_t vrt_tmp = vmul(vadd(vr, surface_vr), t);
|
||||
cpFloatx2_t vrt = vpadd(vrt_tmp, vrt_tmp);
|
||||
|
||||
cpFloatx2_t jtOld = {}; jtOld = vset_lane(con->jtAcc, jtOld, 0);
|
||||
cpFloatx2_t jtMax = vrev(vmul_n(jbn_jn, friction));
|
||||
cpFloatx2_t jt = vmul_n(vrt, -con->tMass);
|
||||
jt = vmax(vneg(jtMax), vmin(vadd(jtOld, jt), jtMax));
|
||||
cpFloatx2_t jtApply = vsub(jt, jtOld);
|
||||
|
||||
cpFloatx2_t i_inv = vmake(-a->i_inv, b->i_inv);
|
||||
cpFloatx2_t nperp = vmake(1.0, -1.0);
|
||||
|
||||
cpFloatx2_t jBias = vmul_n(n, vget_lane(jApply, 0));
|
||||
cpFloatx2_t jBiasCross = vmul(vrev(jBias), nperp);
|
||||
cpFloatx2_t biasCrosses = vpadd(vmul(r1, jBiasCross), vmul(r2, jBiasCross));
|
||||
wBias = vadd(wBias, vmul(i_inv, biasCrosses));
|
||||
|
||||
vBias_a = vsub(vBias_a, vmul_n(jBias, a->m_inv));
|
||||
vBias_b = vadd(vBias_b, vmul_n(jBias, b->m_inv));
|
||||
|
||||
cpFloatx2_t j = vadd(vmul_n(n, vget_lane(jApply, 1)), vmul_n(t, vget_lane(jtApply, 0)));
|
||||
cpFloatx2_t jCross = vmul(vrev(j), nperp);
|
||||
cpFloatx2_t crosses = vpadd(vmul(r1, jCross), vmul(r2, jCross));
|
||||
w = vadd(w, vmul(i_inv, crosses));
|
||||
|
||||
v_a = vsub(v_a, vmul_n(j, a->m_inv));
|
||||
v_b = vadd(v_b, vmul_n(j, b->m_inv));
|
||||
|
||||
// TODO would moving these earlier help pipeline them better?
|
||||
vst((cpFloat_t *)&a->v_bias, vBias_a);
|
||||
vst((cpFloat_t *)&b->v_bias, vBias_b);
|
||||
vst_lane((cpFloat_t *)&a->w_bias, wBias, 0);
|
||||
vst_lane((cpFloat_t *)&b->w_bias, wBias, 1);
|
||||
|
||||
vst((cpFloat_t *)&a->v, v_a);
|
||||
vst((cpFloat_t *)&b->v, v_b);
|
||||
vst_lane((cpFloat_t *)&a->w, w, 0);
|
||||
vst_lane((cpFloat_t *)&b->w, w, 1);
|
||||
|
||||
vst_lane((cpFloat_t *)&con->jBias, jbn_jn, 0);
|
||||
vst_lane((cpFloat_t *)&con->jnAcc, jbn_jn, 1);
|
||||
vst_lane((cpFloat_t *)&con->jtAcc, jt, 0);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
//MARK: PThreads
|
||||
|
||||
// Right now using more than 2 threads probably wont help your performance any.
|
||||
// If you are using a ridiculous number of iterations it could help though.
|
||||
#define MAX_THREADS 2
|
||||
|
||||
struct ThreadContext {
|
||||
pthread_t thread;
|
||||
cpHastySpace *space;
|
||||
unsigned long thread_num;
|
||||
};
|
||||
|
||||
typedef void (*cpHastySpaceWorkFunction)(cpSpace *space, unsigned long worker, unsigned long worker_count);
|
||||
|
||||
struct cpHastySpace {
|
||||
cpSpace space;
|
||||
|
||||
// Number of worker threads (including the main thread)
|
||||
unsigned long num_threads;
|
||||
|
||||
// Number of worker threads currently executing. (also including the main thread)
|
||||
unsigned long num_working;
|
||||
|
||||
// Number of constraints (plus contacts) that must exist per step to start the worker threads.
|
||||
unsigned long constraint_count_threshold;
|
||||
|
||||
pthread_mutex_t mutex;
|
||||
pthread_cond_t cond_work, cond_resume;
|
||||
|
||||
// Work function to invoke.
|
||||
cpHastySpaceWorkFunction work;
|
||||
|
||||
struct ThreadContext workers[MAX_THREADS - 1];
|
||||
};
|
||||
|
||||
static void *
|
||||
WorkerThreadLoop(struct ThreadContext *context)
|
||||
{
|
||||
cpHastySpace *hasty = context->space;
|
||||
|
||||
unsigned long thread = context->thread_num;
|
||||
unsigned long num_threads = hasty->num_threads;
|
||||
|
||||
for(;;){
|
||||
pthread_mutex_lock(&hasty->mutex); {
|
||||
if(--hasty->num_working == 0){
|
||||
pthread_cond_signal(&hasty->cond_resume);
|
||||
}
|
||||
|
||||
pthread_cond_wait(&hasty->cond_work, &hasty->mutex);
|
||||
} pthread_mutex_unlock(&hasty->mutex);
|
||||
|
||||
cpHastySpaceWorkFunction func = hasty->work;
|
||||
if(func){
|
||||
hasty->work(&hasty->space, thread, num_threads);
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static void
|
||||
RunWorkers(cpHastySpace *hasty, cpHastySpaceWorkFunction func)
|
||||
{
|
||||
hasty->num_working = hasty->num_threads - 1;
|
||||
hasty->work = func;
|
||||
|
||||
if(hasty->num_working > 0){
|
||||
pthread_mutex_lock(&hasty->mutex); {
|
||||
pthread_cond_broadcast(&hasty->cond_work);
|
||||
} pthread_mutex_unlock(&hasty->mutex);
|
||||
|
||||
func((cpSpace *)hasty, 0, hasty->num_threads);
|
||||
|
||||
pthread_mutex_lock(&hasty->mutex); {
|
||||
if(hasty->num_working > 0){
|
||||
pthread_cond_wait(&hasty->cond_resume, &hasty->mutex);
|
||||
}
|
||||
} pthread_mutex_unlock(&hasty->mutex);
|
||||
} else {
|
||||
func((cpSpace *)hasty, 0, hasty->num_threads);
|
||||
}
|
||||
|
||||
hasty->work = NULL;
|
||||
}
|
||||
|
||||
static void
|
||||
Solver(cpSpace *space, unsigned long worker, unsigned long worker_count)
|
||||
{
|
||||
cpArray *constraints = space->constraints;
|
||||
cpArray *arbiters = space->arbiters;
|
||||
|
||||
cpFloat dt = space->curr_dt;
|
||||
unsigned long iterations = (space->iterations + worker_count - 1)/worker_count;
|
||||
|
||||
for(unsigned long i=0; i<iterations; i++){
|
||||
for(int j=0; j<arbiters->num; j++){
|
||||
cpArbiter *arb = (cpArbiter *)arbiters->arr[j];
|
||||
#ifdef __ARM_NEON__
|
||||
cpArbiterApplyImpulse_NEON(arb);
|
||||
#else
|
||||
cpArbiterApplyImpulse(arb);
|
||||
#endif
|
||||
}
|
||||
|
||||
for(int j=0; j<constraints->num; j++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
|
||||
constraint->klass->applyImpulse(constraint, dt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Thread Management Functions
|
||||
|
||||
static void
|
||||
HaltThreads(cpHastySpace *hasty)
|
||||
{
|
||||
pthread_mutex_t *mutex = &hasty->mutex;
|
||||
pthread_mutex_lock(mutex); {
|
||||
hasty->work = NULL; // NULL work function means break and exit
|
||||
pthread_cond_broadcast(&hasty->cond_work);
|
||||
} pthread_mutex_unlock(mutex);
|
||||
|
||||
for(unsigned long i=0; i<(hasty->num_threads-1); i++){
|
||||
pthread_join(hasty->workers[i].thread, NULL);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpHastySpaceSetThreads(cpSpace *space, unsigned long threads)
|
||||
{
|
||||
#if TARGET_IPHONE_SIMULATOR == 1
|
||||
// Individual values appear to be written non-atomically when compiled as debug for the simulator.
|
||||
// No idea why, so threads are disabled.
|
||||
threads = 1;
|
||||
#endif
|
||||
|
||||
cpHastySpace *hasty = (cpHastySpace *)space;
|
||||
HaltThreads(hasty);
|
||||
|
||||
#ifdef __APPLE__
|
||||
if(threads == 0){
|
||||
size_t size = sizeof(threads);
|
||||
sysctlbyname("hw.ncpu", &threads, &size, NULL, 0);
|
||||
}
|
||||
#else
|
||||
if(threads == 0) threads = 1;
|
||||
#endif
|
||||
|
||||
hasty->num_threads = (threads < MAX_THREADS ? threads : MAX_THREADS);
|
||||
hasty->num_working = hasty->num_threads - 1;
|
||||
|
||||
// Create the worker threads and wait for them to signal ready.
|
||||
if(hasty->num_working > 0){
|
||||
pthread_mutex_lock(&hasty->mutex);
|
||||
for(unsigned long i=0; i<(hasty->num_threads-1); i++){
|
||||
hasty->workers[i].space = hasty;
|
||||
hasty->workers[i].thread_num = i + 1;
|
||||
|
||||
pthread_create(&hasty->workers[i].thread, NULL, (void*(*)(void*))WorkerThreadLoop, &hasty->workers[i]);
|
||||
}
|
||||
|
||||
pthread_cond_wait(&hasty->cond_resume, &hasty->mutex);
|
||||
pthread_mutex_unlock(&hasty->mutex);
|
||||
}
|
||||
}
|
||||
|
||||
unsigned long
|
||||
cpHastySpaceGetThreads(cpSpace *space)
|
||||
{
|
||||
return ((cpHastySpace *)space)->num_threads;
|
||||
}
|
||||
|
||||
//MARK: Overriden cpSpace Functions.
|
||||
|
||||
cpSpace *
|
||||
cpHastySpaceNew(void)
|
||||
{
|
||||
cpHastySpace *hasty = (cpHastySpace *)cpcalloc(1, sizeof(cpHastySpace));
|
||||
cpSpaceInit((cpSpace *)hasty);
|
||||
|
||||
pthread_mutex_init(&hasty->mutex, NULL);
|
||||
pthread_cond_init(&hasty->cond_work, NULL);
|
||||
pthread_cond_init(&hasty->cond_resume, NULL);
|
||||
|
||||
// TODO magic number, should test this more thoroughly.
|
||||
hasty->constraint_count_threshold = 50;
|
||||
|
||||
// Default to 1 thread for determinism.
|
||||
hasty->num_threads = 1;
|
||||
cpHastySpaceSetThreads((cpSpace *)hasty, 1);
|
||||
|
||||
return (cpSpace *)hasty;
|
||||
}
|
||||
|
||||
void
|
||||
cpHastySpaceFree(cpSpace *space)
|
||||
{
|
||||
cpHastySpace *hasty = (cpHastySpace *)space;
|
||||
|
||||
HaltThreads(hasty);
|
||||
|
||||
pthread_mutex_destroy(&hasty->mutex);
|
||||
pthread_cond_destroy(&hasty->cond_work);
|
||||
pthread_cond_destroy(&hasty->cond_resume);
|
||||
|
||||
cpSpaceFree(space);
|
||||
}
|
||||
|
||||
void
|
||||
cpHastySpaceStep(cpSpace *space, cpFloat dt)
|
||||
{
|
||||
// don't step if the timestep is 0!
|
||||
if(dt == 0.0f) return;
|
||||
|
||||
space->stamp++;
|
||||
|
||||
cpFloat prev_dt = space->curr_dt;
|
||||
space->curr_dt = dt;
|
||||
|
||||
cpArray *bodies = space->dynamicBodies;
|
||||
cpArray *constraints = space->constraints;
|
||||
cpArray *arbiters = space->arbiters;
|
||||
|
||||
// Reset and empty the arbiter list.
|
||||
for(int i=0; i<arbiters->num; i++){
|
||||
cpArbiter *arb = (cpArbiter *)arbiters->arr[i];
|
||||
arb->state = CP_ARBITER_STATE_NORMAL;
|
||||
|
||||
// If both bodies are awake, unthread the arbiter from the contact graph.
|
||||
if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)){
|
||||
cpArbiterUnthread(arb);
|
||||
}
|
||||
}
|
||||
arbiters->num = 0;
|
||||
|
||||
cpSpaceLock(space); {
|
||||
// Integrate positions
|
||||
for(int i=0; i<bodies->num; i++){
|
||||
cpBody *body = (cpBody *)bodies->arr[i];
|
||||
body->position_func(body, dt);
|
||||
}
|
||||
|
||||
// Find colliding pairs.
|
||||
cpSpacePushFreshContactBuffer(space);
|
||||
cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL);
|
||||
cpSpatialIndexReindexQuery(space->dynamicShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space);
|
||||
} cpSpaceUnlock(space, cpFalse);
|
||||
|
||||
// Rebuild the contact graph (and detect sleeping components if sleeping is enabled)
|
||||
cpSpaceProcessComponents(space, dt);
|
||||
|
||||
cpSpaceLock(space); {
|
||||
// Clear out old cached arbiters and call separate callbacks
|
||||
cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space);
|
||||
|
||||
// Prestep the arbiters and constraints.
|
||||
cpFloat slop = space->collisionSlop;
|
||||
cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt);
|
||||
for(int i=0; i<arbiters->num; i++){
|
||||
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef);
|
||||
}
|
||||
|
||||
for(int i=0; i<constraints->num; i++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
|
||||
|
||||
cpConstraintPreSolveFunc preSolve = constraint->preSolve;
|
||||
if(preSolve) preSolve(constraint, space);
|
||||
|
||||
constraint->klass->preStep(constraint, dt);
|
||||
}
|
||||
|
||||
// Integrate velocities.
|
||||
cpFloat damping = cpfpow(space->damping, dt);
|
||||
cpVect gravity = space->gravity;
|
||||
for(int i=0; i<bodies->num; i++){
|
||||
cpBody *body = (cpBody *)bodies->arr[i];
|
||||
body->velocity_func(body, gravity, damping, dt);
|
||||
}
|
||||
|
||||
// Apply cached impulses
|
||||
cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt);
|
||||
for(int i=0; i<arbiters->num; i++){
|
||||
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef);
|
||||
}
|
||||
|
||||
for(int i=0; i<constraints->num; i++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
|
||||
constraint->klass->applyCachedImpulse(constraint, dt_coef);
|
||||
}
|
||||
|
||||
// Run the impulse solver.
|
||||
cpHastySpace *hasty = (cpHastySpace *)space;
|
||||
if((unsigned long)(arbiters->num + constraints->num) > hasty->constraint_count_threshold){
|
||||
RunWorkers(hasty, Solver);
|
||||
} else {
|
||||
Solver(space, 0, 1);
|
||||
}
|
||||
|
||||
// Run the constraint post-solve callbacks
|
||||
for(int i=0; i<constraints->num; i++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
|
||||
|
||||
cpConstraintPostSolveFunc postSolve = constraint->postSolve;
|
||||
if(postSolve) postSolve(constraint, space);
|
||||
}
|
||||
|
||||
// run the post-solve callbacks
|
||||
for(int i=0; i<arbiters->num; i++){
|
||||
cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
|
||||
|
||||
cpCollisionHandler *handler = arb->handler;
|
||||
handler->postSolveFunc(arb, space, handler->userData);
|
||||
}
|
||||
} cpSpaceUnlock(space, cpTrue);
|
||||
}
|
||||
|
|
@ -0,0 +1,157 @@
|
|||
// Copyright 2013 Howling Moon Software. All rights reserved.
|
||||
// See http://chipmunk2d.net/legal.php for more information.
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "chipmunk/chipmunk.h"
|
||||
#include "chipmunk/cpMarch.h"
|
||||
|
||||
|
||||
typedef void (*cpMarchCellFunc)(
|
||||
cpFloat t, cpFloat a, cpFloat b, cpFloat c, cpFloat d,
|
||||
cpFloat x0, cpFloat x1, cpFloat y0, cpFloat y1,
|
||||
cpMarchSegmentFunc segment, void *segment_data
|
||||
);
|
||||
|
||||
// The looping and sample caching code is shared between cpMarchHard() and cpMarchSoft().
|
||||
static void
|
||||
cpMarchCells(
|
||||
cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat t,
|
||||
cpMarchSegmentFunc segment, void *segment_data,
|
||||
cpMarchSampleFunc sample, void *sample_data,
|
||||
cpMarchCellFunc cell
|
||||
){
|
||||
cpFloat x_denom = 1.0/(cpFloat)(x_samples - 1);
|
||||
cpFloat y_denom = 1.0/(cpFloat)(y_samples - 1);
|
||||
|
||||
// TODO range assertions and short circuit for 0 sized windows.
|
||||
|
||||
// Keep a copy of the previous row to avoid double lookups.
|
||||
cpFloat *buffer = (cpFloat *)cpcalloc(x_samples, sizeof(cpFloat));
|
||||
for(unsigned long i=0; i<x_samples; i++) buffer[i] = sample(cpv(cpflerp(bb.l, bb.r, i*x_denom), bb.b), sample_data);
|
||||
|
||||
for(unsigned long j=0; j<y_samples-1; j++){
|
||||
cpFloat y0 = cpflerp(bb.b, bb.t, (j+0)*y_denom);
|
||||
cpFloat y1 = cpflerp(bb.b, bb.t, (j+1)*y_denom);
|
||||
|
||||
cpFloat a, b = buffer[0];
|
||||
cpFloat c, d = sample(cpv(bb.l, y1), sample_data);
|
||||
buffer[0] = d;
|
||||
|
||||
for(unsigned long i=0; i<x_samples-1; i++){
|
||||
cpFloat x0 = cpflerp(bb.l, bb.r, (i+0)*x_denom);
|
||||
cpFloat x1 = cpflerp(bb.l, bb.r, (i+1)*x_denom);
|
||||
|
||||
a = b, b = buffer[i + 1];
|
||||
c = d, d = sample(cpv(x1, y1), sample_data);
|
||||
buffer[i + 1] = d;
|
||||
|
||||
cell(t, a, b, c, d, x0, x1, y0, y1, segment, segment_data);
|
||||
}
|
||||
}
|
||||
|
||||
cpfree(buffer);
|
||||
}
|
||||
|
||||
|
||||
// TODO should flip this around eventually.
|
||||
static inline void
|
||||
seg(cpVect v0, cpVect v1, cpMarchSegmentFunc f, void *data)
|
||||
{
|
||||
if(!cpveql(v0, v1)) f(v1, v0, data);
|
||||
}
|
||||
|
||||
// Lerps between two positions based on their sample values.
|
||||
static inline cpFloat
|
||||
midlerp(cpFloat x0, cpFloat x1, cpFloat s0, cpFloat s1, cpFloat t)
|
||||
{
|
||||
return cpflerp(x0, x1, (t - s0)/(s1 - s0));
|
||||
}
|
||||
|
||||
static void
|
||||
cpMarchCellSoft(
|
||||
cpFloat t, cpFloat a, cpFloat b, cpFloat c, cpFloat d,
|
||||
cpFloat x0, cpFloat x1, cpFloat y0, cpFloat y1,
|
||||
cpMarchSegmentFunc segment, void *segment_data
|
||||
){
|
||||
// TODO this switch part is super expensive, can it be NEONized?
|
||||
switch((a>t)<<0 | (b>t)<<1 | (c>t)<<2 | (d>t)<<3){
|
||||
case 0x1: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data); break;
|
||||
case 0x2: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data); break;
|
||||
case 0x3: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data); break;
|
||||
case 0x4: seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break;
|
||||
case 0x5: seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data); break;
|
||||
case 0x6: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data);
|
||||
seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break;
|
||||
case 0x7: seg(cpv(midlerp(x0,x1,c,d,t), y1), cpv(x1, midlerp(y0,y1,b,d,t)), segment, segment_data); break;
|
||||
case 0x8: seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break;
|
||||
case 0x9: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data);
|
||||
seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break;
|
||||
case 0xA: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break;
|
||||
case 0xB: seg(cpv(x0, midlerp(y0,y1,a,c,t)), cpv(midlerp(x0,x1,c,d,t), y1), segment, segment_data); break;
|
||||
case 0xC: seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break;
|
||||
case 0xD: seg(cpv(x1, midlerp(y0,y1,b,d,t)), cpv(midlerp(x0,x1,a,b,t), y0), segment, segment_data); break;
|
||||
case 0xE: seg(cpv(midlerp(x0,x1,a,b,t), y0), cpv(x0, midlerp(y0,y1,a,c,t)), segment, segment_data); break;
|
||||
default: break; // 0x0 and 0xF
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpMarchSoft(
|
||||
cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat t,
|
||||
cpMarchSegmentFunc segment, void *segment_data,
|
||||
cpMarchSampleFunc sample, void *sample_data
|
||||
){
|
||||
cpMarchCells(bb, x_samples, y_samples, t, segment, segment_data, sample, sample_data, cpMarchCellSoft);
|
||||
}
|
||||
|
||||
|
||||
// TODO should flip this around eventually.
|
||||
static inline void
|
||||
segs(cpVect a, cpVect b, cpVect c, cpMarchSegmentFunc f, void *data)
|
||||
{
|
||||
seg(b, c, f, data);
|
||||
seg(a, b, f, data);
|
||||
}
|
||||
|
||||
static void
|
||||
cpMarchCellHard(
|
||||
cpFloat t, cpFloat a, cpFloat b, cpFloat c, cpFloat d,
|
||||
cpFloat x0, cpFloat x1, cpFloat y0, cpFloat y1,
|
||||
cpMarchSegmentFunc segment, void *segment_data
|
||||
){
|
||||
// midpoints
|
||||
cpFloat xm = cpflerp(x0, x1, 0.5f);
|
||||
cpFloat ym = cpflerp(y0, y1, 0.5f);
|
||||
|
||||
switch((a>t)<<0 | (b>t)<<1 | (c>t)<<2 | (d>t)<<3){
|
||||
case 0x1: segs(cpv(x0, ym), cpv(xm, ym), cpv(xm, y0), segment, segment_data); break;
|
||||
case 0x2: segs(cpv(xm, y0), cpv(xm, ym), cpv(x1, ym), segment, segment_data); break;
|
||||
case 0x3: seg(cpv(x0, ym), cpv(x1, ym), segment, segment_data); break;
|
||||
case 0x4: segs(cpv(xm, y1), cpv(xm, ym), cpv(x0, ym), segment, segment_data); break;
|
||||
case 0x5: seg(cpv(xm, y1), cpv(xm, y0), segment, segment_data); break;
|
||||
case 0x6: segs(cpv(xm, y0), cpv(xm, ym), cpv(x0, ym), segment, segment_data);
|
||||
segs(cpv(xm, y1), cpv(xm, ym), cpv(x1, ym), segment, segment_data); break;
|
||||
case 0x7: segs(cpv(xm, y1), cpv(xm, ym), cpv(x1, ym), segment, segment_data); break;
|
||||
case 0x8: segs(cpv(x1, ym), cpv(xm, ym), cpv(xm, y1), segment, segment_data); break;
|
||||
case 0x9: segs(cpv(x1, ym), cpv(xm, ym), cpv(xm, y0), segment, segment_data);
|
||||
segs(cpv(x0, ym), cpv(xm, ym), cpv(xm, y1), segment, segment_data); break;
|
||||
case 0xA: seg(cpv(xm, y0), cpv(xm, y1), segment, segment_data); break;
|
||||
case 0xB: segs(cpv(x0, ym), cpv(xm, ym), cpv(xm, y1), segment, segment_data); break;
|
||||
case 0xC: seg(cpv(x1, ym), cpv(x0, ym), segment, segment_data); break;
|
||||
case 0xD: segs(cpv(x1, ym), cpv(xm, ym), cpv(xm, y0), segment, segment_data); break;
|
||||
case 0xE: segs(cpv(xm, y0), cpv(xm, ym), cpv(x0, ym), segment, segment_data); break;
|
||||
default: break; // 0x0 and 0xF
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpMarchHard(
|
||||
cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat t,
|
||||
cpMarchSegmentFunc segment, void *segment_data,
|
||||
cpMarchSampleFunc sample, void *sample_data
|
||||
){
|
||||
cpMarchCells(bb, x_samples, y_samples, t, segment, segment_data, sample, sample_data, cpMarchCellHard);
|
||||
}
|
||||
|
|
@ -0,0 +1,172 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
static void
|
||||
preStep(cpPinJoint *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog));
|
||||
joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
|
||||
|
||||
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
|
||||
cpFloat dist = cpvlength(delta);
|
||||
joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
|
||||
|
||||
// calculate mass normal
|
||||
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
|
||||
|
||||
// calculate bias velocity
|
||||
cpFloat maxBias = joint->constraint.maxBias;
|
||||
joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(dist - joint->dist)/dt, -maxBias, maxBias);
|
||||
}
|
||||
|
||||
static void
|
||||
applyCachedImpulse(cpPinJoint *joint, cpFloat dt_coef)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef);
|
||||
apply_impulses(a, b, joint->r1, joint->r2, j);
|
||||
}
|
||||
|
||||
static void
|
||||
applyImpulse(cpPinJoint *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
cpVect n = joint->n;
|
||||
|
||||
// compute relative velocity
|
||||
cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n);
|
||||
|
||||
cpFloat jnMax = joint->constraint.maxForce*dt;
|
||||
|
||||
// compute normal impulse
|
||||
cpFloat jn = (joint->bias - vrn)*joint->nMass;
|
||||
cpFloat jnOld = joint->jnAcc;
|
||||
joint->jnAcc = cpfclamp(jnOld + jn, -jnMax, jnMax);
|
||||
jn = joint->jnAcc - jnOld;
|
||||
|
||||
// apply impulse
|
||||
apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
|
||||
}
|
||||
|
||||
static cpFloat
|
||||
getImpulse(cpPinJoint *joint)
|
||||
{
|
||||
return cpfabs(joint->jnAcc);
|
||||
}
|
||||
|
||||
static const cpConstraintClass klass = {
|
||||
(cpConstraintPreStepImpl)preStep,
|
||||
(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
|
||||
(cpConstraintApplyImpulseImpl)applyImpulse,
|
||||
(cpConstraintGetImpulseImpl)getImpulse,
|
||||
};
|
||||
|
||||
|
||||
cpPinJoint *
|
||||
cpPinJointAlloc(void)
|
||||
{
|
||||
return (cpPinJoint *)cpcalloc(1, sizeof(cpPinJoint));
|
||||
}
|
||||
|
||||
cpPinJoint *
|
||||
cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB)
|
||||
{
|
||||
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
||||
|
||||
joint->anchorA = anchorA;
|
||||
joint->anchorB = anchorB;
|
||||
|
||||
// STATIC_BODY_CHECK
|
||||
cpVect p1 = (a ? cpTransformPoint(a->transform, anchorA) : anchorA);
|
||||
cpVect p2 = (b ? cpTransformPoint(b->transform, anchorB) : anchorB);
|
||||
joint->dist = cpvlength(cpvsub(p2, p1));
|
||||
|
||||
cpAssertWarn(joint->dist > 0.0, "You created a 0 length pin joint. A pivot joint will be much more stable.");
|
||||
|
||||
joint->jnAcc = 0.0f;
|
||||
|
||||
return joint;
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpPinJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB)
|
||||
{
|
||||
return (cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchorA, anchorB);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpConstraintIsPinJoint(const cpConstraint *constraint)
|
||||
{
|
||||
return (constraint->klass == &klass);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpPinJointGetAnchorA(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
|
||||
return ((cpPinJoint *)constraint)->anchorA;
|
||||
}
|
||||
|
||||
void
|
||||
cpPinJointSetAnchorA(cpConstraint *constraint, cpVect anchorA)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpPinJoint *)constraint)->anchorA = anchorA;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpPinJointGetAnchorB(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
|
||||
return ((cpPinJoint *)constraint)->anchorB;
|
||||
}
|
||||
|
||||
void
|
||||
cpPinJointSetAnchorB(cpConstraint *constraint, cpVect anchorB)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpPinJoint *)constraint)->anchorB = anchorB;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpPinJointGetDist(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
|
||||
return ((cpPinJoint *)constraint)->dist;
|
||||
}
|
||||
|
||||
void
|
||||
cpPinJointSetDist(cpConstraint *constraint, cpFloat dist)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpPinJoint *)constraint)->dist = dist;
|
||||
}
|
||||
|
|
@ -0,0 +1,152 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
static void
|
||||
preStep(cpPivotJoint *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog));
|
||||
joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
|
||||
|
||||
// Calculate mass tensor
|
||||
joint-> k = k_tensor(a, b, joint->r1, joint->r2);
|
||||
|
||||
// calculate bias velocity
|
||||
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
|
||||
joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
|
||||
}
|
||||
|
||||
static void
|
||||
applyCachedImpulse(cpPivotJoint *joint, cpFloat dt_coef)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef));
|
||||
}
|
||||
|
||||
static void
|
||||
applyImpulse(cpPivotJoint *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
cpVect r1 = joint->r1;
|
||||
cpVect r2 = joint->r2;
|
||||
|
||||
// compute relative velocity
|
||||
cpVect vr = relative_velocity(a, b, r1, r2);
|
||||
|
||||
// compute normal impulse
|
||||
cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr));
|
||||
cpVect jOld = joint->jAcc;
|
||||
joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->constraint.maxForce*dt);
|
||||
j = cpvsub(joint->jAcc, jOld);
|
||||
|
||||
// apply impulse
|
||||
apply_impulses(a, b, joint->r1, joint->r2, j);
|
||||
}
|
||||
|
||||
static cpFloat
|
||||
getImpulse(cpConstraint *joint)
|
||||
{
|
||||
return cpvlength(((cpPivotJoint *)joint)->jAcc);
|
||||
}
|
||||
|
||||
static const cpConstraintClass klass = {
|
||||
(cpConstraintPreStepImpl)preStep,
|
||||
(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
|
||||
(cpConstraintApplyImpulseImpl)applyImpulse,
|
||||
(cpConstraintGetImpulseImpl)getImpulse,
|
||||
};
|
||||
|
||||
cpPivotJoint *
|
||||
cpPivotJointAlloc(void)
|
||||
{
|
||||
return (cpPivotJoint *)cpcalloc(1, sizeof(cpPivotJoint));
|
||||
}
|
||||
|
||||
cpPivotJoint *
|
||||
cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB)
|
||||
{
|
||||
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
||||
|
||||
joint->anchorA = anchorA;
|
||||
joint->anchorB = anchorB;
|
||||
|
||||
joint->jAcc = cpvzero;
|
||||
|
||||
return joint;
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB)
|
||||
{
|
||||
return (cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchorA, anchorB);
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
|
||||
{
|
||||
cpVect anchorA = (a ? cpBodyWorldToLocal(a, pivot) : pivot);
|
||||
cpVect anchorB = (b ? cpBodyWorldToLocal(b, pivot) : pivot);
|
||||
return cpPivotJointNew2(a, b, anchorA, anchorB);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpConstraintIsPivotJoint(const cpConstraint *constraint)
|
||||
{
|
||||
return (constraint->klass == &klass);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpPivotJointGetAnchorA(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint.");
|
||||
return ((cpPivotJoint *)constraint)->anchorA;
|
||||
}
|
||||
|
||||
void
|
||||
cpPivotJointSetAnchorA(cpConstraint *constraint, cpVect anchorA)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpPivotJoint *)constraint)->anchorA = anchorA;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpPivotJointGetAnchorB(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint.");
|
||||
return ((cpPivotJoint *)constraint)->anchorB;
|
||||
}
|
||||
|
||||
void
|
||||
cpPivotJointSetAnchorB(cpConstraint *constraint, cpVect anchorB)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpPivotJoint *)constraint)->anchorB = anchorB;
|
||||
}
|
||||
|
|
@ -0,0 +1,323 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
#include "chipmunk/chipmunk_unsafe.h"
|
||||
|
||||
cpPolyShape *
|
||||
cpPolyShapeAlloc(void)
|
||||
{
|
||||
return (cpPolyShape *)cpcalloc(1, sizeof(cpPolyShape));
|
||||
}
|
||||
|
||||
static void
|
||||
cpPolyShapeDestroy(cpPolyShape *poly)
|
||||
{
|
||||
if(poly->count > CP_POLY_SHAPE_INLINE_ALLOC){
|
||||
cpfree(poly->planes);
|
||||
}
|
||||
}
|
||||
|
||||
static cpBB
|
||||
cpPolyShapeCacheData(cpPolyShape *poly, cpTransform transform)
|
||||
{
|
||||
int count = poly->count;
|
||||
struct cpSplittingPlane *dst = poly->planes;
|
||||
struct cpSplittingPlane *src = dst + count;
|
||||
|
||||
cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY;
|
||||
cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)INFINITY;
|
||||
|
||||
for(int i=0; i<count; i++){
|
||||
cpVect v = cpTransformPoint(transform, src[i].v0);
|
||||
cpVect n = cpTransformVect(transform, src[i].n);
|
||||
|
||||
dst[i].v0 = v;
|
||||
dst[i].n = n;
|
||||
|
||||
l = cpfmin(l, v.x);
|
||||
r = cpfmax(r, v.x);
|
||||
b = cpfmin(b, v.y);
|
||||
t = cpfmax(t, v.y);
|
||||
}
|
||||
|
||||
cpFloat radius = poly->r;
|
||||
return (poly->shape.bb = cpBBNew(l - radius, b - radius, r + radius, t + radius));
|
||||
}
|
||||
|
||||
static void
|
||||
cpPolyShapePointQuery(cpPolyShape *poly, cpVect p, cpPointQueryInfo *info){
|
||||
int count = poly->count;
|
||||
struct cpSplittingPlane *planes = poly->planes;
|
||||
cpFloat r = poly->r;
|
||||
|
||||
cpVect v0 = planes[count - 1].v0;
|
||||
cpFloat minDist = INFINITY;
|
||||
cpVect closestPoint = cpvzero;
|
||||
cpVect closestNormal = cpvzero;
|
||||
cpBool outside = cpFalse;
|
||||
|
||||
for(int i=0; i<count; i++){
|
||||
cpVect v1 = planes[i].v0;
|
||||
outside = outside || (cpvdot(planes[i].n, cpvsub(p,v1)) > 0.0f);
|
||||
|
||||
cpVect closest = cpClosetPointOnSegment(p, v0, v1);
|
||||
|
||||
cpFloat dist = cpvdist(p, closest);
|
||||
if(dist < minDist){
|
||||
minDist = dist;
|
||||
closestPoint = closest;
|
||||
closestNormal = planes[i].n;
|
||||
}
|
||||
|
||||
v0 = v1;
|
||||
}
|
||||
|
||||
cpFloat dist = (outside ? minDist : -minDist);
|
||||
cpVect g = cpvmult(cpvsub(p, closestPoint), 1.0f/dist);
|
||||
|
||||
info->shape = (cpShape *)poly;
|
||||
info->point = cpvadd(closestPoint, cpvmult(g, r));
|
||||
info->distance = dist - r;
|
||||
|
||||
// Use the normal of the closest segment if the distance is small.
|
||||
info->gradient = (minDist > MAGIC_EPSILON ? g : closestNormal);
|
||||
}
|
||||
|
||||
static void
|
||||
cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
|
||||
{
|
||||
struct cpSplittingPlane *planes = poly->planes;
|
||||
int count = poly->count;
|
||||
cpFloat r = poly->r;
|
||||
cpFloat rsum = r + r2;
|
||||
|
||||
for(int i=0; i<count; i++){
|
||||
cpVect n = planes[i].n;
|
||||
cpFloat an = cpvdot(a, n);
|
||||
cpFloat d = an - cpvdot(planes[i].v0, n) - rsum;
|
||||
if(d < 0.0f) continue;
|
||||
|
||||
cpFloat bn = cpvdot(b, n);
|
||||
cpFloat t = d/(an - bn);
|
||||
if(t < 0.0f || 1.0f < t) continue;
|
||||
|
||||
cpVect point = cpvlerp(a, b, t);
|
||||
cpFloat dt = cpvcross(n, point);
|
||||
cpFloat dtMin = cpvcross(n, planes[(i - 1 + count)%count].v0);
|
||||
cpFloat dtMax = cpvcross(n, planes[i].v0);
|
||||
|
||||
if(dtMin <= dt && dt <= dtMax){
|
||||
info->shape = (cpShape *)poly;
|
||||
info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2));
|
||||
info->normal = n;
|
||||
info->alpha = t;
|
||||
}
|
||||
}
|
||||
|
||||
// Also check against the beveled vertexes.
|
||||
if(rsum > 0.0f){
|
||||
for(int i=0; i<count; i++){
|
||||
cpSegmentQueryInfo circle_info = {NULL, b, cpvzero, 1.0f};
|
||||
CircleSegmentQuery(&poly->shape, planes[i].v0, r, a, b, r2, &circle_info);
|
||||
if(circle_info.alpha < info->alpha) (*info) = circle_info;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
SetVerts(cpPolyShape *poly, int count, const cpVect *verts)
|
||||
{
|
||||
poly->count = count;
|
||||
if(count <= CP_POLY_SHAPE_INLINE_ALLOC){
|
||||
poly->planes = poly->_planes;
|
||||
} else {
|
||||
poly->planes = (struct cpSplittingPlane *)cpcalloc(2*count, sizeof(struct cpSplittingPlane));
|
||||
}
|
||||
|
||||
for(int i=0; i<count; i++){
|
||||
cpVect a = verts[(i - 1 + count)%count];
|
||||
cpVect b = verts[i];
|
||||
cpVect n = cpvnormalize(cpvrperp(cpvsub(b, a)));
|
||||
|
||||
poly->planes[i + count].v0 = b;
|
||||
poly->planes[i + count].n = n;
|
||||
}
|
||||
}
|
||||
|
||||
static struct cpShapeMassInfo
|
||||
cpPolyShapeMassInfo(cpFloat mass, int count, const cpVect *verts, cpFloat radius)
|
||||
{
|
||||
// TODO moment is approximate due to radius.
|
||||
|
||||
cpVect centroid = cpCentroidForPoly(count, verts);
|
||||
struct cpShapeMassInfo info = {
|
||||
mass, cpMomentForPoly(1.0f, count, verts, cpvneg(centroid), radius),
|
||||
centroid,
|
||||
cpAreaForPoly(count, verts, radius),
|
||||
};
|
||||
|
||||
return info;
|
||||
}
|
||||
|
||||
static const cpShapeClass polyClass = {
|
||||
CP_POLY_SHAPE,
|
||||
(cpShapeCacheDataImpl)cpPolyShapeCacheData,
|
||||
(cpShapeDestroyImpl)cpPolyShapeDestroy,
|
||||
(cpShapePointQueryImpl)cpPolyShapePointQuery,
|
||||
(cpShapeSegmentQueryImpl)cpPolyShapeSegmentQuery,
|
||||
};
|
||||
|
||||
cpPolyShape *
|
||||
cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius)
|
||||
{
|
||||
cpVect *hullVerts = (cpVect *)alloca(count*sizeof(cpVect));
|
||||
|
||||
// Transform the verts before building the hull in case of a negative scale.
|
||||
for(int i=0; i<count; i++) hullVerts[i] = cpTransformPoint(transform, verts[i]);
|
||||
|
||||
unsigned int hullCount = cpConvexHull(count, hullVerts, hullVerts, NULL, 0.0);
|
||||
return cpPolyShapeInitRaw(poly, body, hullCount, hullVerts, radius);
|
||||
}
|
||||
|
||||
cpPolyShape *
|
||||
cpPolyShapeInitRaw(cpPolyShape *poly, cpBody *body, int count, const cpVect *verts, cpFloat radius)
|
||||
{
|
||||
cpShapeInit((cpShape *)poly, &polyClass, body, cpPolyShapeMassInfo(0.0f, count, verts, radius));
|
||||
|
||||
SetVerts(poly, count, verts);
|
||||
poly->r = radius;
|
||||
|
||||
return poly;
|
||||
}
|
||||
|
||||
cpShape *
|
||||
cpPolyShapeNew(cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius)
|
||||
{
|
||||
return (cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), body, count, verts, transform, radius);
|
||||
}
|
||||
|
||||
cpShape *
|
||||
cpPolyShapeNewRaw(cpBody *body, int count, const cpVect *verts, cpFloat radius)
|
||||
{
|
||||
return (cpShape *)cpPolyShapeInitRaw(cpPolyShapeAlloc(), body, count, verts, radius);
|
||||
}
|
||||
|
||||
cpPolyShape *
|
||||
cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height, cpFloat radius)
|
||||
{
|
||||
cpFloat hw = width/2.0f;
|
||||
cpFloat hh = height/2.0f;
|
||||
|
||||
return cpBoxShapeInit2(poly, body, cpBBNew(-hw, -hh, hw, hh), radius);
|
||||
}
|
||||
|
||||
cpPolyShape *
|
||||
cpBoxShapeInit2(cpPolyShape *poly, cpBody *body, cpBB box, cpFloat radius)
|
||||
{
|
||||
cpVect verts[] = {
|
||||
cpv(box.r, box.b),
|
||||
cpv(box.r, box.t),
|
||||
cpv(box.l, box.t),
|
||||
cpv(box.l, box.b),
|
||||
};
|
||||
|
||||
return cpPolyShapeInitRaw(poly, body, 4, verts, radius);
|
||||
}
|
||||
|
||||
cpShape *
|
||||
cpBoxShapeNew(cpBody *body, cpFloat width, cpFloat height, cpFloat radius)
|
||||
{
|
||||
return (cpShape *)cpBoxShapeInit(cpPolyShapeAlloc(), body, width, height, radius);
|
||||
}
|
||||
|
||||
cpShape *
|
||||
cpBoxShapeNew2(cpBody *body, cpBB box, cpFloat radius)
|
||||
{
|
||||
return (cpShape *)cpBoxShapeInit2(cpPolyShapeAlloc(), body, box, radius);
|
||||
}
|
||||
|
||||
int
|
||||
cpPolyShapeGetCount(const cpShape *shape)
|
||||
{
|
||||
cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
|
||||
return ((cpPolyShape *)shape)->count;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpPolyShapeGetVert(const cpShape *shape, int i)
|
||||
{
|
||||
cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
|
||||
|
||||
int count = cpPolyShapeGetCount(shape);
|
||||
cpAssertHard(0 <= i && i < count, "Index out of range.");
|
||||
|
||||
return ((cpPolyShape *)shape)->planes[i + count].v0;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpPolyShapeGetRadius(const cpShape *shape)
|
||||
{
|
||||
cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
|
||||
return ((cpPolyShape *)shape)->r;
|
||||
}
|
||||
|
||||
// Unsafe API (chipmunk_unsafe.h)
|
||||
|
||||
void
|
||||
cpPolyShapeSetVerts(cpShape *shape, int count, cpVect *verts, cpTransform transform)
|
||||
{
|
||||
cpVect *hullVerts = (cpVect *)alloca(count*sizeof(cpVect));
|
||||
|
||||
// Transform the verts before building the hull in case of a negative scale.
|
||||
for(int i=0; i<count; i++) hullVerts[i] = cpTransformPoint(transform, verts[i]);
|
||||
|
||||
unsigned int hullCount = cpConvexHull(count, hullVerts, hullVerts, NULL, 0.0);
|
||||
cpPolyShapeSetVertsRaw(shape, hullCount, hullVerts);
|
||||
}
|
||||
|
||||
void
|
||||
cpPolyShapeSetVertsRaw(cpShape *shape, int count, cpVect *verts)
|
||||
{
|
||||
cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
|
||||
cpPolyShape *poly = (cpPolyShape *)shape;
|
||||
cpPolyShapeDestroy(poly);
|
||||
|
||||
SetVerts(poly, count, verts);
|
||||
|
||||
cpFloat mass = shape->massInfo.m;
|
||||
shape->massInfo = cpPolyShapeMassInfo(shape->massInfo.m, count, verts, poly->r);
|
||||
if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
|
||||
}
|
||||
|
||||
void
|
||||
cpPolyShapeSetRadius(cpShape *shape, cpFloat radius)
|
||||
{
|
||||
cpAssertHard(shape->klass == &polyClass, "Shape is not a poly shape.");
|
||||
cpPolyShape *poly = (cpPolyShape *)shape;
|
||||
poly->r = radius;
|
||||
|
||||
|
||||
// TODO radius is not handled by moment/area
|
||||
// cpFloat mass = shape->massInfo.m;
|
||||
// shape->massInfo = cpPolyShapeMassInfo(shape->massInfo.m, poly->count, poly->verts, poly->r);
|
||||
// if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
|
||||
}
|
||||
|
|
@ -0,0 +1,652 @@
|
|||
// Copyright 2013 Howling Moon Software. All rights reserved.
|
||||
// See http://chipmunk2d.net/legal.php for more information.
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "chipmunk/chipmunk_private.h"
|
||||
#include "chipmunk/cpPolyline.h"
|
||||
|
||||
|
||||
static inline int Next(int i, int count){return (i+1)%count;}
|
||||
|
||||
//MARK: Polylines
|
||||
|
||||
#define DEFAULT_POLYLINE_CAPACITY 16
|
||||
|
||||
static int
|
||||
cpPolylineSizeForCapacity(int capacity)
|
||||
{
|
||||
return sizeof(cpPolyline) + capacity*sizeof(cpVect);
|
||||
}
|
||||
|
||||
static cpPolyline *
|
||||
cpPolylineMake(int capacity)
|
||||
{
|
||||
capacity = (capacity > DEFAULT_POLYLINE_CAPACITY ? capacity : DEFAULT_POLYLINE_CAPACITY);
|
||||
|
||||
cpPolyline *line = (cpPolyline *)cpcalloc(1, cpPolylineSizeForCapacity(capacity));
|
||||
line->count = 0;
|
||||
line->capacity = capacity;
|
||||
|
||||
return line;
|
||||
}
|
||||
|
||||
static cpPolyline *
|
||||
cpPolylineMake2(int capacity, cpVect a, cpVect b)
|
||||
{
|
||||
cpPolyline *line = cpPolylineMake(capacity);
|
||||
line->count = 2;
|
||||
line->verts[0] = a;
|
||||
line->verts[1] = b;
|
||||
|
||||
return line;
|
||||
}
|
||||
|
||||
static cpPolyline *
|
||||
cpPolylineShrink(cpPolyline *line)
|
||||
{
|
||||
line->capacity = line->count;
|
||||
return (cpPolyline*) cprealloc(line, cpPolylineSizeForCapacity(line->count));
|
||||
}
|
||||
|
||||
void
|
||||
cpPolylineFree(cpPolyline *line)
|
||||
{
|
||||
cpfree(line);
|
||||
}
|
||||
|
||||
// Grow the allocated memory for a polyline.
|
||||
static cpPolyline *
|
||||
cpPolylineGrow(cpPolyline *line, int count)
|
||||
{
|
||||
line->count += count;
|
||||
|
||||
int capacity = line->capacity;
|
||||
while(line->count > capacity) capacity *= 2;
|
||||
|
||||
if(line->capacity < capacity){
|
||||
line->capacity = capacity;
|
||||
line = (cpPolyline*) cprealloc(line, cpPolylineSizeForCapacity(capacity));
|
||||
}
|
||||
|
||||
return line;
|
||||
}
|
||||
|
||||
// Push v onto the end of line.
|
||||
static cpPolyline *
|
||||
cpPolylinePush(cpPolyline *line, cpVect v)
|
||||
{
|
||||
int count = line->count;
|
||||
line = cpPolylineGrow(line, 1);
|
||||
line->verts[count] = v;
|
||||
|
||||
return line;
|
||||
}
|
||||
|
||||
// Push v onto the beginning of line.
|
||||
static cpPolyline *
|
||||
cpPolylineEnqueue(cpPolyline *line, cpVect v)
|
||||
{
|
||||
// TODO could optimize this to grow in both directions.
|
||||
// Probably doesn't matter though.
|
||||
int count = line->count;
|
||||
line = cpPolylineGrow(line, 1);
|
||||
memmove(line->verts + 1, line->verts, count*sizeof(cpVect));
|
||||
line->verts[0] = v;
|
||||
|
||||
return line;
|
||||
}
|
||||
|
||||
// Returns true if the polyline starts and ends with the same vertex.
|
||||
cpBool
|
||||
cpPolylineIsClosed(cpPolyline *line)
|
||||
{
|
||||
return (line->count > 1 && cpveql(line->verts[0], line->verts[line->count-1]));
|
||||
}
|
||||
|
||||
// Check if a cpPolyline is longer than a certain length
|
||||
// Takes a range which can wrap around if the polyline is looped.
|
||||
static cpBool
|
||||
cpPolylineIsShort(cpVect *points, int count, int start, int end, cpFloat min)
|
||||
{
|
||||
cpFloat length = 0.0f;
|
||||
for(int i=start; i!=end; i=Next(i, count)){
|
||||
length += cpvdist(points[i], points[Next(i, count)]);
|
||||
if(length > min) return cpFalse;
|
||||
}
|
||||
|
||||
return cpTrue;
|
||||
}
|
||||
|
||||
//MARK: Polyline Simplification
|
||||
|
||||
static inline cpFloat
|
||||
Sharpness(cpVect a, cpVect b, cpVect c)
|
||||
{
|
||||
// TODO could speed this up by caching the normals instead of calculating each twice.
|
||||
return cpvdot(cpvnormalize(cpvsub(a, b)), cpvnormalize(cpvsub(c, b)));
|
||||
}
|
||||
|
||||
// Join similar adjacent line segments together. Works well for hard edged shapes.
|
||||
// 'tol' is the minimum anglular difference in radians of a vertex.
|
||||
cpPolyline *
|
||||
cpPolylineSimplifyVertexes(cpPolyline *line, cpFloat tol)
|
||||
{
|
||||
cpPolyline *reduced = cpPolylineMake2(0, line->verts[0], line->verts[1]);
|
||||
|
||||
cpFloat minSharp = -cpfcos(tol);
|
||||
|
||||
for(int i=2; i<line->count; i++){
|
||||
cpVect vert = line->verts[i];
|
||||
cpFloat sharp = Sharpness(reduced->verts[reduced->count - 2], reduced->verts[reduced->count - 1], vert);
|
||||
|
||||
if(sharp <= minSharp){
|
||||
reduced->verts[reduced->count - 1] = vert;
|
||||
} else {
|
||||
reduced = cpPolylinePush(reduced, vert);
|
||||
}
|
||||
}
|
||||
|
||||
if(
|
||||
cpPolylineIsClosed(line) &&
|
||||
Sharpness(reduced->verts[reduced->count - 2], reduced->verts[0], reduced->verts[1]) < minSharp
|
||||
){
|
||||
reduced->verts[0] = reduced->verts[reduced->count - 2];
|
||||
reduced->count--;
|
||||
}
|
||||
|
||||
// TODO shrink
|
||||
return reduced;
|
||||
}
|
||||
|
||||
// Recursive function used by cpPolylineSimplifyCurves().
|
||||
static cpPolyline *
|
||||
DouglasPeucker(
|
||||
cpVect *verts, cpPolyline *reduced,
|
||||
int length, int start, int end,
|
||||
cpFloat min, cpFloat tol
|
||||
){
|
||||
// Early exit if the points are adjacent
|
||||
if((end - start + length)%length < 2) return reduced;
|
||||
|
||||
cpVect a = verts[start];
|
||||
cpVect b = verts[end];
|
||||
|
||||
// Check if the length is below the threshold
|
||||
if(cpvnear(a, b, min) && cpPolylineIsShort(verts, length, start, end, min)) return reduced;
|
||||
|
||||
// Find the maximal vertex to split and recurse on
|
||||
cpFloat max = 0.0;
|
||||
int maxi = start;
|
||||
|
||||
cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));
|
||||
cpFloat d = cpvdot(n, a);
|
||||
|
||||
for(int i=Next(start, length); i!=end; i=Next(i, length)){
|
||||
cpFloat dist = fabs(cpvdot(n, verts[i]) - d);
|
||||
|
||||
if(dist > max){
|
||||
max = dist;
|
||||
maxi = i;
|
||||
}
|
||||
}
|
||||
|
||||
if(max > tol){
|
||||
reduced = DouglasPeucker(verts, reduced, length, start, maxi, min, tol);
|
||||
reduced = cpPolylinePush(reduced, verts[maxi]);
|
||||
reduced = DouglasPeucker(verts, reduced, length, maxi, end, min, tol);
|
||||
}
|
||||
|
||||
return reduced;
|
||||
}
|
||||
|
||||
// Recursively reduce the vertex count on a polyline. Works best for smooth shapes.
|
||||
// 'tol' is the maximum error for the reduction.
|
||||
// The reduced polyline will never be farther than this distance from the original polyline.
|
||||
cpPolyline *
|
||||
cpPolylineSimplifyCurves(cpPolyline *line, cpFloat tol)
|
||||
{
|
||||
cpPolyline *reduced = cpPolylineMake(line->count);
|
||||
|
||||
cpFloat min = tol/2.0f;
|
||||
|
||||
if(cpPolylineIsClosed(line)){
|
||||
int start, end;
|
||||
cpLoopIndexes(line->verts, line->count - 1, &start, &end);
|
||||
|
||||
reduced = cpPolylinePush(reduced, line->verts[start]);
|
||||
reduced = DouglasPeucker(line->verts, reduced, line->count - 1, start, end, min, tol);
|
||||
reduced = cpPolylinePush(reduced, line->verts[end]);
|
||||
reduced = DouglasPeucker(line->verts, reduced, line->count - 1, end, start, min, tol);
|
||||
reduced = cpPolylinePush(reduced, line->verts[start]);
|
||||
} else {
|
||||
reduced = cpPolylinePush(reduced, line->verts[0]);
|
||||
reduced = DouglasPeucker(line->verts, reduced, line->count, 0, line->count - 1, min, tol);
|
||||
reduced = cpPolylinePush(reduced, line->verts[line->count - 1]);
|
||||
}
|
||||
|
||||
return cpPolylineShrink(reduced);
|
||||
}
|
||||
|
||||
//MARK: Polyline Sets
|
||||
|
||||
cpPolylineSet *
|
||||
cpPolylineSetAlloc(void)
|
||||
{
|
||||
return (cpPolylineSet *)cpcalloc(1, sizeof(cpPolylineSet));
|
||||
}
|
||||
|
||||
cpPolylineSet *
|
||||
cpPolylineSetInit(cpPolylineSet *set)
|
||||
{
|
||||
set->count = 0;
|
||||
set->capacity = 8;
|
||||
set->lines = (cpPolyline**) cpcalloc(set->capacity, sizeof(cpPolyline));
|
||||
|
||||
return set;
|
||||
}
|
||||
|
||||
|
||||
cpPolylineSet *
|
||||
cpPolylineSetNew(void)
|
||||
{
|
||||
return cpPolylineSetInit(cpPolylineSetAlloc());
|
||||
}
|
||||
|
||||
void
|
||||
cpPolylineSetDestroy(cpPolylineSet *set, cpBool freePolylines)
|
||||
{
|
||||
if(freePolylines){
|
||||
for(int i=0; i<set->count; i++){
|
||||
cpPolylineFree(set->lines[i]);
|
||||
}
|
||||
}
|
||||
|
||||
cpfree(set->lines);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
cpPolylineSetFree(cpPolylineSet *set, cpBool freePolylines)
|
||||
{
|
||||
if(set){
|
||||
cpPolylineSetDestroy(set, freePolylines);
|
||||
cpfree(set);
|
||||
}
|
||||
}
|
||||
|
||||
// Find the polyline that ends with v.
|
||||
static int
|
||||
cpPolylineSetFindEnds(cpPolylineSet *set, cpVect v){
|
||||
int count = set->count;
|
||||
cpPolyline **lines = set->lines;
|
||||
|
||||
for(int i=0; i<count; i++){
|
||||
cpPolyline *line = lines[i];
|
||||
if(cpveql(line->verts[line->count - 1], v)) return i;
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Find the polyline that starts with v.
|
||||
static int
|
||||
cpPolylineSetFindStarts(cpPolylineSet *set, cpVect v){
|
||||
int count = set->count;
|
||||
cpPolyline **lines = set->lines;
|
||||
|
||||
for(int i=0; i<count; i++){
|
||||
if(cpveql(lines[i]->verts[0], v)) return i;
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Add a new polyline to a polyline set.
|
||||
static void
|
||||
cpPolylineSetPush(cpPolylineSet *set, cpPolyline *line)
|
||||
{
|
||||
// grow set
|
||||
set->count++;
|
||||
if(set->count > set->capacity){
|
||||
set->capacity *= 2;
|
||||
set->lines = (cpPolyline**) cprealloc(set->lines, set->capacity*sizeof(cpPolyline));
|
||||
}
|
||||
|
||||
set->lines[set->count - 1] = line;
|
||||
}
|
||||
|
||||
// Add a new polyline to a polyline set.
|
||||
static void
|
||||
cpPolylineSetAdd(cpPolylineSet *set, cpVect v0, cpVect v1)
|
||||
{
|
||||
cpPolylineSetPush(set, cpPolylineMake2(DEFAULT_POLYLINE_CAPACITY, v0, v1));
|
||||
}
|
||||
|
||||
// Join two cpPolylines in a polyline set together.
|
||||
static void
|
||||
cpPolylineSetJoin(cpPolylineSet *set, int before, int after)
|
||||
{
|
||||
cpPolyline *lbefore = set->lines[before];
|
||||
cpPolyline *lafter = set->lines[after];
|
||||
|
||||
// append
|
||||
int count = lbefore->count;
|
||||
lbefore = cpPolylineGrow(lbefore, lafter->count);
|
||||
memmove(lbefore->verts + count, lafter->verts, lafter->count*sizeof(cpVect));
|
||||
set->lines[before] = lbefore;
|
||||
|
||||
// delete lafter
|
||||
set->count--;
|
||||
cpPolylineFree(set->lines[after]);
|
||||
set->lines[after] = set->lines[set->count];
|
||||
}
|
||||
|
||||
// Add a segment to a polyline set.
|
||||
// A segment will either start a new polyline, join two others, or add to or loop an existing polyline.
|
||||
void
|
||||
cpPolylineSetCollectSegment(cpVect v0, cpVect v1, cpPolylineSet *lines)
|
||||
{
|
||||
int before = cpPolylineSetFindEnds(lines, v0);
|
||||
int after = cpPolylineSetFindStarts(lines, v1);
|
||||
|
||||
if(before >= 0 && after >= 0){
|
||||
if(before == after){
|
||||
// loop by pushing v1 onto before
|
||||
lines->lines[before] = cpPolylinePush(lines->lines[before], v1);
|
||||
} else {
|
||||
// join before and after
|
||||
cpPolylineSetJoin(lines, before, after);
|
||||
}
|
||||
} else if(before >= 0){
|
||||
// push v1 onto before
|
||||
lines->lines[before] = cpPolylinePush(lines->lines[before], v1);
|
||||
} else if(after >= 0){
|
||||
// enqueue v0 onto after
|
||||
lines->lines[after] = cpPolylineEnqueue(lines->lines[after], v0);
|
||||
} else {
|
||||
// create new line from v0 and v1
|
||||
cpPolylineSetAdd(lines, v0, v1);
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Convex Hull Functions
|
||||
|
||||
cpPolyline *
|
||||
cpPolylineToConvexHull(cpPolyline *line, cpFloat tol)
|
||||
{
|
||||
cpPolyline *hull = cpPolylineMake(line->count + 1);
|
||||
hull->count = cpConvexHull(line->count, line->verts, hull->verts, NULL, tol);
|
||||
hull = cpPolylinePush(hull, hull->verts[0]);
|
||||
|
||||
return cpPolylineShrink(hull);
|
||||
}
|
||||
|
||||
//MARK: Approximate Concave Decompostition
|
||||
|
||||
struct Notch {
|
||||
int i;
|
||||
cpFloat d;
|
||||
cpVect v;
|
||||
cpVect n;
|
||||
};
|
||||
|
||||
static cpFloat
|
||||
FindSteiner(int count, cpVect *verts, struct Notch notch)
|
||||
{
|
||||
cpFloat min = INFINITY;
|
||||
cpFloat feature = -1.0;
|
||||
|
||||
for(int i=1; i<count-1; i++){
|
||||
int index = (notch.i + i)%count;
|
||||
|
||||
cpVect seg_a = verts[index];
|
||||
cpVect seg_b = verts[Next(index, count)];
|
||||
|
||||
cpFloat thing_a = cpvcross(notch.n, cpvsub(seg_a, notch.v));
|
||||
cpFloat thing_b = cpvcross(notch.n, cpvsub(seg_b, notch.v));
|
||||
if(thing_a*thing_b <= 0.0){
|
||||
cpFloat t = thing_a/(thing_a - thing_b);
|
||||
cpFloat dist = cpvdot(notch.n, cpvsub(cpvlerp(seg_a, seg_b, t), notch.v));
|
||||
|
||||
if(dist >= 0.0 && dist <= min){
|
||||
min = dist;
|
||||
feature = index + t;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return feature;
|
||||
}
|
||||
|
||||
//static cpFloat
|
||||
//FindSteiner2(cpVect *verts, int count, struct Notch notch)
|
||||
//{
|
||||
// cpVect a = verts[(notch.i + count - 1)%count];
|
||||
// cpVect b = verts[(notch.i + 1)%count];
|
||||
// cpVect n = cpvnormalize(cpvadd(cpvnormalize(cpvsub(notch.v, a)), cpvnormalize(cpvsub(notch.v, b))));
|
||||
//
|
||||
// cpFloat min = INFINITY;
|
||||
// cpFloat feature = -1.0;
|
||||
//
|
||||
// for(int i=1; i<count-1; i++){
|
||||
// int index = (notch.i + i)%count;
|
||||
//
|
||||
// cpVect seg_a = verts[index];
|
||||
// cpVect seg_b = verts[Next(index, count)];
|
||||
//
|
||||
// cpFloat thing_a = cpvcross(n, cpvsub(seg_a, notch.v));
|
||||
// cpFloat thing_b = cpvcross(n, cpvsub(seg_b, notch.v));
|
||||
// if(thing_a*thing_b <= 0.0){
|
||||
// cpFloat t = thing_a/(thing_a - thing_b);
|
||||
// cpFloat dist = cpvdot(n, cpvsub(cpvlerp(seg_a, seg_b, t), notch.v));
|
||||
//
|
||||
// if(dist >= 0.0 && dist <= min){
|
||||
// min = dist;
|
||||
// feature = index + t;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// cpAssertSoft(feature >= 0.0, "No closest features detected. This is likely due to a self intersecting polygon.");
|
||||
// return feature;
|
||||
//}
|
||||
|
||||
//struct Range {cpFloat min, max;};
|
||||
//static inline struct Range
|
||||
//clip_range(cpVect delta_a, cpVect delta_b, cpVect clip)
|
||||
//{
|
||||
// cpFloat da = cpvcross(delta_a, clip);
|
||||
// cpFloat db = cpvcross(delta_b, clip);
|
||||
// cpFloat clamp = da/(da - db);
|
||||
// if(da > db){
|
||||
// return (struct Range){-INFINITY, clamp};
|
||||
// } else if(da < db){
|
||||
// return (struct Range){clamp, INFINITY};
|
||||
// } else {
|
||||
// return (struct Range){-INFINITY, INFINITY};
|
||||
// }
|
||||
//}
|
||||
//
|
||||
//static cpFloat
|
||||
//FindSteiner3(cpVect *verts, int count, struct Notch notch)
|
||||
//{
|
||||
// cpFloat min = INFINITY;
|
||||
// cpFloat feature = -1.0;
|
||||
//
|
||||
// cpVect support_a = verts[(notch.i - 1 + count)%count];
|
||||
// cpVect support_b = verts[(notch.i + 1)%count];
|
||||
//
|
||||
// cpVect clip_a = cpvlerp(support_a, support_b, 0.1);
|
||||
// cpVect clip_b = cpvlerp(support_b, support_b, 0.9);
|
||||
//
|
||||
// for(int i=1; i<count - 1; i++){
|
||||
// int index = (notch.i + i)%count;
|
||||
// cpVect seg_a = verts[index];
|
||||
// cpVect seg_b = verts[Next(index, count)];
|
||||
//
|
||||
// cpVect delta_a = cpvsub(seg_a, notch.v);
|
||||
// cpVect delta_b = cpvsub(seg_b, notch.v);
|
||||
//
|
||||
// // Ignore if the segment faces away from the point.
|
||||
// if(cpvcross(delta_b, delta_a) > 0.0){
|
||||
// struct Range range1 = clip_range(delta_a, delta_b, cpvsub(notch.v, clip_a));
|
||||
// struct Range range2 = clip_range(delta_a, delta_b, cpvsub(clip_b, notch.v));
|
||||
//
|
||||
// cpFloat min_t = cpfmax(0.0, cpfmax(range1.min, range2.min));
|
||||
// cpFloat max_t = cpfmin(1.0, cpfmin(range1.max, range2.max));
|
||||
//
|
||||
// // Ignore if the segment has been completely clipped away.
|
||||
// if(min_t < max_t){
|
||||
// cpVect seg_delta = cpvsub(seg_b, seg_a);
|
||||
// cpFloat closest_t = cpfclamp(cpvdot(seg_delta, cpvsub(notch.v, seg_a))/cpvlengthsq(seg_delta), min_t, max_t);
|
||||
// cpVect closest = cpvlerp(seg_a, seg_b, closest_t);
|
||||
//
|
||||
// cpFloat dist = cpvdistsq(notch.v, closest);
|
||||
// if(dist < min){
|
||||
// min = dist;
|
||||
// feature = index + closest_t;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// cpAssertWarn(feature >= 0.0, "Internal Error: No closest features detected.");
|
||||
// return feature;
|
||||
//}
|
||||
|
||||
//static cpBool
|
||||
//VertexUnobscured(int count, cpVect *verts, int index, int notch_i)
|
||||
//{
|
||||
// cpVect v = verts[notch_i];
|
||||
// cpVect n = cpvnormalize(cpvsub(verts[index], v));
|
||||
//
|
||||
// for(int i=0; i<count; i++){
|
||||
// if(i == index || i == Next(i, count) || i == notch_i || i == Next(notch_i, count)) continue;
|
||||
//
|
||||
// cpVect seg_a = verts[i];
|
||||
// cpVect seg_b = verts[Next(i, count)];
|
||||
//
|
||||
// cpFloat thing_a = cpvcross(n, cpvsub(seg_a, v));
|
||||
// cpFloat thing_b = cpvcross(n, cpvsub(seg_b, v));
|
||||
// if(thing_a*thing_b <= 0.0) return cpTrue;
|
||||
// }
|
||||
//
|
||||
// return cpFalse;
|
||||
//}
|
||||
//
|
||||
//static cpFloat
|
||||
//FindSteiner4(int count, cpVect *verts, struct Notch notch, cpFloat *convexity)
|
||||
//{
|
||||
// cpFloat min = INFINITY;
|
||||
// cpFloat feature = -1.0;
|
||||
//
|
||||
// for(int i=Next(notch.b, count); i!=notch.a; i=Next(i, count)){
|
||||
// cpVect v = verts[i];
|
||||
// cpFloat weight = (1.0 + 0.1*convexity[i])/(1.0*cpvdist(notch.v, v));
|
||||
//
|
||||
// if(weight <= min && VertexUnobscured(count, verts, i, notch.i)){
|
||||
// min = weight;
|
||||
// feature = i;
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// cpAssertSoft(feature >= 0.0, "No closest features detected. This is likely due to a self intersecting polygon.");
|
||||
// return feature;
|
||||
//}
|
||||
|
||||
static struct Notch
|
||||
DeepestNotch(int count, cpVect *verts, int hullCount, cpVect *hullVerts, int first, cpFloat tol)
|
||||
{
|
||||
struct Notch notch = {0};
|
||||
int j = Next(first, count);
|
||||
|
||||
for(int i=0; i<hullCount; i++){
|
||||
cpVect a = hullVerts[i];
|
||||
cpVect b = hullVerts[Next(i, hullCount)];
|
||||
|
||||
// TODO use a cross check instead?
|
||||
cpVect n = cpvnormalize(cpvrperp(cpvsub(a, b)));
|
||||
cpFloat d = cpvdot(n, a);
|
||||
|
||||
cpVect v = verts[j];
|
||||
while(!cpveql(v, b)){
|
||||
cpFloat depth = cpvdot(n, v) - d;
|
||||
|
||||
if(depth > notch.d){
|
||||
notch.d = depth;
|
||||
notch.i = j;
|
||||
notch.v = v;
|
||||
notch.n = n;
|
||||
}
|
||||
|
||||
j = Next(j, count);
|
||||
v = verts[j];
|
||||
}
|
||||
|
||||
j = Next(j, count);
|
||||
}
|
||||
|
||||
return notch;
|
||||
}
|
||||
|
||||
static inline int IMAX(int a, int b){return (a > b ? a : b);}
|
||||
|
||||
static void
|
||||
ApproximateConcaveDecomposition(cpVect *verts, int count, cpFloat tol, cpPolylineSet *set)
|
||||
{
|
||||
int first;
|
||||
cpVect *hullVerts = (cpVect*) alloca(count*sizeof(cpVect));
|
||||
int hullCount = cpConvexHull(count, verts, hullVerts, &first, 0.0);
|
||||
|
||||
if(hullCount != count){
|
||||
struct Notch notch = DeepestNotch(count, verts, hullCount, hullVerts, first, tol);
|
||||
|
||||
if(notch.d > tol){
|
||||
cpFloat steiner_it = FindSteiner(count, verts, notch);
|
||||
|
||||
if(steiner_it >= 0.0){
|
||||
int steiner_i = (int)steiner_it;
|
||||
cpVect steiner = cpvlerp(verts[steiner_i], verts[Next(steiner_i, count)], steiner_it - steiner_i);
|
||||
|
||||
// Vertex counts NOT including the steiner point.
|
||||
int sub1_count = (steiner_i - notch.i + count)%count + 1;
|
||||
int sub2_count = count - (steiner_i - notch.i + count)%count;
|
||||
cpVect *scratch = (cpVect*) alloca((IMAX(sub1_count, sub2_count) + 1)*sizeof(cpVect));
|
||||
|
||||
for(int i=0; i<sub1_count; i++) scratch[i] = verts[(notch.i + i)%count];
|
||||
scratch[sub1_count] = steiner;
|
||||
ApproximateConcaveDecomposition(scratch, sub1_count + 1, tol, set);
|
||||
|
||||
for(int i=0; i<sub2_count; i++) scratch[i] = verts[(steiner_i + 1 + i)%count];
|
||||
scratch[sub2_count] = steiner;
|
||||
ApproximateConcaveDecomposition(scratch, sub2_count + 1, tol, set);
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
cpPolyline *hull = cpPolylineMake(hullCount + 1);
|
||||
|
||||
memcpy(hull->verts, hullVerts, hullCount*sizeof(cpVect));
|
||||
hull->verts[hullCount] = hullVerts[0];
|
||||
hull->count = hullCount + 1;
|
||||
|
||||
cpPolylineSetPush(set, hull);
|
||||
}
|
||||
|
||||
cpPolylineSet *
|
||||
cpPolylineConvexDecomposition_BETA(cpPolyline *line, cpFloat tol)
|
||||
{
|
||||
cpAssertSoft(cpPolylineIsClosed(line), "Cannot decompose an open polygon.");
|
||||
cpAssertSoft(cpAreaForPoly(line->count, line->verts, 0.0) >= 0.0, "Winding is backwards. (Are you passing a hole?)");
|
||||
|
||||
cpPolylineSet *set = cpPolylineSetNew();
|
||||
ApproximateConcaveDecomposition(line->verts, line->count - 1, tol, set);
|
||||
|
||||
return set;
|
||||
}
|
||||
|
|
@ -0,0 +1,179 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
static void
|
||||
preStep(cpRatchetJoint *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
cpFloat angle = joint->angle;
|
||||
cpFloat phase = joint->phase;
|
||||
cpFloat ratchet = joint->ratchet;
|
||||
|
||||
cpFloat delta = b->a - a->a;
|
||||
cpFloat diff = angle - delta;
|
||||
cpFloat pdist = 0.0f;
|
||||
|
||||
if(diff*ratchet > 0.0f){
|
||||
pdist = diff;
|
||||
} else {
|
||||
joint->angle = cpffloor((delta - phase)/ratchet)*ratchet + phase;
|
||||
}
|
||||
|
||||
// calculate moment of inertia coefficient.
|
||||
joint->iSum = 1.0f/(a->i_inv + b->i_inv);
|
||||
|
||||
// calculate bias velocity
|
||||
cpFloat maxBias = joint->constraint.maxBias;
|
||||
joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias);
|
||||
|
||||
// If the bias is 0, the joint is not at a limit. Reset the impulse.
|
||||
if(!joint->bias) joint->jAcc = 0.0f;
|
||||
}
|
||||
|
||||
static void
|
||||
applyCachedImpulse(cpRatchetJoint *joint, cpFloat dt_coef)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
cpFloat j = joint->jAcc*dt_coef;
|
||||
a->w -= j*a->i_inv;
|
||||
b->w += j*b->i_inv;
|
||||
}
|
||||
|
||||
static void
|
||||
applyImpulse(cpRatchetJoint *joint, cpFloat dt)
|
||||
{
|
||||
if(!joint->bias) return; // early exit
|
||||
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
// compute relative rotational velocity
|
||||
cpFloat wr = b->w - a->w;
|
||||
cpFloat ratchet = joint->ratchet;
|
||||
|
||||
cpFloat jMax = joint->constraint.maxForce*dt;
|
||||
|
||||
// compute normal impulse
|
||||
cpFloat j = -(joint->bias + wr)*joint->iSum;
|
||||
cpFloat jOld = joint->jAcc;
|
||||
joint->jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, jMax*cpfabs(ratchet))/ratchet;
|
||||
j = joint->jAcc - jOld;
|
||||
|
||||
// apply impulse
|
||||
a->w -= j*a->i_inv;
|
||||
b->w += j*b->i_inv;
|
||||
}
|
||||
|
||||
static cpFloat
|
||||
getImpulse(cpRatchetJoint *joint)
|
||||
{
|
||||
return cpfabs(joint->jAcc);
|
||||
}
|
||||
|
||||
static const cpConstraintClass klass = {
|
||||
(cpConstraintPreStepImpl)preStep,
|
||||
(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
|
||||
(cpConstraintApplyImpulseImpl)applyImpulse,
|
||||
(cpConstraintGetImpulseImpl)getImpulse,
|
||||
};
|
||||
|
||||
cpRatchetJoint *
|
||||
cpRatchetJointAlloc(void)
|
||||
{
|
||||
return (cpRatchetJoint *)cpcalloc(1, sizeof(cpRatchetJoint));
|
||||
}
|
||||
|
||||
cpRatchetJoint *
|
||||
cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet)
|
||||
{
|
||||
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
||||
|
||||
joint->angle = 0.0f;
|
||||
joint->phase = phase;
|
||||
joint->ratchet = ratchet;
|
||||
|
||||
// STATIC_BODY_CHECK
|
||||
joint->angle = (b ? b->a : 0.0f) - (a ? a->a : 0.0f);
|
||||
|
||||
return joint;
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet)
|
||||
{
|
||||
return (cpConstraint *)cpRatchetJointInit(cpRatchetJointAlloc(), a, b, phase, ratchet);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpConstraintIsRatchetJoint(const cpConstraint *constraint)
|
||||
{
|
||||
return (constraint->klass == &klass);
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpRatchetJointGetAngle(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint.");
|
||||
return ((cpRatchetJoint *)constraint)->angle;
|
||||
}
|
||||
|
||||
void
|
||||
cpRatchetJointSetAngle(cpConstraint *constraint, cpFloat angle)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpRatchetJoint *)constraint)->angle = angle;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpRatchetJointGetPhase(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint.");
|
||||
return ((cpRatchetJoint *)constraint)->phase;
|
||||
}
|
||||
|
||||
void
|
||||
cpRatchetJointSetPhase(cpConstraint *constraint, cpFloat phase)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpRatchetJoint *)constraint)->phase = phase;
|
||||
}
|
||||
cpFloat
|
||||
cpRatchetJointGetRatchet(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint.");
|
||||
return ((cpRatchetJoint *)constraint)->ratchet;
|
||||
}
|
||||
|
||||
void
|
||||
cpRatchetJointSetRatchet(cpConstraint *constraint, cpFloat ratchet)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsRatchetJoint(constraint), "Constraint is not a ratchet joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpRatchetJoint *)constraint)->ratchet = ratchet;
|
||||
}
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
#include "chipmunk/cpRobust.h"
|
||||
|
||||
|
||||
cpBool
|
||||
cpCheckPointGreater(const cpVect a, const cpVect b, const cpVect c)
|
||||
{
|
||||
return (b.y - a.y)*(a.x + b.x - 2*c.x) > (b.x - a.x)*(a.y + b.y - 2*c.y);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpCheckAxis(cpVect v0, cpVect v1, cpVect p, cpVect n){
|
||||
return cpvdot(p, n) <= cpfmax(cpvdot(v0, n), cpvdot(v1, n));
|
||||
}
|
||||
|
|
@ -0,0 +1,160 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
static void
|
||||
preStep(cpRotaryLimitJoint *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
cpFloat dist = b->a - a->a;
|
||||
cpFloat pdist = 0.0f;
|
||||
if(dist > joint->max) {
|
||||
pdist = joint->max - dist;
|
||||
} else if(dist < joint->min) {
|
||||
pdist = joint->min - dist;
|
||||
}
|
||||
|
||||
// calculate moment of inertia coefficient.
|
||||
joint->iSum = 1.0f/(a->i_inv + b->i_inv);
|
||||
|
||||
// calculate bias velocity
|
||||
cpFloat maxBias = joint->constraint.maxBias;
|
||||
joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias);
|
||||
|
||||
// If the bias is 0, the joint is not at a limit. Reset the impulse.
|
||||
if(!joint->bias) joint->jAcc = 0.0f;
|
||||
}
|
||||
|
||||
static void
|
||||
applyCachedImpulse(cpRotaryLimitJoint *joint, cpFloat dt_coef)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
cpFloat j = joint->jAcc*dt_coef;
|
||||
a->w -= j*a->i_inv;
|
||||
b->w += j*b->i_inv;
|
||||
}
|
||||
|
||||
static void
|
||||
applyImpulse(cpRotaryLimitJoint *joint, cpFloat dt)
|
||||
{
|
||||
if(!joint->bias) return; // early exit
|
||||
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
// compute relative rotational velocity
|
||||
cpFloat wr = b->w - a->w;
|
||||
|
||||
cpFloat jMax = joint->constraint.maxForce*dt;
|
||||
|
||||
// compute normal impulse
|
||||
cpFloat j = -(joint->bias + wr)*joint->iSum;
|
||||
cpFloat jOld = joint->jAcc;
|
||||
if(joint->bias < 0.0f){
|
||||
joint->jAcc = cpfclamp(jOld + j, 0.0f, jMax);
|
||||
} else {
|
||||
joint->jAcc = cpfclamp(jOld + j, -jMax, 0.0f);
|
||||
}
|
||||
j = joint->jAcc - jOld;
|
||||
|
||||
// apply impulse
|
||||
a->w -= j*a->i_inv;
|
||||
b->w += j*b->i_inv;
|
||||
}
|
||||
|
||||
static cpFloat
|
||||
getImpulse(cpRotaryLimitJoint *joint)
|
||||
{
|
||||
return cpfabs(joint->jAcc);
|
||||
}
|
||||
|
||||
static const cpConstraintClass klass = {
|
||||
(cpConstraintPreStepImpl)preStep,
|
||||
(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
|
||||
(cpConstraintApplyImpulseImpl)applyImpulse,
|
||||
(cpConstraintGetImpulseImpl)getImpulse,
|
||||
};
|
||||
|
||||
cpRotaryLimitJoint *
|
||||
cpRotaryLimitJointAlloc(void)
|
||||
{
|
||||
return (cpRotaryLimitJoint *)cpcalloc(1, sizeof(cpRotaryLimitJoint));
|
||||
}
|
||||
|
||||
cpRotaryLimitJoint *
|
||||
cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max)
|
||||
{
|
||||
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
||||
|
||||
joint->min = min;
|
||||
joint->max = max;
|
||||
|
||||
joint->jAcc = 0.0f;
|
||||
|
||||
return joint;
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max)
|
||||
{
|
||||
return (cpConstraint *)cpRotaryLimitJointInit(cpRotaryLimitJointAlloc(), a, b, min, max);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpConstraintIsRotaryLimitJoint(const cpConstraint *constraint)
|
||||
{
|
||||
return (constraint->klass == &klass);
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpRotaryLimitJointGetMin(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint.");
|
||||
return ((cpRotaryLimitJoint *)constraint)->min;
|
||||
}
|
||||
|
||||
void
|
||||
cpRotaryLimitJointSetMin(cpConstraint *constraint, cpFloat min)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpRotaryLimitJoint *)constraint)->min = min;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpRotaryLimitJointGetMax(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint.");
|
||||
return ((cpRotaryLimitJoint *)constraint)->max;
|
||||
}
|
||||
|
||||
void
|
||||
cpRotaryLimitJointSetMax(cpConstraint *constraint, cpFloat max)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsRotaryLimitJoint(constraint), "Constraint is not a rotary limit joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpRotaryLimitJoint *)constraint)->max = max;
|
||||
}
|
||||
|
|
@ -0,0 +1,603 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
#include "chipmunk/chipmunk_unsafe.h"
|
||||
|
||||
#define CP_DefineShapeGetter(struct, type, member, name) \
|
||||
CP_DeclareShapeGetter(struct, type, name){ \
|
||||
cpAssertHard(shape->klass == &struct##Class, "shape is not a "#struct); \
|
||||
return ((struct *)shape)->member; \
|
||||
}
|
||||
|
||||
cpShape *
|
||||
cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body, struct cpShapeMassInfo massInfo)
|
||||
{
|
||||
shape->klass = klass;
|
||||
|
||||
shape->body = body;
|
||||
shape->massInfo = massInfo;
|
||||
|
||||
shape->sensor = 0;
|
||||
|
||||
shape->e = 0.0f;
|
||||
shape->u = 0.0f;
|
||||
shape->surfaceV = cpvzero;
|
||||
|
||||
shape->type = 0;
|
||||
shape->filter.group = CP_NO_GROUP;
|
||||
shape->filter.categories = CP_ALL_CATEGORIES;
|
||||
shape->filter.mask = CP_ALL_CATEGORIES;
|
||||
|
||||
shape->userData = NULL;
|
||||
|
||||
shape->space = NULL;
|
||||
|
||||
shape->next = NULL;
|
||||
shape->prev = NULL;
|
||||
|
||||
return shape;
|
||||
}
|
||||
|
||||
void
|
||||
cpShapeDestroy(cpShape *shape)
|
||||
{
|
||||
if(shape->klass && shape->klass->destroy) shape->klass->destroy(shape);
|
||||
}
|
||||
|
||||
void
|
||||
cpShapeFree(cpShape *shape)
|
||||
{
|
||||
if(shape){
|
||||
cpShapeDestroy(shape);
|
||||
cpfree(shape);
|
||||
}
|
||||
}
|
||||
|
||||
cpSpace *
|
||||
cpShapeGetSpace(const cpShape *shape)
|
||||
{
|
||||
return shape->space;
|
||||
}
|
||||
|
||||
cpBody *
|
||||
cpShapeGetBody(const cpShape *shape)
|
||||
{
|
||||
return shape->body;
|
||||
}
|
||||
|
||||
void
|
||||
cpShapeSetBody(cpShape *shape, cpBody *body)
|
||||
{
|
||||
cpAssertHard(!cpShapeActive(shape), "You cannot change the body on an active shape. You must remove the shape from the space before changing the body.");
|
||||
shape->body = body;
|
||||
}
|
||||
|
||||
cpFloat cpShapeGetMass(cpShape *shape){ return shape->massInfo.m; }
|
||||
|
||||
void
|
||||
cpShapeSetMass(cpShape *shape, cpFloat mass){
|
||||
cpBody *body = shape->body;
|
||||
cpBodyActivate(body);
|
||||
|
||||
shape->massInfo.m = mass;
|
||||
cpBodyAccumulateMassFromShapes(body);
|
||||
}
|
||||
|
||||
cpFloat cpShapeGetDensity(cpShape *shape){ return shape->massInfo.m/shape->massInfo.area; }
|
||||
void cpShapeSetDensity(cpShape *shape, cpFloat density){ cpShapeSetMass(shape, density*shape->massInfo.area); }
|
||||
|
||||
cpFloat cpShapeGetMoment(cpShape *shape){ return shape->massInfo.m*shape->massInfo.i; }
|
||||
cpFloat cpShapeGetArea(cpShape *shape){ return shape->massInfo.area; }
|
||||
cpVect cpShapeGetCenterOfGravity(cpShape *shape) { return shape->massInfo.cog; }
|
||||
|
||||
cpBB
|
||||
cpShapeGetBB(const cpShape *shape)
|
||||
{
|
||||
return shape->bb;
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpShapeGetSensor(const cpShape *shape)
|
||||
{
|
||||
return shape->sensor;
|
||||
}
|
||||
|
||||
void
|
||||
cpShapeSetSensor(cpShape *shape, cpBool sensor)
|
||||
{
|
||||
cpBodyActivate(shape->body);
|
||||
shape->sensor = sensor;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpShapeGetElasticity(const cpShape *shape)
|
||||
{
|
||||
return shape->e;
|
||||
}
|
||||
|
||||
void
|
||||
cpShapeSetElasticity(cpShape *shape, cpFloat elasticity)
|
||||
{
|
||||
cpAssertHard(elasticity >= 0.0f, "Elasticity must be positive.");
|
||||
cpBodyActivate(shape->body);
|
||||
shape->e = elasticity;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpShapeGetFriction(const cpShape *shape)
|
||||
{
|
||||
return shape->u;
|
||||
}
|
||||
|
||||
void
|
||||
cpShapeSetFriction(cpShape *shape, cpFloat friction)
|
||||
{
|
||||
cpAssertHard(friction >= 0.0f, "Friction must be postive.");
|
||||
cpBodyActivate(shape->body);
|
||||
shape->u = friction;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpShapeGetSurfaceVelocity(const cpShape *shape)
|
||||
{
|
||||
return shape->surfaceV;
|
||||
}
|
||||
|
||||
void
|
||||
cpShapeSetSurfaceVelocity(cpShape *shape, cpVect surfaceVelocity)
|
||||
{
|
||||
cpBodyActivate(shape->body);
|
||||
shape->surfaceV = surfaceVelocity;
|
||||
}
|
||||
|
||||
cpDataPointer
|
||||
cpShapeGetUserData(const cpShape *shape)
|
||||
{
|
||||
return shape->userData;
|
||||
}
|
||||
|
||||
void
|
||||
cpShapeSetUserData(cpShape *shape, cpDataPointer userData)
|
||||
{
|
||||
shape->userData = userData;
|
||||
}
|
||||
|
||||
cpCollisionType
|
||||
cpShapeGetCollisionType(const cpShape *shape)
|
||||
{
|
||||
return shape->type;
|
||||
}
|
||||
|
||||
void
|
||||
cpShapeSetCollisionType(cpShape *shape, cpCollisionType collisionType)
|
||||
{
|
||||
cpBodyActivate(shape->body);
|
||||
shape->type = collisionType;
|
||||
}
|
||||
|
||||
cpShapeFilter
|
||||
cpShapeGetFilter(const cpShape *shape)
|
||||
{
|
||||
return shape->filter;
|
||||
}
|
||||
|
||||
void
|
||||
cpShapeSetFilter(cpShape *shape, cpShapeFilter filter)
|
||||
{
|
||||
cpBodyActivate(shape->body);
|
||||
shape->filter = filter;
|
||||
}
|
||||
|
||||
cpBB
|
||||
cpShapeCacheBB(cpShape *shape)
|
||||
{
|
||||
return cpShapeUpdate(shape, shape->body->transform);
|
||||
}
|
||||
|
||||
cpBB
|
||||
cpShapeUpdate(cpShape *shape, cpTransform transform)
|
||||
{
|
||||
return (shape->bb = shape->klass->cacheData(shape, transform));
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpShapePointQuery(const cpShape *shape, cpVect p, cpPointQueryInfo *info)
|
||||
{
|
||||
cpPointQueryInfo blank = {NULL, cpvzero, INFINITY, cpvzero};
|
||||
if(info){
|
||||
(*info) = blank;
|
||||
} else {
|
||||
info = ␣
|
||||
}
|
||||
|
||||
shape->klass->pointQuery(shape, p, info);
|
||||
return info->distance;
|
||||
}
|
||||
|
||||
|
||||
cpBool
|
||||
cpShapeSegmentQuery(const cpShape *shape, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info){
|
||||
cpSegmentQueryInfo blank = {NULL, b, cpvzero, 1.0f};
|
||||
if(info){
|
||||
(*info) = blank;
|
||||
} else {
|
||||
info = ␣
|
||||
}
|
||||
|
||||
cpPointQueryInfo nearest;
|
||||
shape->klass->pointQuery(shape, a, &nearest);
|
||||
if(nearest.distance <= radius){
|
||||
info->shape = shape;
|
||||
info->alpha = 0.0;
|
||||
info->normal = cpvnormalize(cpvsub(a, nearest.point));
|
||||
} else {
|
||||
shape->klass->segmentQuery(shape, a, b, radius, info);
|
||||
}
|
||||
|
||||
return (info->shape != NULL);
|
||||
}
|
||||
|
||||
cpContactPointSet
|
||||
cpShapesCollide(const cpShape *a, const cpShape *b)
|
||||
{
|
||||
struct cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER];
|
||||
struct cpCollisionInfo info = cpCollide(a, b, 0, contacts);
|
||||
|
||||
cpContactPointSet set;
|
||||
set.count = info.count;
|
||||
|
||||
// cpCollideShapes() may have swapped the contact order. Flip the normal.
|
||||
cpBool swapped = (a != info.a);
|
||||
set.normal = (swapped ? cpvneg(info.n) : info.n);
|
||||
|
||||
for(int i=0; i<info.count; i++){
|
||||
// cpCollideShapesInfo() returns contacts with absolute positions.
|
||||
cpVect p1 = contacts[i].r1;
|
||||
cpVect p2 = contacts[i].r2;
|
||||
|
||||
set.points[i].pointA = (swapped ? p2 : p1);
|
||||
set.points[i].pointB = (swapped ? p1 : p2);
|
||||
set.points[i].distance = cpvdot(cpvsub(p2, p1), set.normal);
|
||||
}
|
||||
|
||||
return set;
|
||||
}
|
||||
|
||||
cpCircleShape *
|
||||
cpCircleShapeAlloc(void)
|
||||
{
|
||||
return (cpCircleShape *)cpcalloc(1, sizeof(cpCircleShape));
|
||||
}
|
||||
|
||||
static cpBB
|
||||
cpCircleShapeCacheData(cpCircleShape *circle, cpTransform transform)
|
||||
{
|
||||
cpVect c = circle->tc = cpTransformPoint(transform, circle->c);
|
||||
return cpBBNewForCircle(c, circle->r);
|
||||
}
|
||||
|
||||
static void
|
||||
cpCircleShapePointQuery(cpCircleShape *circle, cpVect p, cpPointQueryInfo *info)
|
||||
{
|
||||
cpVect delta = cpvsub(p, circle->tc);
|
||||
cpFloat d = cpvlength(delta);
|
||||
cpFloat r = circle->r;
|
||||
|
||||
info->shape = (cpShape *)circle;
|
||||
info->point = cpvadd(circle->tc, cpvmult(delta, r/d)); // TODO: div/0
|
||||
info->distance = d - r;
|
||||
|
||||
// Use up for the gradient if the distance is very small.
|
||||
info->gradient = (d > MAGIC_EPSILON ? cpvmult(delta, 1.0f/d) : cpv(0.0f, 1.0f));
|
||||
}
|
||||
|
||||
static void
|
||||
cpCircleShapeSegmentQuery(cpCircleShape *circle, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info)
|
||||
{
|
||||
CircleSegmentQuery((cpShape *)circle, circle->tc, circle->r, a, b, radius, info);
|
||||
}
|
||||
|
||||
static struct cpShapeMassInfo
|
||||
cpCircleShapeMassInfo(cpFloat mass, cpFloat radius, cpVect center)
|
||||
{
|
||||
struct cpShapeMassInfo info = {
|
||||
mass, cpMomentForCircle(1.0f, 0.0f, radius, cpvzero),
|
||||
center,
|
||||
cpAreaForCircle(0.0f, radius),
|
||||
};
|
||||
|
||||
return info;
|
||||
}
|
||||
|
||||
static const cpShapeClass cpCircleShapeClass = {
|
||||
CP_CIRCLE_SHAPE,
|
||||
(cpShapeCacheDataImpl)cpCircleShapeCacheData,
|
||||
NULL,
|
||||
(cpShapePointQueryImpl)cpCircleShapePointQuery,
|
||||
(cpShapeSegmentQueryImpl)cpCircleShapeSegmentQuery,
|
||||
};
|
||||
|
||||
cpCircleShape *
|
||||
cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset)
|
||||
{
|
||||
circle->c = offset;
|
||||
circle->r = radius;
|
||||
|
||||
cpShapeInit((cpShape *)circle, &cpCircleShapeClass, body, cpCircleShapeMassInfo(0.0f, radius, offset));
|
||||
|
||||
return circle;
|
||||
}
|
||||
|
||||
cpShape *
|
||||
cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset)
|
||||
{
|
||||
return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpCircleShapeGetOffset(const cpShape *shape)
|
||||
{
|
||||
cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
|
||||
return ((cpCircleShape *)shape)->c;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpCircleShapeGetRadius(const cpShape *shape)
|
||||
{
|
||||
cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
|
||||
return ((cpCircleShape *)shape)->r;
|
||||
}
|
||||
|
||||
|
||||
cpSegmentShape *
|
||||
cpSegmentShapeAlloc(void)
|
||||
{
|
||||
return (cpSegmentShape *)cpcalloc(1, sizeof(cpSegmentShape));
|
||||
}
|
||||
|
||||
static cpBB
|
||||
cpSegmentShapeCacheData(cpSegmentShape *seg, cpTransform transform)
|
||||
{
|
||||
seg->ta = cpTransformPoint(transform, seg->a);
|
||||
seg->tb = cpTransformPoint(transform, seg->b);
|
||||
seg->tn = cpTransformVect(transform, seg->n);
|
||||
|
||||
cpFloat l,r,b,t;
|
||||
|
||||
if(seg->ta.x < seg->tb.x){
|
||||
l = seg->ta.x;
|
||||
r = seg->tb.x;
|
||||
} else {
|
||||
l = seg->tb.x;
|
||||
r = seg->ta.x;
|
||||
}
|
||||
|
||||
if(seg->ta.y < seg->tb.y){
|
||||
b = seg->ta.y;
|
||||
t = seg->tb.y;
|
||||
} else {
|
||||
b = seg->tb.y;
|
||||
t = seg->ta.y;
|
||||
}
|
||||
|
||||
cpFloat rad = seg->r;
|
||||
return cpBBNew(l - rad, b - rad, r + rad, t + rad);
|
||||
}
|
||||
|
||||
static void
|
||||
cpSegmentShapePointQuery(cpSegmentShape *seg, cpVect p, cpPointQueryInfo *info)
|
||||
{
|
||||
cpVect closest = cpClosetPointOnSegment(p, seg->ta, seg->tb);
|
||||
|
||||
cpVect delta = cpvsub(p, closest);
|
||||
cpFloat d = cpvlength(delta);
|
||||
cpFloat r = seg->r;
|
||||
cpVect g = cpvmult(delta, 1.0f/d);
|
||||
|
||||
info->shape = (cpShape *)seg;
|
||||
info->point = (d ? cpvadd(closest, cpvmult(g, r)) : closest);
|
||||
info->distance = d - r;
|
||||
|
||||
// Use the segment's normal if the distance is very small.
|
||||
info->gradient = (d > MAGIC_EPSILON ? g : seg->n);
|
||||
}
|
||||
|
||||
static void
|
||||
cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
|
||||
{
|
||||
cpVect n = seg->tn;
|
||||
cpFloat d = cpvdot(cpvsub(seg->ta, a), n);
|
||||
cpFloat r = seg->r + r2;
|
||||
|
||||
cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n);
|
||||
cpVect seg_offset = cpvsub(cpvmult(flipped_n, r), a);
|
||||
|
||||
// Make the endpoints relative to 'a' and move them by the thickness of the segment.
|
||||
cpVect seg_a = cpvadd(seg->ta, seg_offset);
|
||||
cpVect seg_b = cpvadd(seg->tb, seg_offset);
|
||||
cpVect delta = cpvsub(b, a);
|
||||
|
||||
if(cpvcross(delta, seg_a)*cpvcross(delta, seg_b) <= 0.0f){
|
||||
cpFloat d_offset = d + (d > 0.0f ? -r : r);
|
||||
cpFloat ad = -d_offset;
|
||||
cpFloat bd = cpvdot(delta, n) - d_offset;
|
||||
|
||||
if(ad*bd < 0.0f){
|
||||
cpFloat t = ad/(ad - bd);
|
||||
|
||||
info->shape = (cpShape *)seg;
|
||||
info->point = cpvsub(cpvlerp(a, b, t), cpvmult(flipped_n, r2));
|
||||
info->normal = flipped_n;
|
||||
info->alpha = t;
|
||||
}
|
||||
} else if(r != 0.0f){
|
||||
cpSegmentQueryInfo info1 = {NULL, b, cpvzero, 1.0f};
|
||||
cpSegmentQueryInfo info2 = {NULL, b, cpvzero, 1.0f};
|
||||
CircleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, r2, &info1);
|
||||
CircleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, r2, &info2);
|
||||
|
||||
if(info1.alpha < info2.alpha){
|
||||
(*info) = info1;
|
||||
} else {
|
||||
(*info) = info2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static struct cpShapeMassInfo
|
||||
cpSegmentShapeMassInfo(cpFloat mass, cpVect a, cpVect b, cpFloat r)
|
||||
{
|
||||
struct cpShapeMassInfo info = {
|
||||
mass, cpMomentForBox(1.0f, cpvdist(a, b) + 2.0f*r, 2.0f*r), // TODO is an approximation.
|
||||
cpvlerp(a, b, 0.5f),
|
||||
cpAreaForSegment(a, b, r),
|
||||
};
|
||||
|
||||
return info;
|
||||
}
|
||||
|
||||
static const cpShapeClass cpSegmentShapeClass = {
|
||||
CP_SEGMENT_SHAPE,
|
||||
(cpShapeCacheDataImpl)cpSegmentShapeCacheData,
|
||||
NULL,
|
||||
(cpShapePointQueryImpl)cpSegmentShapePointQuery,
|
||||
(cpShapeSegmentQueryImpl)cpSegmentShapeSegmentQuery,
|
||||
};
|
||||
|
||||
cpSegmentShape *
|
||||
cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r)
|
||||
{
|
||||
seg->a = a;
|
||||
seg->b = b;
|
||||
seg->n = cpvrperp(cpvnormalize(cpvsub(b, a)));
|
||||
|
||||
seg->r = r;
|
||||
|
||||
seg->a_tangent = cpvzero;
|
||||
seg->b_tangent = cpvzero;
|
||||
|
||||
cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body, cpSegmentShapeMassInfo(0.0f, a, b, r));
|
||||
|
||||
return seg;
|
||||
}
|
||||
|
||||
cpShape*
|
||||
cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r)
|
||||
{
|
||||
return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpSegmentShapeGetA(const cpShape *shape)
|
||||
{
|
||||
cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
|
||||
return ((cpSegmentShape *)shape)->a;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpSegmentShapeGetB(const cpShape *shape)
|
||||
{
|
||||
cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
|
||||
return ((cpSegmentShape *)shape)->b;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpSegmentShapeGetNormal(const cpShape *shape)
|
||||
{
|
||||
cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
|
||||
return ((cpSegmentShape *)shape)->n;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpSegmentShapeGetRadius(const cpShape *shape)
|
||||
{
|
||||
cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
|
||||
return ((cpSegmentShape *)shape)->r;
|
||||
}
|
||||
|
||||
void
|
||||
cpSegmentShapeSetNeighbors(cpShape *shape, cpVect prev, cpVect next)
|
||||
{
|
||||
cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
|
||||
cpSegmentShape *seg = (cpSegmentShape *)shape;
|
||||
|
||||
seg->a_tangent = cpvsub(prev, seg->a);
|
||||
seg->b_tangent = cpvsub(next, seg->b);
|
||||
}
|
||||
|
||||
// Unsafe API (chipmunk_unsafe.h)
|
||||
|
||||
// TODO setters should wake the shape up?
|
||||
|
||||
void
|
||||
cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
|
||||
{
|
||||
cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
|
||||
cpCircleShape *circle = (cpCircleShape *)shape;
|
||||
|
||||
circle->r = radius;
|
||||
|
||||
cpFloat mass = shape->massInfo.m;
|
||||
shape->massInfo = cpCircleShapeMassInfo(mass, circle->r, circle->c);
|
||||
if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
|
||||
}
|
||||
|
||||
void
|
||||
cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
|
||||
{
|
||||
cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
|
||||
cpCircleShape *circle = (cpCircleShape *)shape;
|
||||
|
||||
circle->c = offset;
|
||||
|
||||
cpFloat mass = shape->massInfo.m;
|
||||
shape->massInfo = cpCircleShapeMassInfo(shape->massInfo.m, circle->r, circle->c);
|
||||
if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
|
||||
}
|
||||
|
||||
void
|
||||
cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
|
||||
{
|
||||
cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
|
||||
cpSegmentShape *seg = (cpSegmentShape *)shape;
|
||||
|
||||
seg->a = a;
|
||||
seg->b = b;
|
||||
seg->n = cpvperp(cpvnormalize(cpvsub(b, a)));
|
||||
|
||||
cpFloat mass = shape->massInfo.m;
|
||||
shape->massInfo = cpSegmentShapeMassInfo(shape->massInfo.m, seg->a, seg->b, seg->r);
|
||||
if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
|
||||
}
|
||||
|
||||
void
|
||||
cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
|
||||
{
|
||||
cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
|
||||
cpSegmentShape *seg = (cpSegmentShape *)shape;
|
||||
|
||||
seg->r = radius;
|
||||
|
||||
cpFloat mass = shape->massInfo.m;
|
||||
shape->massInfo = cpSegmentShapeMassInfo(shape->massInfo.m, seg->a, seg->b, seg->r);
|
||||
if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
|
||||
}
|
||||
|
|
@ -0,0 +1,123 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
static void
|
||||
preStep(cpSimpleMotor *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
// calculate moment of inertia coefficient.
|
||||
joint->iSum = 1.0f/(a->i_inv + b->i_inv);
|
||||
}
|
||||
|
||||
static void
|
||||
applyCachedImpulse(cpSimpleMotor *joint, cpFloat dt_coef)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
cpFloat j = joint->jAcc*dt_coef;
|
||||
a->w -= j*a->i_inv;
|
||||
b->w += j*b->i_inv;
|
||||
}
|
||||
|
||||
static void
|
||||
applyImpulse(cpSimpleMotor *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
// compute relative rotational velocity
|
||||
cpFloat wr = b->w - a->w + joint->rate;
|
||||
|
||||
cpFloat jMax = joint->constraint.maxForce*dt;
|
||||
|
||||
// compute normal impulse
|
||||
cpFloat j = -wr*joint->iSum;
|
||||
cpFloat jOld = joint->jAcc;
|
||||
joint->jAcc = cpfclamp(jOld + j, -jMax, jMax);
|
||||
j = joint->jAcc - jOld;
|
||||
|
||||
// apply impulse
|
||||
a->w -= j*a->i_inv;
|
||||
b->w += j*b->i_inv;
|
||||
}
|
||||
|
||||
static cpFloat
|
||||
getImpulse(cpSimpleMotor *joint)
|
||||
{
|
||||
return cpfabs(joint->jAcc);
|
||||
}
|
||||
|
||||
static const cpConstraintClass klass = {
|
||||
(cpConstraintPreStepImpl)preStep,
|
||||
(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
|
||||
(cpConstraintApplyImpulseImpl)applyImpulse,
|
||||
(cpConstraintGetImpulseImpl)getImpulse,
|
||||
};
|
||||
|
||||
cpSimpleMotor *
|
||||
cpSimpleMotorAlloc(void)
|
||||
{
|
||||
return (cpSimpleMotor *)cpcalloc(1, sizeof(cpSimpleMotor));
|
||||
}
|
||||
|
||||
cpSimpleMotor *
|
||||
cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate)
|
||||
{
|
||||
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
||||
|
||||
joint->rate = rate;
|
||||
|
||||
joint->jAcc = 0.0f;
|
||||
|
||||
return joint;
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate)
|
||||
{
|
||||
return (cpConstraint *)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpConstraintIsSimpleMotor(const cpConstraint *constraint)
|
||||
{
|
||||
return (constraint->klass == &klass);
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpSimpleMotorGetRate(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsSimpleMotor(constraint), "Constraint is not a pin joint.");
|
||||
return ((cpSimpleMotor *)constraint)->rate;
|
||||
}
|
||||
|
||||
void
|
||||
cpSimpleMotorSetRate(cpConstraint *constraint, cpFloat rate)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsSimpleMotor(constraint), "Constraint is not a pin joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpSimpleMotor *)constraint)->rate = rate;
|
||||
}
|
||||
|
|
@ -0,0 +1,195 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
static void
|
||||
preStep(cpSlideJoint *joint, cpFloat dt)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog));
|
||||
joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
|
||||
|
||||
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
|
||||
cpFloat dist = cpvlength(delta);
|
||||
cpFloat pdist = 0.0f;
|
||||
if(dist > joint->max) {
|
||||
pdist = dist - joint->max;
|
||||
joint->n = cpvnormalize(delta);
|
||||
} else if(dist < joint->min) {
|
||||
pdist = joint->min - dist;
|
||||
joint->n = cpvneg(cpvnormalize(delta));
|
||||
} else {
|
||||
joint->n = cpvzero;
|
||||
joint->jnAcc = 0.0f;
|
||||
}
|
||||
|
||||
// calculate mass normal
|
||||
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
|
||||
|
||||
// calculate bias velocity
|
||||
cpFloat maxBias = joint->constraint.maxBias;
|
||||
joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias);
|
||||
}
|
||||
|
||||
static void
|
||||
applyCachedImpulse(cpSlideJoint *joint, cpFloat dt_coef)
|
||||
{
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef);
|
||||
apply_impulses(a, b, joint->r1, joint->r2, j);
|
||||
}
|
||||
|
||||
static void
|
||||
applyImpulse(cpSlideJoint *joint, cpFloat dt)
|
||||
{
|
||||
if(cpveql(joint->n, cpvzero)) return; // early exit
|
||||
|
||||
cpBody *a = joint->constraint.a;
|
||||
cpBody *b = joint->constraint.b;
|
||||
|
||||
cpVect n = joint->n;
|
||||
cpVect r1 = joint->r1;
|
||||
cpVect r2 = joint->r2;
|
||||
|
||||
// compute relative velocity
|
||||
cpVect vr = relative_velocity(a, b, r1, r2);
|
||||
cpFloat vrn = cpvdot(vr, n);
|
||||
|
||||
// compute normal impulse
|
||||
cpFloat jn = (joint->bias - vrn)*joint->nMass;
|
||||
cpFloat jnOld = joint->jnAcc;
|
||||
joint->jnAcc = cpfclamp(jnOld + jn, -joint->constraint.maxForce*dt, 0.0f);
|
||||
jn = joint->jnAcc - jnOld;
|
||||
|
||||
// apply impulse
|
||||
apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
|
||||
}
|
||||
|
||||
static cpFloat
|
||||
getImpulse(cpConstraint *joint)
|
||||
{
|
||||
return cpfabs(((cpSlideJoint *)joint)->jnAcc);
|
||||
}
|
||||
|
||||
static const cpConstraintClass klass = {
|
||||
(cpConstraintPreStepImpl)preStep,
|
||||
(cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
|
||||
(cpConstraintApplyImpulseImpl)applyImpulse,
|
||||
(cpConstraintGetImpulseImpl)getImpulse,
|
||||
};
|
||||
|
||||
cpSlideJoint *
|
||||
cpSlideJointAlloc(void)
|
||||
{
|
||||
return (cpSlideJoint *)cpcalloc(1, sizeof(cpSlideJoint));
|
||||
}
|
||||
|
||||
cpSlideJoint *
|
||||
cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max)
|
||||
{
|
||||
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
|
||||
|
||||
joint->anchorA = anchorA;
|
||||
joint->anchorB = anchorB;
|
||||
joint->min = min;
|
||||
joint->max = max;
|
||||
|
||||
joint->jnAcc = 0.0f;
|
||||
|
||||
return joint;
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max)
|
||||
{
|
||||
return (cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchorA, anchorB, min, max);
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpConstraintIsSlideJoint(const cpConstraint *constraint)
|
||||
{
|
||||
return (constraint->klass == &klass);
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpSlideJointGetAnchorA(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
|
||||
return ((cpSlideJoint *)constraint)->anchorA;
|
||||
}
|
||||
|
||||
void
|
||||
cpSlideJointSetAnchorA(cpConstraint *constraint, cpVect anchorA)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpSlideJoint *)constraint)->anchorA = anchorA;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpSlideJointGetAnchorB(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
|
||||
return ((cpSlideJoint *)constraint)->anchorB;
|
||||
}
|
||||
|
||||
void
|
||||
cpSlideJointSetAnchorB(cpConstraint *constraint, cpVect anchorB)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpSlideJoint *)constraint)->anchorB = anchorB;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpSlideJointGetMin(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
|
||||
return ((cpSlideJoint *)constraint)->min;
|
||||
}
|
||||
|
||||
void
|
||||
cpSlideJointSetMin(cpConstraint *constraint, cpFloat min)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpSlideJoint *)constraint)->min = min;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpSlideJointGetMax(const cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
|
||||
return ((cpSlideJoint *)constraint)->max;
|
||||
}
|
||||
|
||||
void
|
||||
cpSlideJointSetMax(cpConstraint *constraint, cpFloat max)
|
||||
{
|
||||
cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
|
||||
cpConstraintActivateBodies(constraint);
|
||||
((cpSlideJoint *)constraint)->max = max;
|
||||
}
|
||||
|
|
@ -0,0 +1,700 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
//MARK: Contact Set Helpers
|
||||
|
||||
// Equal function for arbiterSet.
|
||||
static cpBool
|
||||
arbiterSetEql(cpShape **shapes, cpArbiter *arb)
|
||||
{
|
||||
cpShape *a = shapes[0];
|
||||
cpShape *b = shapes[1];
|
||||
|
||||
return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b));
|
||||
}
|
||||
|
||||
//MARK: Collision Handler Set HelperFunctions
|
||||
|
||||
// Equals function for collisionHandlers.
|
||||
static cpBool
|
||||
handlerSetEql(cpCollisionHandler *check, cpCollisionHandler *pair)
|
||||
{
|
||||
return (
|
||||
(check->typeA == pair->typeA && check->typeB == pair->typeB) ||
|
||||
(check->typeB == pair->typeA && check->typeA == pair->typeB)
|
||||
);
|
||||
}
|
||||
|
||||
// Transformation function for collisionHandlers.
|
||||
static void *
|
||||
handlerSetTrans(cpCollisionHandler *handler, void *unused)
|
||||
{
|
||||
cpCollisionHandler *copy = (cpCollisionHandler *)cpcalloc(1, sizeof(cpCollisionHandler));
|
||||
memcpy(copy, handler, sizeof(cpCollisionHandler));
|
||||
|
||||
return copy;
|
||||
}
|
||||
|
||||
//MARK: Misc Helper Funcs
|
||||
|
||||
// Default collision functions.
|
||||
|
||||
static cpBool
|
||||
DefaultBegin(cpArbiter *arb, cpSpace *space, void *data){
|
||||
cpBool retA = cpArbiterCallWildcardBeginA(arb, space);
|
||||
cpBool retB = cpArbiterCallWildcardBeginB(arb, space);
|
||||
return retA && retB;
|
||||
}
|
||||
|
||||
static cpBool
|
||||
DefaultPreSolve(cpArbiter *arb, cpSpace *space, void *data){
|
||||
cpBool retA = cpArbiterCallWildcardPreSolveA(arb, space);
|
||||
cpBool retB = cpArbiterCallWildcardPreSolveB(arb, space);
|
||||
return retA && retB;
|
||||
}
|
||||
|
||||
static void
|
||||
DefaultPostSolve(cpArbiter *arb, cpSpace *space, void *data){
|
||||
cpArbiterCallWildcardPostSolveA(arb, space);
|
||||
cpArbiterCallWildcardPostSolveB(arb, space);
|
||||
}
|
||||
|
||||
static void
|
||||
DefaultSeparate(cpArbiter *arb, cpSpace *space, void *data){
|
||||
cpArbiterCallWildcardSeparateA(arb, space);
|
||||
cpArbiterCallWildcardSeparateB(arb, space);
|
||||
}
|
||||
|
||||
// Use the wildcard identifier since the default handler should never match any type pair.
|
||||
static cpCollisionHandler cpCollisionHandlerDefault = {
|
||||
CP_WILDCARD_COLLISION_TYPE, CP_WILDCARD_COLLISION_TYPE,
|
||||
DefaultBegin, DefaultPreSolve, DefaultPostSolve, DefaultSeparate, NULL
|
||||
};
|
||||
|
||||
static cpBool AlwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return cpTrue;}
|
||||
static void DoNothing(cpArbiter *arb, cpSpace *space, void *data){}
|
||||
|
||||
cpCollisionHandler cpCollisionHandlerDoNothing = {
|
||||
CP_WILDCARD_COLLISION_TYPE, CP_WILDCARD_COLLISION_TYPE,
|
||||
AlwaysCollide, AlwaysCollide, DoNothing, DoNothing, NULL
|
||||
};
|
||||
|
||||
// function to get the estimated velocity of a shape for the cpBBTree.
|
||||
static cpVect ShapeVelocityFunc(cpShape *shape){return shape->body->v;}
|
||||
|
||||
// Used for disposing of collision handlers.
|
||||
static void FreeWrap(void *ptr, void *unused){cpfree(ptr);}
|
||||
|
||||
//MARK: Memory Management Functions
|
||||
|
||||
cpSpace *
|
||||
cpSpaceAlloc(void)
|
||||
{
|
||||
return (cpSpace *)cpcalloc(1, sizeof(cpSpace));
|
||||
}
|
||||
|
||||
cpSpace*
|
||||
cpSpaceInit(cpSpace *space)
|
||||
{
|
||||
#ifndef NDEBUG
|
||||
static cpBool done = cpFalse;
|
||||
if(!done){
|
||||
printf("Initializing cpSpace - Chipmunk v%s (Debug Enabled)\n", cpVersionString);
|
||||
printf("Compile with -DNDEBUG defined to disable debug mode and runtime assertion checks\n");
|
||||
done = cpTrue;
|
||||
}
|
||||
#endif
|
||||
|
||||
space->iterations = 10;
|
||||
|
||||
space->gravity = cpvzero;
|
||||
space->damping = 1.0f;
|
||||
|
||||
space->collisionSlop = 0.1f;
|
||||
space->collisionBias = cpfpow(1.0f - 0.1f, 60.0f);
|
||||
space->collisionPersistence = 3;
|
||||
|
||||
space->locked = 0;
|
||||
space->stamp = 0;
|
||||
|
||||
space->shapeIDCounter = 0;
|
||||
space->staticShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, NULL);
|
||||
space->dynamicShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, space->staticShapes);
|
||||
cpBBTreeSetVelocityFunc(space->dynamicShapes, (cpBBTreeVelocityFunc)ShapeVelocityFunc);
|
||||
|
||||
space->allocatedBuffers = cpArrayNew(0);
|
||||
|
||||
space->dynamicBodies = cpArrayNew(0);
|
||||
space->staticBodies = cpArrayNew(0);
|
||||
space->sleepingComponents = cpArrayNew(0);
|
||||
space->rousedBodies = cpArrayNew(0);
|
||||
|
||||
space->sleepTimeThreshold = INFINITY;
|
||||
space->idleSpeedThreshold = 0.0f;
|
||||
|
||||
space->arbiters = cpArrayNew(0);
|
||||
space->pooledArbiters = cpArrayNew(0);
|
||||
|
||||
space->contactBuffersHead = NULL;
|
||||
space->cachedArbiters = cpHashSetNew(0, (cpHashSetEqlFunc)arbiterSetEql);
|
||||
|
||||
space->constraints = cpArrayNew(0);
|
||||
|
||||
space->usesWildcards = cpFalse;
|
||||
memcpy(&space->defaultHandler, &cpCollisionHandlerDoNothing, sizeof(cpCollisionHandler));
|
||||
space->collisionHandlers = cpHashSetNew(0, (cpHashSetEqlFunc)handlerSetEql);
|
||||
|
||||
space->postStepCallbacks = cpArrayNew(0);
|
||||
space->skipPostStep = cpFalse;
|
||||
|
||||
cpBody *staticBody = cpBodyInit(&space->_staticBody, 0.0f, 0.0f);
|
||||
cpBodySetType(staticBody, CP_BODY_TYPE_STATIC);
|
||||
cpSpaceSetStaticBody(space, staticBody);
|
||||
|
||||
return space;
|
||||
}
|
||||
|
||||
cpSpace*
|
||||
cpSpaceNew(void)
|
||||
{
|
||||
return cpSpaceInit(cpSpaceAlloc());
|
||||
}
|
||||
|
||||
static void cpBodyActivateWrap(cpBody *body, void *unused){cpBodyActivate(body);}
|
||||
|
||||
void
|
||||
cpSpaceDestroy(cpSpace *space)
|
||||
{
|
||||
cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)cpBodyActivateWrap, NULL);
|
||||
|
||||
cpSpatialIndexFree(space->staticShapes);
|
||||
cpSpatialIndexFree(space->dynamicShapes);
|
||||
|
||||
cpArrayFree(space->dynamicBodies);
|
||||
cpArrayFree(space->staticBodies);
|
||||
cpArrayFree(space->sleepingComponents);
|
||||
cpArrayFree(space->rousedBodies);
|
||||
|
||||
cpArrayFree(space->constraints);
|
||||
|
||||
cpHashSetFree(space->cachedArbiters);
|
||||
|
||||
cpArrayFree(space->arbiters);
|
||||
cpArrayFree(space->pooledArbiters);
|
||||
|
||||
if(space->allocatedBuffers){
|
||||
cpArrayFreeEach(space->allocatedBuffers, cpfree);
|
||||
cpArrayFree(space->allocatedBuffers);
|
||||
}
|
||||
|
||||
if(space->postStepCallbacks){
|
||||
cpArrayFreeEach(space->postStepCallbacks, cpfree);
|
||||
cpArrayFree(space->postStepCallbacks);
|
||||
}
|
||||
|
||||
if(space->collisionHandlers) cpHashSetEach(space->collisionHandlers, FreeWrap, NULL);
|
||||
cpHashSetFree(space->collisionHandlers);
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceFree(cpSpace *space)
|
||||
{
|
||||
if(space){
|
||||
cpSpaceDestroy(space);
|
||||
cpfree(space);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//MARK: Basic properties:
|
||||
|
||||
int
|
||||
cpSpaceGetIterations(const cpSpace *space)
|
||||
{
|
||||
return space->iterations;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSetIterations(cpSpace *space, int iterations)
|
||||
{
|
||||
cpAssertHard(iterations > 0, "Iterations must be positive and non-zero.");
|
||||
space->iterations = iterations;
|
||||
}
|
||||
|
||||
cpVect
|
||||
cpSpaceGetGravity(const cpSpace *space)
|
||||
{
|
||||
return space->gravity;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSetGravity(cpSpace *space, cpVect gravity)
|
||||
{
|
||||
space->gravity = gravity;
|
||||
|
||||
// Wake up all of the bodies since the gravity changed.
|
||||
cpArray *components = space->sleepingComponents;
|
||||
for(int i=0; i<components->num; i++){
|
||||
cpBodyActivate((cpBody *)components->arr[i]);
|
||||
}
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpSpaceGetDamping(const cpSpace *space)
|
||||
{
|
||||
return space->damping;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSetDamping(cpSpace *space, cpFloat damping)
|
||||
{
|
||||
cpAssertHard(damping >= 0.0, "Damping must be positive.");
|
||||
space->damping = damping;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpSpaceGetIdleSpeedThreshold(const cpSpace *space)
|
||||
{
|
||||
return space->idleSpeedThreshold;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSetIdleSpeedThreshold(cpSpace *space, cpFloat idleSpeedThreshold)
|
||||
{
|
||||
space->idleSpeedThreshold = idleSpeedThreshold;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpSpaceGetSleepTimeThreshold(const cpSpace *space)
|
||||
{
|
||||
return space->sleepTimeThreshold;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSetSleepTimeThreshold(cpSpace *space, cpFloat sleepTimeThreshold)
|
||||
{
|
||||
space->sleepTimeThreshold = sleepTimeThreshold;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpSpaceGetCollisionSlop(const cpSpace *space)
|
||||
{
|
||||
return space->collisionSlop;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSetCollisionSlop(cpSpace *space, cpFloat collisionSlop)
|
||||
{
|
||||
space->collisionSlop = collisionSlop;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpSpaceGetCollisionBias(const cpSpace *space)
|
||||
{
|
||||
return space->collisionBias;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSetCollisionBias(cpSpace *space, cpFloat collisionBias)
|
||||
{
|
||||
space->collisionBias = collisionBias;
|
||||
}
|
||||
|
||||
cpTimestamp
|
||||
cpSpaceGetCollisionPersistence(const cpSpace *space)
|
||||
{
|
||||
return space->collisionPersistence;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSetCollisionPersistence(cpSpace *space, cpTimestamp collisionPersistence)
|
||||
{
|
||||
space->collisionPersistence = collisionPersistence;
|
||||
}
|
||||
|
||||
cpDataPointer
|
||||
cpSpaceGetUserData(const cpSpace *space)
|
||||
{
|
||||
return space->userData;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSetUserData(cpSpace *space, cpDataPointer userData)
|
||||
{
|
||||
space->userData = userData;
|
||||
}
|
||||
|
||||
cpBody *
|
||||
cpSpaceGetStaticBody(const cpSpace *space)
|
||||
{
|
||||
return space->staticBody;
|
||||
}
|
||||
|
||||
cpFloat
|
||||
cpSpaceGetCurrentTimeStep(const cpSpace *space)
|
||||
{
|
||||
return space->curr_dt;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSetStaticBody(cpSpace *space, cpBody *body)
|
||||
{
|
||||
if(space->staticBody != NULL){
|
||||
cpAssertHard(space->staticBody->shapeList == NULL, "Internal Error: Changing the designated static body while the old one still had shapes attached.");
|
||||
space->staticBody->space = NULL;
|
||||
}
|
||||
|
||||
space->staticBody = body;
|
||||
body->space = space;
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpSpaceIsLocked(cpSpace *space)
|
||||
{
|
||||
return (space->locked > 0);
|
||||
}
|
||||
|
||||
//MARK: Collision Handler Function Management
|
||||
|
||||
static void
|
||||
cpSpaceUseWildcardDefaultHandler(cpSpace *space)
|
||||
{
|
||||
// Spaces default to using the slightly faster "do nothing" default handler until wildcards are potentially needed.
|
||||
if(!space->usesWildcards){
|
||||
space->usesWildcards = cpTrue;
|
||||
memcpy(&space->defaultHandler, &cpCollisionHandlerDefault, sizeof(cpCollisionHandler));
|
||||
}
|
||||
}
|
||||
|
||||
cpCollisionHandler *cpSpaceAddDefaultCollisionHandler(cpSpace *space)
|
||||
{
|
||||
cpSpaceUseWildcardDefaultHandler(space);
|
||||
return &space->defaultHandler;
|
||||
}
|
||||
|
||||
cpCollisionHandler *cpSpaceAddCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b)
|
||||
{
|
||||
cpHashValue hash = CP_HASH_PAIR(a, b);
|
||||
cpCollisionHandler handler = {a, b, DefaultBegin, DefaultPreSolve, DefaultPostSolve, DefaultSeparate, NULL};
|
||||
return (cpCollisionHandler*)cpHashSetInsert(space->collisionHandlers, hash, &handler, (cpHashSetTransFunc)handlerSetTrans, NULL);
|
||||
}
|
||||
|
||||
cpCollisionHandler *
|
||||
cpSpaceAddWildcardHandler(cpSpace *space, cpCollisionType type)
|
||||
{
|
||||
cpSpaceUseWildcardDefaultHandler(space);
|
||||
|
||||
cpHashValue hash = CP_HASH_PAIR(type, CP_WILDCARD_COLLISION_TYPE);
|
||||
cpCollisionHandler handler = {type, CP_WILDCARD_COLLISION_TYPE, AlwaysCollide, AlwaysCollide, DoNothing, DoNothing, NULL};
|
||||
return (cpCollisionHandler*)cpHashSetInsert(space->collisionHandlers, hash, &handler, (cpHashSetTransFunc)handlerSetTrans, NULL);
|
||||
}
|
||||
|
||||
|
||||
//MARK: Body, Shape, and Joint Management
|
||||
cpShape *
|
||||
cpSpaceAddShape(cpSpace *space, cpShape *shape)
|
||||
{
|
||||
cpBody *body = shape->body;
|
||||
|
||||
cpAssertHard(shape->space != space, "You have already added this shape to this space. You must not add it a second time.");
|
||||
cpAssertHard(!shape->space, "You have already added this shape to another space. You cannot add it to a second.");
|
||||
// cpAssertHard(body->space == space, "The shape's body must be added to the space before the shape.");
|
||||
cpAssertSpaceUnlocked(space);
|
||||
|
||||
cpBool isStatic = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC);
|
||||
if(!isStatic) cpBodyActivate(body);
|
||||
cpBodyAddShape(body, shape);
|
||||
|
||||
shape->hashid = space->shapeIDCounter++;
|
||||
cpShapeUpdate(shape, body->transform);
|
||||
cpSpatialIndexInsert(isStatic ? space->staticShapes : space->dynamicShapes, shape, shape->hashid);
|
||||
shape->space = space;
|
||||
|
||||
return shape;
|
||||
}
|
||||
|
||||
cpBody *
|
||||
cpSpaceAddBody(cpSpace *space, cpBody *body)
|
||||
{
|
||||
cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time.");
|
||||
cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second.");
|
||||
cpAssertSpaceUnlocked(space);
|
||||
|
||||
cpArrayPush(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body);
|
||||
body->space = space;
|
||||
|
||||
return body;
|
||||
}
|
||||
|
||||
cpConstraint *
|
||||
cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(constraint->space != space, "You have already added this constraint to this space. You must not add it a second time.");
|
||||
cpAssertHard(!constraint->space, "You have already added this constraint to another space. You cannot add it to a second.");
|
||||
cpAssertSpaceUnlocked(space);
|
||||
|
||||
cpBody *a = constraint->a, *b = constraint->b;
|
||||
cpAssertHard(a != NULL && b != NULL, "Constraint is attached to a NULL body.");
|
||||
// cpAssertHard(a->space == space && b->space == space, "The constraint's bodies must be added to the space before the constraint.");
|
||||
|
||||
cpBodyActivate(a);
|
||||
cpBodyActivate(b);
|
||||
cpArrayPush(space->constraints, constraint);
|
||||
|
||||
// Push onto the heads of the bodies' constraint lists
|
||||
constraint->next_a = a->constraintList; a->constraintList = constraint;
|
||||
constraint->next_b = b->constraintList; b->constraintList = constraint;
|
||||
constraint->space = space;
|
||||
|
||||
return constraint;
|
||||
}
|
||||
|
||||
struct arbiterFilterContext {
|
||||
cpSpace *space;
|
||||
cpBody *body;
|
||||
cpShape *shape;
|
||||
};
|
||||
|
||||
static cpBool
|
||||
cachedArbitersFilter(cpArbiter *arb, struct arbiterFilterContext *context)
|
||||
{
|
||||
cpShape *shape = context->shape;
|
||||
cpBody *body = context->body;
|
||||
|
||||
|
||||
// Match on the filter shape, or if it's NULL the filter body
|
||||
if(
|
||||
(body == arb->body_a && (shape == arb->a || shape == NULL)) ||
|
||||
(body == arb->body_b && (shape == arb->b || shape == NULL))
|
||||
){
|
||||
// Call separate when removing shapes.
|
||||
if(shape && arb->state != CP_ARBITER_STATE_CACHED){
|
||||
// Invalidate the arbiter since one of the shapes was removed.
|
||||
arb->state = CP_ARBITER_STATE_INVALIDATED;
|
||||
|
||||
cpCollisionHandler *handler = arb->handler;
|
||||
handler->separateFunc(arb, context->space, handler->userData);
|
||||
}
|
||||
|
||||
cpArbiterUnthread(arb);
|
||||
cpArrayDeleteObj(context->space->arbiters, arb);
|
||||
cpArrayPush(context->space->pooledArbiters, arb);
|
||||
|
||||
return cpFalse;
|
||||
}
|
||||
|
||||
return cpTrue;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter)
|
||||
{
|
||||
cpSpaceLock(space); {
|
||||
struct arbiterFilterContext context = {space, body, filter};
|
||||
cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cachedArbitersFilter, &context);
|
||||
} cpSpaceUnlock(space, cpTrue);
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
|
||||
{
|
||||
cpBody *body = shape->body;
|
||||
cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a shape that was not added to the space. (Removed twice maybe?)");
|
||||
cpAssertSpaceUnlocked(space);
|
||||
|
||||
cpBool isStatic = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC);
|
||||
if(isStatic){
|
||||
cpBodyActivateStatic(body, shape);
|
||||
} else {
|
||||
cpBodyActivate(body);
|
||||
}
|
||||
|
||||
cpBodyRemoveShape(body, shape);
|
||||
cpSpaceFilterArbiters(space, body, shape);
|
||||
cpSpatialIndexRemove(isStatic ? space->staticShapes : space->dynamicShapes, shape, shape->hashid);
|
||||
shape->space = NULL;
|
||||
shape->hashid = 0;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
|
||||
{
|
||||
cpAssertHard(body != cpSpaceGetStaticBody(space), "Cannot remove the designated static body for the space.");
|
||||
cpAssertHard(cpSpaceContainsBody(space, body), "Cannot remove a body that was not added to the space. (Removed twice maybe?)");
|
||||
// cpAssertHard(body->shapeList == NULL, "Cannot remove a body from the space before removing the bodies attached to it.");
|
||||
// cpAssertHard(body->constraintList == NULL, "Cannot remove a body from the space before removing the constraints attached to it.");
|
||||
cpAssertSpaceUnlocked(space);
|
||||
|
||||
cpBodyActivate(body);
|
||||
// cpSpaceFilterArbiters(space, body, NULL);
|
||||
cpArrayDeleteObj(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body);
|
||||
body->space = NULL;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
|
||||
{
|
||||
cpAssertHard(cpSpaceContainsConstraint(space, constraint), "Cannot remove a constraint that was not added to the space. (Removed twice maybe?)");
|
||||
cpAssertSpaceUnlocked(space);
|
||||
|
||||
cpBodyActivate(constraint->a);
|
||||
cpBodyActivate(constraint->b);
|
||||
cpArrayDeleteObj(space->constraints, constraint);
|
||||
|
||||
cpBodyRemoveConstraint(constraint->a, constraint);
|
||||
cpBodyRemoveConstraint(constraint->b, constraint);
|
||||
constraint->space = NULL;
|
||||
}
|
||||
|
||||
cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape)
|
||||
{
|
||||
return (shape->space == space);
|
||||
}
|
||||
|
||||
cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body)
|
||||
{
|
||||
return (body->space == space);
|
||||
}
|
||||
|
||||
cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint)
|
||||
{
|
||||
return (constraint->space == space);
|
||||
}
|
||||
|
||||
//MARK: Iteration
|
||||
|
||||
void
|
||||
cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data)
|
||||
{
|
||||
cpSpaceLock(space); {
|
||||
cpArray *bodies = space->dynamicBodies;
|
||||
for(int i=0; i<bodies->num; i++){
|
||||
func((cpBody *)bodies->arr[i], data);
|
||||
}
|
||||
|
||||
cpArray *otherBodies = space->staticBodies;
|
||||
for(int i=0; i<otherBodies->num; i++){
|
||||
func((cpBody *)otherBodies->arr[i], data);
|
||||
}
|
||||
|
||||
cpArray *components = space->sleepingComponents;
|
||||
for(int i=0; i<components->num; i++){
|
||||
cpBody *root = (cpBody *)components->arr[i];
|
||||
|
||||
cpBody *body = root;
|
||||
while(body){
|
||||
cpBody *next = body->sleeping.next;
|
||||
func(body, data);
|
||||
body = next;
|
||||
}
|
||||
}
|
||||
} cpSpaceUnlock(space, cpTrue);
|
||||
}
|
||||
|
||||
typedef struct spaceShapeContext {
|
||||
cpSpaceShapeIteratorFunc func;
|
||||
void *data;
|
||||
} spaceShapeContext;
|
||||
|
||||
static void
|
||||
spaceEachShapeIterator(cpShape *shape, spaceShapeContext *context)
|
||||
{
|
||||
context->func(shape, context->data);
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data)
|
||||
{
|
||||
cpSpaceLock(space); {
|
||||
spaceShapeContext context = {func, data};
|
||||
cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context);
|
||||
cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context);
|
||||
} cpSpaceUnlock(space, cpTrue);
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void *data)
|
||||
{
|
||||
cpSpaceLock(space); {
|
||||
cpArray *constraints = space->constraints;
|
||||
|
||||
for(int i=0; i<constraints->num; i++){
|
||||
func((cpConstraint *)constraints->arr[i], data);
|
||||
}
|
||||
} cpSpaceUnlock(space, cpTrue);
|
||||
}
|
||||
|
||||
//MARK: Spatial Index Management
|
||||
|
||||
void
|
||||
cpSpaceReindexStatic(cpSpace *space)
|
||||
{
|
||||
cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete.");
|
||||
|
||||
cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)&cpShapeUpdateFunc, NULL);
|
||||
cpSpatialIndexReindex(space->staticShapes);
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceReindexShape(cpSpace *space, cpShape *shape)
|
||||
{
|
||||
cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete.");
|
||||
|
||||
cpShapeCacheBB(shape);
|
||||
|
||||
// attempt to rehash the shape in both hashes
|
||||
cpSpatialIndexReindexObject(space->dynamicShapes, shape, shape->hashid);
|
||||
cpSpatialIndexReindexObject(space->staticShapes, shape, shape->hashid);
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceReindexShapesForBody(cpSpace *space, cpBody *body)
|
||||
{
|
||||
CP_BODY_FOREACH_SHAPE(body, shape) cpSpaceReindexShape(space, shape);
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
copyShapes(cpShape *shape, cpSpatialIndex *index)
|
||||
{
|
||||
cpSpatialIndexInsert(index, shape, shape->hashid);
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count)
|
||||
{
|
||||
cpSpatialIndex *staticShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, NULL);
|
||||
cpSpatialIndex *dynamicShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, staticShapes);
|
||||
|
||||
cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)copyShapes, staticShapes);
|
||||
cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)copyShapes, dynamicShapes);
|
||||
|
||||
cpSpatialIndexFree(space->staticShapes);
|
||||
cpSpatialIndexFree(space->dynamicShapes);
|
||||
|
||||
space->staticShapes = staticShapes;
|
||||
space->dynamicShapes = dynamicShapes;
|
||||
}
|
||||
|
|
@ -0,0 +1,349 @@
|
|||
/* Copyright (c) 2007 Scott Lembcke
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 <string.h>
|
||||
|
||||
#include "chipmunk/chipmunk_private.h"
|
||||
|
||||
//MARK: Sleeping Functions
|
||||
|
||||
void
|
||||
cpSpaceActivateBody(cpSpace *space, cpBody *body)
|
||||
{
|
||||
cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Internal error: Attempting to activate a non-dynamic body.");
|
||||
|
||||
if(space->locked){
|
||||
// cpSpaceActivateBody() is called again once the space is unlocked
|
||||
if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body);
|
||||
} else {
|
||||
cpAssertSoft(body->sleeping.root == NULL && body->sleeping.next == NULL, "Internal error: Activating body non-NULL node pointers.");
|
||||
cpArrayPush(space->dynamicBodies, body);
|
||||
|
||||
CP_BODY_FOREACH_SHAPE(body, shape){
|
||||
cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid);
|
||||
cpSpatialIndexInsert(space->dynamicShapes, shape, shape->hashid);
|
||||
}
|
||||
|
||||
CP_BODY_FOREACH_ARBITER(body, arb){
|
||||
cpBody *bodyA = arb->body_a;
|
||||
|
||||
// Arbiters are shared between two bodies that are always woken up together.
|
||||
// You only want to restore the arbiter once, so bodyA is arbitrarily chosen to own the arbiter.
|
||||
// The edge case is when static bodies are involved as the static bodies never actually sleep.
|
||||
// If the static body is bodyB then all is good. If the static body is bodyA, that can easily be checked.
|
||||
if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC){
|
||||
int numContacts = arb->count;
|
||||
struct cpContact *contacts = arb->contacts;
|
||||
|
||||
// Restore contact values back to the space's contact buffer memory
|
||||
arb->contacts = cpContactBufferGetArray(space);
|
||||
memcpy(arb->contacts, contacts, numContacts*sizeof(struct cpContact));
|
||||
cpSpacePushContacts(space, numContacts);
|
||||
|
||||
// Reinsert the arbiter into the arbiter cache
|
||||
const cpShape *a = arb->a, *b = arb->b;
|
||||
const cpShape *shape_pair[] = {a, b};
|
||||
cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b);
|
||||
cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, NULL, arb);
|
||||
|
||||
// Update the arbiter's state
|
||||
arb->stamp = space->stamp;
|
||||
cpArrayPush(space->arbiters, arb);
|
||||
|
||||
cpfree(contacts);
|
||||
}
|
||||
}
|
||||
|
||||
CP_BODY_FOREACH_CONSTRAINT(body, constraint){
|
||||
cpBody *bodyA = constraint->a;
|
||||
if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC) cpArrayPush(space->constraints, constraint);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
cpSpaceDeactivateBody(cpSpace *space, cpBody *body)
|
||||
{
|
||||
cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Internal error: Attempting to deactivate a non-dynamic body.");
|
||||
|
||||
cpArrayDeleteObj(space->dynamicBodies, body);
|
||||
|
||||
CP_BODY_FOREACH_SHAPE(body, shape){
|
||||
cpSpatialIndexRemove(space->dynamicShapes, shape, shape->hashid);
|
||||
cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid);
|
||||
}
|
||||
|
||||
CP_BODY_FOREACH_ARBITER(body, arb){
|
||||
cpBody *bodyA = arb->body_a;
|
||||
if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC){
|
||||
cpSpaceUncacheArbiter(space, arb);
|
||||
|
||||
// Save contact values to a new block of memory so they won't time out
|
||||
size_t bytes = arb->count*sizeof(struct cpContact);
|
||||
struct cpContact *contacts = (struct cpContact *)cpcalloc(1, bytes);
|
||||
memcpy(contacts, arb->contacts, bytes);
|
||||
arb->contacts = contacts;
|
||||
}
|
||||
}
|
||||
|
||||
CP_BODY_FOREACH_CONSTRAINT(body, constraint){
|
||||
cpBody *bodyA = constraint->a;
|
||||
if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC) cpArrayDeleteObj(space->constraints, constraint);
|
||||
}
|
||||
}
|
||||
|
||||
static inline cpBody *
|
||||
ComponentRoot(cpBody *body)
|
||||
{
|
||||
return (body ? body->sleeping.root : NULL);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyActivate(cpBody *body)
|
||||
{
|
||||
if(body != NULL && cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC){
|
||||
body->sleeping.idleTime = 0.0f;
|
||||
|
||||
cpBody *root = ComponentRoot(body);
|
||||
if(root && cpBodyIsSleeping(root)){
|
||||
// TODO should cpBodyIsSleeping(root) be an assertion?
|
||||
cpAssertSoft(cpBodyGetType(root) == CP_BODY_TYPE_DYNAMIC, "Internal Error: Non-dynamic body component root detected.");
|
||||
|
||||
cpSpace *space = root->space;
|
||||
cpBody *body = root;
|
||||
while(body){
|
||||
cpBody *next = body->sleeping.next;
|
||||
|
||||
body->sleeping.idleTime = 0.0f;
|
||||
body->sleeping.root = NULL;
|
||||
body->sleeping.next = NULL;
|
||||
cpSpaceActivateBody(space, body);
|
||||
|
||||
body = next;
|
||||
}
|
||||
|
||||
cpArrayDeleteObj(space->sleepingComponents, root);
|
||||
}
|
||||
|
||||
CP_BODY_FOREACH_ARBITER(body, arb){
|
||||
// Reset the idle timer of things the body is touching as well.
|
||||
// That way things don't get left hanging in the air.
|
||||
cpBody *other = (arb->body_a == body ? arb->body_b : arb->body_a);
|
||||
if(cpBodyGetType(other) != CP_BODY_TYPE_STATIC) other->sleeping.idleTime = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyActivateStatic(cpBody *body, cpShape *filter)
|
||||
{
|
||||
cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_STATIC, "cpBodyActivateStatic() called on a non-static body.");
|
||||
|
||||
CP_BODY_FOREACH_ARBITER(body, arb){
|
||||
if(!filter || filter == arb->a || filter == arb->b){
|
||||
cpBodyActivate(arb->body_a == body ? arb->body_b : arb->body_a);
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: should also activate joints?
|
||||
}
|
||||
|
||||
static inline void
|
||||
cpBodyPushArbiter(cpBody *body, cpArbiter *arb)
|
||||
{
|
||||
cpAssertSoft(cpArbiterThreadForBody(arb, body)->next == NULL, "Internal Error: Dangling contact graph pointers detected. (A)");
|
||||
cpAssertSoft(cpArbiterThreadForBody(arb, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (B)");
|
||||
|
||||
cpArbiter *next = body->arbiterList;
|
||||
cpAssertSoft(next == NULL || cpArbiterThreadForBody(next, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (C)");
|
||||
cpArbiterThreadForBody(arb, body)->next = next;
|
||||
|
||||
if(next) cpArbiterThreadForBody(next, body)->prev = arb;
|
||||
body->arbiterList = arb;
|
||||
}
|
||||
|
||||
static inline void
|
||||
ComponentAdd(cpBody *root, cpBody *body){
|
||||
body->sleeping.root = root;
|
||||
|
||||
if(body != root){
|
||||
body->sleeping.next = root->sleeping.next;
|
||||
root->sleeping.next = body;
|
||||
}
|
||||
}
|
||||
|
||||
static inline void
|
||||
FloodFillComponent(cpBody *root, cpBody *body)
|
||||
{
|
||||
// Kinematic bodies cannot be put to sleep and prevent bodies they are touching from sleeping.
|
||||
// Static bodies are effectively sleeping all the time.
|
||||
if(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC){
|
||||
cpBody *other_root = ComponentRoot(body);
|
||||
if(other_root == NULL){
|
||||
ComponentAdd(root, body);
|
||||
CP_BODY_FOREACH_ARBITER(body, arb) FloodFillComponent(root, (body == arb->body_a ? arb->body_b : arb->body_a));
|
||||
CP_BODY_FOREACH_CONSTRAINT(body, constraint) FloodFillComponent(root, (body == constraint->a ? constraint->b : constraint->a));
|
||||
} else {
|
||||
cpAssertSoft(other_root == root, "Internal Error: Inconsistency dectected in the contact graph.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static inline cpBool
|
||||
ComponentActive(cpBody *root, cpFloat threshold)
|
||||
{
|
||||
CP_BODY_FOREACH_COMPONENT(root, body){
|
||||
if(body->sleeping.idleTime < threshold) return cpTrue;
|
||||
}
|
||||
|
||||
return cpFalse;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
|
||||
{
|
||||
cpBool sleep = (space->sleepTimeThreshold != INFINITY);
|
||||
cpArray *bodies = space->dynamicBodies;
|
||||
|
||||
#ifndef NDEBUG
|
||||
for(int i=0; i<bodies->num; i++){
|
||||
cpBody *body = (cpBody*)bodies->arr[i];
|
||||
|
||||
cpAssertSoft(body->sleeping.next == NULL, "Internal Error: Dangling next pointer detected in contact graph.");
|
||||
cpAssertSoft(body->sleeping.root == NULL, "Internal Error: Dangling root pointer detected in contact graph.");
|
||||
}
|
||||
#endif
|
||||
|
||||
// Calculate the kinetic energy of all the bodies.
|
||||
if(sleep){
|
||||
cpFloat dv = space->idleSpeedThreshold;
|
||||
cpFloat dvsq = (dv ? dv*dv : cpvlengthsq(space->gravity)*dt*dt);
|
||||
|
||||
// update idling and reset component nodes
|
||||
for(int i=0; i<bodies->num; i++){
|
||||
cpBody *body = (cpBody*)bodies->arr[i];
|
||||
|
||||
// TODO should make a separate array for kinematic bodies.
|
||||
if(cpBodyGetType(body) != CP_BODY_TYPE_DYNAMIC) continue;
|
||||
|
||||
// Need to deal with infinite mass objects
|
||||
cpFloat keThreshold = (dvsq ? body->m*dvsq : 0.0f);
|
||||
body->sleeping.idleTime = (cpBodyKineticEnergy(body) > keThreshold ? 0.0f : body->sleeping.idleTime + dt);
|
||||
}
|
||||
}
|
||||
|
||||
// Awaken any sleeping bodies found and then push arbiters to the bodies' lists.
|
||||
cpArray *arbiters = space->arbiters;
|
||||
for(int i=0, count=arbiters->num; i<count; i++){
|
||||
cpArbiter *arb = (cpArbiter*)arbiters->arr[i];
|
||||
cpBody *a = arb->body_a, *b = arb->body_b;
|
||||
|
||||
if(sleep){
|
||||
// TODO checking cpBodyIsSleepin() redundant?
|
||||
if(cpBodyGetType(b) == CP_BODY_TYPE_KINEMATIC || cpBodyIsSleeping(a)) cpBodyActivate(a);
|
||||
if(cpBodyGetType(a) == CP_BODY_TYPE_KINEMATIC || cpBodyIsSleeping(b)) cpBodyActivate(b);
|
||||
}
|
||||
|
||||
cpBodyPushArbiter(a, arb);
|
||||
cpBodyPushArbiter(b, arb);
|
||||
}
|
||||
|
||||
if(sleep){
|
||||
// Bodies should be held active if connected by a joint to a kinematic.
|
||||
cpArray *constraints = space->constraints;
|
||||
for(int i=0; i<constraints->num; i++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
|
||||
cpBody *a = constraint->a, *b = constraint->b;
|
||||
|
||||
if(cpBodyGetType(b) == CP_BODY_TYPE_KINEMATIC) cpBodyActivate(a);
|
||||
if(cpBodyGetType(a) == CP_BODY_TYPE_KINEMATIC) cpBodyActivate(b);
|
||||
}
|
||||
|
||||
// Generate components and deactivate sleeping ones
|
||||
for(int i=0; i<bodies->num;){
|
||||
cpBody *body = (cpBody*)bodies->arr[i];
|
||||
|
||||
if(ComponentRoot(body) == NULL){
|
||||
// Body not in a component yet. Perform a DFS to flood fill mark
|
||||
// the component in the contact graph using this body as the root.
|
||||
FloodFillComponent(body, body);
|
||||
|
||||
// Check if the component should be put to sleep.
|
||||
if(!ComponentActive(body, space->sleepTimeThreshold)){
|
||||
cpArrayPush(space->sleepingComponents, body);
|
||||
CP_BODY_FOREACH_COMPONENT(body, other) cpSpaceDeactivateBody(space, other);
|
||||
|
||||
// cpSpaceDeactivateBody() removed the current body from the list.
|
||||
// Skip incrementing the index counter.
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
i++;
|
||||
|
||||
// Only sleeping bodies retain their component node pointers.
|
||||
body->sleeping.root = NULL;
|
||||
body->sleeping.next = NULL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySleep(cpBody *body)
|
||||
{
|
||||
cpBodySleepWithGroup(body, NULL);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySleepWithGroup(cpBody *body, cpBody *group){
|
||||
cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Non-dynamic bodies cannot be put to sleep.");
|
||||
|
||||
cpSpace *space = body->space;
|
||||
cpAssertHard(!cpSpaceIsLocked(space), "Bodies cannot be put to sleep during a query or a call to cpSpaceStep(). Put these calls into a post-step callback.");
|
||||
cpAssertHard(cpSpaceGetSleepTimeThreshold(space) < INFINITY, "Sleeping is not enabled on the space. You cannot sleep a body without setting a sleep time threshold on the space.");
|
||||
cpAssertHard(group == NULL || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier.");
|
||||
|
||||
if(cpBodyIsSleeping(body)){
|
||||
cpAssertHard(ComponentRoot(body) == ComponentRoot(group), "The body is already sleeping and it's group cannot be reassigned.");
|
||||
return;
|
||||
}
|
||||
|
||||
CP_BODY_FOREACH_SHAPE(body, shape) cpShapeCacheBB(shape);
|
||||
cpSpaceDeactivateBody(space, body);
|
||||
|
||||
if(group){
|
||||
cpBody *root = ComponentRoot(group);
|
||||
|
||||
body->sleeping.root = root;
|
||||
body->sleeping.next = root->sleeping.next;
|
||||
body->sleeping.idleTime = 0.0f;
|
||||
|
||||
root->sleeping.next = body;
|
||||
} else {
|
||||
body->sleeping.root = body;
|
||||
body->sleeping.next = NULL;
|
||||
body->sleeping.idleTime = 0.0f;
|
||||
|
||||
cpArrayPush(space->sleepingComponents, body);
|
||||
}
|
||||
|
||||
cpArrayDeleteObj(space->dynamicBodies, body);
|
||||
}
|
||||
|
|
@ -0,0 +1,189 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
#ifndef CP_SPACE_DISABLE_DEBUG_API
|
||||
|
||||
static void
|
||||
cpSpaceDebugDrawShape(cpShape *shape, cpSpaceDebugDrawOptions *options)
|
||||
{
|
||||
cpBody *body = shape->body;
|
||||
cpDataPointer data = options->data;
|
||||
|
||||
cpSpaceDebugColor outline_color = options->shapeOutlineColor;
|
||||
cpSpaceDebugColor fill_color = options->colorForShape(shape, data);
|
||||
|
||||
switch(shape->klass->type){
|
||||
case CP_CIRCLE_SHAPE: {
|
||||
cpCircleShape *circle = (cpCircleShape *)shape;
|
||||
options->drawCircle(circle->tc, body->a, circle->r, outline_color, fill_color, data);
|
||||
break;
|
||||
}
|
||||
case CP_SEGMENT_SHAPE: {
|
||||
cpSegmentShape *seg = (cpSegmentShape *)shape;
|
||||
options->drawFatSegment(seg->ta, seg->tb, seg->r, outline_color, fill_color, data);
|
||||
break;
|
||||
}
|
||||
case CP_POLY_SHAPE: {
|
||||
cpPolyShape *poly = (cpPolyShape *)shape;
|
||||
|
||||
int count = poly->count;
|
||||
struct cpSplittingPlane *planes = poly->planes;
|
||||
cpVect *verts = (cpVect *)alloca(count*sizeof(cpVect));
|
||||
|
||||
for(int i=0; i<count; i++) verts[i] = planes[i].v0;
|
||||
options->drawPolygon(count, verts, poly->r, outline_color, fill_color, data);
|
||||
break;
|
||||
}
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
|
||||
static const cpVect spring_verts[] = {
|
||||
{0.00f, 0.0f},
|
||||
{0.20f, 0.0f},
|
||||
{0.25f, 3.0f},
|
||||
{0.30f,-6.0f},
|
||||
{0.35f, 6.0f},
|
||||
{0.40f,-6.0f},
|
||||
{0.45f, 6.0f},
|
||||
{0.50f,-6.0f},
|
||||
{0.55f, 6.0f},
|
||||
{0.60f,-6.0f},
|
||||
{0.65f, 6.0f},
|
||||
{0.70f,-3.0f},
|
||||
{0.75f, 6.0f},
|
||||
{0.80f, 0.0f},
|
||||
{1.00f, 0.0f},
|
||||
};
|
||||
static const int spring_count = sizeof(spring_verts)/sizeof(cpVect);
|
||||
|
||||
static void
|
||||
cpSpaceDebugDrawConstraint(cpConstraint *constraint, cpSpaceDebugDrawOptions *options)
|
||||
{
|
||||
cpDataPointer data = options->data;
|
||||
cpSpaceDebugColor color = options->constraintColor;
|
||||
|
||||
cpBody *body_a = constraint->a;
|
||||
cpBody *body_b = constraint->b;
|
||||
|
||||
if(cpConstraintIsPinJoint(constraint)){
|
||||
cpPinJoint *joint = (cpPinJoint *)constraint;
|
||||
|
||||
cpVect a = cpTransformPoint(body_a->transform, joint->anchorA);
|
||||
cpVect b = cpTransformPoint(body_b->transform, joint->anchorB);
|
||||
|
||||
options->drawDot(5, a, color, data);
|
||||
options->drawDot(5, b, color, data);
|
||||
options->drawSegment(a, b, color, data);
|
||||
} else if(cpConstraintIsSlideJoint(constraint)){
|
||||
cpSlideJoint *joint = (cpSlideJoint *)constraint;
|
||||
|
||||
cpVect a = cpTransformPoint(body_a->transform, joint->anchorA);
|
||||
cpVect b = cpTransformPoint(body_b->transform, joint->anchorB);
|
||||
|
||||
options->drawDot(5, a, color, data);
|
||||
options->drawDot(5, b, color, data);
|
||||
options->drawSegment(a, b, color, data);
|
||||
} else if(cpConstraintIsPivotJoint(constraint)){
|
||||
cpPivotJoint *joint = (cpPivotJoint *)constraint;
|
||||
|
||||
cpVect a = cpTransformPoint(body_a->transform, joint->anchorA);
|
||||
cpVect b = cpTransformPoint(body_b->transform, joint->anchorB);
|
||||
|
||||
options->drawDot(5, a, color, data);
|
||||
options->drawDot(5, b, color, data);
|
||||
} else if(cpConstraintIsGrooveJoint(constraint)){
|
||||
cpGrooveJoint *joint = (cpGrooveJoint *)constraint;
|
||||
|
||||
cpVect a = cpTransformPoint(body_a->transform, joint->grv_a);
|
||||
cpVect b = cpTransformPoint(body_a->transform, joint->grv_b);
|
||||
cpVect c = cpTransformPoint(body_b->transform, joint->anchorB);
|
||||
|
||||
options->drawDot(5, c, color, data);
|
||||
options->drawSegment(a, b, color, data);
|
||||
} else if(cpConstraintIsDampedSpring(constraint)){
|
||||
cpDampedSpring *spring = (cpDampedSpring *)constraint;
|
||||
cpDataPointer data = options->data;
|
||||
cpSpaceDebugColor color = options->constraintColor;
|
||||
|
||||
cpVect a = cpTransformPoint(body_a->transform, spring->anchorA);
|
||||
cpVect b = cpTransformPoint(body_b->transform, spring->anchorB);
|
||||
|
||||
options->drawDot(5, a, color, data);
|
||||
options->drawDot(5, b, color, data);
|
||||
|
||||
cpVect delta = cpvsub(b, a);
|
||||
cpFloat cos = delta.x;
|
||||
cpFloat sin = delta.y;
|
||||
cpFloat s = 1.0f/cpvlength(delta);
|
||||
|
||||
cpVect r1 = cpv(cos, -sin*s);
|
||||
cpVect r2 = cpv(sin, cos*s);
|
||||
|
||||
cpVect *verts = (cpVect *)alloca(spring_count*sizeof(cpVect));
|
||||
for(int i=0; i<spring_count; i++){
|
||||
cpVect v = spring_verts[i];
|
||||
verts[i] = cpv(cpvdot(v, r1) + a.x, cpvdot(v, r2) + a.y);
|
||||
}
|
||||
|
||||
for(int i=0; i<spring_count-1; i++){
|
||||
options->drawSegment(verts[i], verts[i + 1], color, data);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceDebugDraw(cpSpace *space, cpSpaceDebugDrawOptions *options)
|
||||
{
|
||||
if(options->flags & CP_SPACE_DEBUG_DRAW_SHAPES){
|
||||
cpSpaceEachShape(space, (cpSpaceShapeIteratorFunc)cpSpaceDebugDrawShape, options);
|
||||
}
|
||||
|
||||
if(options->flags & CP_SPACE_DEBUG_DRAW_CONSTRAINTS){
|
||||
cpSpaceEachConstraint(space, (cpSpaceConstraintIteratorFunc)cpSpaceDebugDrawConstraint, options);
|
||||
}
|
||||
|
||||
if(options->flags & CP_SPACE_DEBUG_DRAW_COLLISION_POINTS){
|
||||
cpArray *arbiters = space->arbiters;
|
||||
cpSpaceDebugColor color = options->collisionPointColor;
|
||||
cpSpaceDebugDrawSegmentImpl draw_seg = options->drawSegment;
|
||||
cpDataPointer data = options->data;
|
||||
|
||||
for(int i=0; i<arbiters->num; i++){
|
||||
cpArbiter *arb = (cpArbiter*)arbiters->arr[i];
|
||||
cpVect n = arb->n;
|
||||
|
||||
for(int j=0; j<arb->count; j++){
|
||||
cpVect p1 = cpvadd(arb->body_a->p, arb->contacts[j].r1);
|
||||
cpVect p2 = cpvadd(arb->body_b->p, arb->contacts[j].r2);
|
||||
|
||||
cpFloat d = 2.0f;
|
||||
cpVect a = cpvadd(p1, cpvmult(n, -d));
|
||||
cpVect b = cpvadd(p2, cpvmult(n, d));
|
||||
draw_seg(a, b, color, data);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,634 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
#include "prime.h"
|
||||
|
||||
typedef struct cpSpaceHashBin cpSpaceHashBin;
|
||||
typedef struct cpHandle cpHandle;
|
||||
|
||||
struct cpSpaceHash {
|
||||
cpSpatialIndex spatialIndex;
|
||||
|
||||
int numcells;
|
||||
cpFloat celldim;
|
||||
|
||||
cpSpaceHashBin **table;
|
||||
cpHashSet *handleSet;
|
||||
|
||||
cpSpaceHashBin *pooledBins;
|
||||
cpArray *pooledHandles;
|
||||
cpArray *allocatedBuffers;
|
||||
|
||||
cpTimestamp stamp;
|
||||
};
|
||||
|
||||
|
||||
//MARK: Handle Functions
|
||||
|
||||
struct cpHandle {
|
||||
void *obj;
|
||||
int retain;
|
||||
cpTimestamp stamp;
|
||||
};
|
||||
|
||||
static cpHandle*
|
||||
cpHandleInit(cpHandle *hand, void *obj)
|
||||
{
|
||||
hand->obj = obj;
|
||||
hand->retain = 0;
|
||||
hand->stamp = 0;
|
||||
|
||||
return hand;
|
||||
}
|
||||
|
||||
static inline void cpHandleRetain(cpHandle *hand){hand->retain++;}
|
||||
|
||||
static inline void
|
||||
cpHandleRelease(cpHandle *hand, cpArray *pooledHandles)
|
||||
{
|
||||
hand->retain--;
|
||||
if(hand->retain == 0) cpArrayPush(pooledHandles, hand);
|
||||
}
|
||||
|
||||
static int handleSetEql(void *obj, cpHandle *hand){return (obj == hand->obj);}
|
||||
|
||||
static void *
|
||||
handleSetTrans(void *obj, cpSpaceHash *hash)
|
||||
{
|
||||
if(hash->pooledHandles->num == 0){
|
||||
// handle pool is exhausted, make more
|
||||
int count = CP_BUFFER_BYTES/sizeof(cpHandle);
|
||||
cpAssertHard(count, "Internal Error: Buffer size is too small.");
|
||||
|
||||
cpHandle *buffer = (cpHandle *)cpcalloc(1, CP_BUFFER_BYTES);
|
||||
cpArrayPush(hash->allocatedBuffers, buffer);
|
||||
|
||||
for(int i=0; i<count; i++) cpArrayPush(hash->pooledHandles, buffer + i);
|
||||
}
|
||||
|
||||
cpHandle *hand = cpHandleInit((cpHandle *)cpArrayPop(hash->pooledHandles), obj);
|
||||
cpHandleRetain(hand);
|
||||
|
||||
return hand;
|
||||
}
|
||||
|
||||
//MARK: Bin Functions
|
||||
|
||||
struct cpSpaceHashBin {
|
||||
cpHandle *handle;
|
||||
cpSpaceHashBin *next;
|
||||
};
|
||||
|
||||
static inline void
|
||||
recycleBin(cpSpaceHash *hash, cpSpaceHashBin *bin)
|
||||
{
|
||||
bin->next = hash->pooledBins;
|
||||
hash->pooledBins = bin;
|
||||
}
|
||||
|
||||
static inline void
|
||||
clearTableCell(cpSpaceHash *hash, int idx)
|
||||
{
|
||||
cpSpaceHashBin *bin = hash->table[idx];
|
||||
while(bin){
|
||||
cpSpaceHashBin *next = bin->next;
|
||||
|
||||
cpHandleRelease(bin->handle, hash->pooledHandles);
|
||||
recycleBin(hash, bin);
|
||||
|
||||
bin = next;
|
||||
}
|
||||
|
||||
hash->table[idx] = NULL;
|
||||
}
|
||||
|
||||
static void
|
||||
clearTable(cpSpaceHash *hash)
|
||||
{
|
||||
for(int i=0; i<hash->numcells; i++) clearTableCell(hash, i);
|
||||
}
|
||||
|
||||
// Get a recycled or new bin.
|
||||
static inline cpSpaceHashBin *
|
||||
getEmptyBin(cpSpaceHash *hash)
|
||||
{
|
||||
cpSpaceHashBin *bin = hash->pooledBins;
|
||||
|
||||
if(bin){
|
||||
hash->pooledBins = bin->next;
|
||||
return bin;
|
||||
} else {
|
||||
// Pool is exhausted, make more
|
||||
int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin);
|
||||
cpAssertHard(count, "Internal Error: Buffer size is too small.");
|
||||
|
||||
cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpcalloc(1, CP_BUFFER_BYTES);
|
||||
cpArrayPush(hash->allocatedBuffers, buffer);
|
||||
|
||||
// push all but the first one, return the first instead
|
||||
for(int i=1; i<count; i++) recycleBin(hash, buffer + i);
|
||||
return buffer;
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Memory Management Functions
|
||||
|
||||
cpSpaceHash *
|
||||
cpSpaceHashAlloc(void)
|
||||
{
|
||||
return (cpSpaceHash *)cpcalloc(1, sizeof(cpSpaceHash));
|
||||
}
|
||||
|
||||
// Frees the old table, and allocate a new one.
|
||||
static void
|
||||
cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells)
|
||||
{
|
||||
cpfree(hash->table);
|
||||
|
||||
hash->numcells = numcells;
|
||||
hash->table = (cpSpaceHashBin **)cpcalloc(numcells, sizeof(cpSpaceHashBin *));
|
||||
}
|
||||
|
||||
static inline cpSpatialIndexClass *Klass();
|
||||
|
||||
cpSpatialIndex *
|
||||
cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
|
||||
{
|
||||
cpSpatialIndexInit((cpSpatialIndex *)hash, Klass(), bbfunc, staticIndex);
|
||||
|
||||
cpSpaceHashAllocTable(hash, next_prime(numcells));
|
||||
hash->celldim = celldim;
|
||||
|
||||
hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql);
|
||||
|
||||
hash->pooledHandles = cpArrayNew(0);
|
||||
|
||||
hash->pooledBins = NULL;
|
||||
hash->allocatedBuffers = cpArrayNew(0);
|
||||
|
||||
hash->stamp = 1;
|
||||
|
||||
return (cpSpatialIndex *)hash;
|
||||
}
|
||||
|
||||
cpSpatialIndex *
|
||||
cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
|
||||
{
|
||||
return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc, staticIndex);
|
||||
}
|
||||
|
||||
static void
|
||||
cpSpaceHashDestroy(cpSpaceHash *hash)
|
||||
{
|
||||
if(hash->table) clearTable(hash);
|
||||
cpfree(hash->table);
|
||||
|
||||
cpHashSetFree(hash->handleSet);
|
||||
|
||||
cpArrayFreeEach(hash->allocatedBuffers, cpfree);
|
||||
cpArrayFree(hash->allocatedBuffers);
|
||||
cpArrayFree(hash->pooledHandles);
|
||||
}
|
||||
|
||||
//MARK: Helper Functions
|
||||
|
||||
static inline cpBool
|
||||
containsHandle(cpSpaceHashBin *bin, cpHandle *hand)
|
||||
{
|
||||
while(bin){
|
||||
if(bin->handle == hand) return cpTrue;
|
||||
bin = bin->next;
|
||||
}
|
||||
|
||||
return cpFalse;
|
||||
}
|
||||
|
||||
// The hash function itself.
|
||||
static inline cpHashValue
|
||||
hash_func(cpHashValue x, cpHashValue y, cpHashValue n)
|
||||
{
|
||||
return (x*1640531513ul ^ y*2654435789ul) % n;
|
||||
}
|
||||
|
||||
// Much faster than (int)floor(f)
|
||||
// Profiling showed floor() to be a sizable performance hog
|
||||
static inline int
|
||||
floor_int(cpFloat f)
|
||||
{
|
||||
int i = (int)f;
|
||||
return (f < 0.0f && f != i ? i - 1 : i);
|
||||
}
|
||||
|
||||
static inline void
|
||||
hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb)
|
||||
{
|
||||
// Find the dimensions in cell coordinates.
|
||||
cpFloat dim = hash->celldim;
|
||||
int l = floor_int(bb.l/dim); // Fix by ShiftZ
|
||||
int r = floor_int(bb.r/dim);
|
||||
int b = floor_int(bb.b/dim);
|
||||
int t = floor_int(bb.t/dim);
|
||||
|
||||
int n = hash->numcells;
|
||||
for(int i=l; i<=r; i++){
|
||||
for(int j=b; j<=t; j++){
|
||||
cpHashValue idx = hash_func(i,j,n);
|
||||
cpSpaceHashBin *bin = hash->table[idx];
|
||||
|
||||
// Don't add an object twice to the same cell.
|
||||
if(containsHandle(bin, hand)) continue;
|
||||
|
||||
cpHandleRetain(hand);
|
||||
// Insert a new bin for the handle in this cell.
|
||||
cpSpaceHashBin *newBin = getEmptyBin(hash);
|
||||
newBin->handle = hand;
|
||||
newBin->next = bin;
|
||||
hash->table[idx] = newBin;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Basic Operations
|
||||
|
||||
static void
|
||||
cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid)
|
||||
{
|
||||
cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, hashid, obj, (cpHashSetTransFunc)handleSetTrans, hash);
|
||||
hashHandle(hash, hand, hash->spatialIndex.bbfunc(obj));
|
||||
}
|
||||
|
||||
static void
|
||||
cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid)
|
||||
{
|
||||
cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj);
|
||||
|
||||
if(hand){
|
||||
hand->obj = NULL;
|
||||
cpHandleRelease(hand, hash->pooledHandles);
|
||||
|
||||
cpSpaceHashInsert(hash, obj, hashid);
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
rehash_helper(cpHandle *hand, cpSpaceHash *hash)
|
||||
{
|
||||
hashHandle(hash, hand, hash->spatialIndex.bbfunc(hand->obj));
|
||||
}
|
||||
|
||||
static void
|
||||
cpSpaceHashRehash(cpSpaceHash *hash)
|
||||
{
|
||||
clearTable(hash);
|
||||
cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)rehash_helper, hash);
|
||||
}
|
||||
|
||||
static void
|
||||
cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid)
|
||||
{
|
||||
cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj);
|
||||
|
||||
if(hand){
|
||||
hand->obj = NULL;
|
||||
cpHandleRelease(hand, hash->pooledHandles);
|
||||
}
|
||||
}
|
||||
|
||||
typedef struct eachContext {
|
||||
cpSpatialIndexIteratorFunc func;
|
||||
void *data;
|
||||
} eachContext;
|
||||
|
||||
static void eachHelper(cpHandle *hand, eachContext *context){context->func(hand->obj, context->data);}
|
||||
|
||||
static void
|
||||
cpSpaceHashEach(cpSpaceHash *hash, cpSpatialIndexIteratorFunc func, void *data)
|
||||
{
|
||||
eachContext context = {func, data};
|
||||
cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)eachHelper, &context);
|
||||
}
|
||||
|
||||
static void
|
||||
remove_orphaned_handles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr)
|
||||
{
|
||||
cpSpaceHashBin *bin = *bin_ptr;
|
||||
while(bin){
|
||||
cpHandle *hand = bin->handle;
|
||||
cpSpaceHashBin *next = bin->next;
|
||||
|
||||
if(!hand->obj){
|
||||
// orphaned handle, unlink and recycle the bin
|
||||
(*bin_ptr) = bin->next;
|
||||
recycleBin(hash, bin);
|
||||
|
||||
cpHandleRelease(hand, hash->pooledHandles);
|
||||
} else {
|
||||
bin_ptr = &bin->next;
|
||||
}
|
||||
|
||||
bin = next;
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Query Functions
|
||||
|
||||
static inline void
|
||||
query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexQueryFunc func, void *data)
|
||||
{
|
||||
restart:
|
||||
for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){
|
||||
cpHandle *hand = bin->handle;
|
||||
void *other = hand->obj;
|
||||
|
||||
if(hand->stamp == hash->stamp || obj == other){
|
||||
continue;
|
||||
} else if(other){
|
||||
func(obj, other, 0, data);
|
||||
hand->stamp = hash->stamp;
|
||||
} else {
|
||||
// The object for this handle has been removed
|
||||
// cleanup this cell and restart the query
|
||||
remove_orphaned_handles(hash, bin_ptr);
|
||||
goto restart; // GCC not smart enough/able to tail call an inlined function.
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data)
|
||||
{
|
||||
// Get the dimensions in cell coordinates.
|
||||
cpFloat dim = hash->celldim;
|
||||
int l = floor_int(bb.l/dim); // Fix by ShiftZ
|
||||
int r = floor_int(bb.r/dim);
|
||||
int b = floor_int(bb.b/dim);
|
||||
int t = floor_int(bb.t/dim);
|
||||
|
||||
int n = hash->numcells;
|
||||
cpSpaceHashBin **table = hash->table;
|
||||
|
||||
// Iterate over the cells and query them.
|
||||
for(int i=l; i<=r; i++){
|
||||
for(int j=b; j<=t; j++){
|
||||
query_helper(hash, &table[hash_func(i,j,n)], obj, func, data);
|
||||
}
|
||||
}
|
||||
|
||||
hash->stamp++;
|
||||
}
|
||||
|
||||
// Similar to struct eachPair above.
|
||||
typedef struct queryRehashContext {
|
||||
cpSpaceHash *hash;
|
||||
cpSpatialIndexQueryFunc func;
|
||||
void *data;
|
||||
} queryRehashContext;
|
||||
|
||||
// Hashset iterator func used with cpSpaceHashQueryRehash().
|
||||
static void
|
||||
queryRehash_helper(cpHandle *hand, queryRehashContext *context)
|
||||
{
|
||||
cpSpaceHash *hash = context->hash;
|
||||
cpSpatialIndexQueryFunc func = context->func;
|
||||
void *data = context->data;
|
||||
|
||||
cpFloat dim = hash->celldim;
|
||||
int n = hash->numcells;
|
||||
|
||||
void *obj = hand->obj;
|
||||
cpBB bb = hash->spatialIndex.bbfunc(obj);
|
||||
|
||||
int l = floor_int(bb.l/dim);
|
||||
int r = floor_int(bb.r/dim);
|
||||
int b = floor_int(bb.b/dim);
|
||||
int t = floor_int(bb.t/dim);
|
||||
|
||||
cpSpaceHashBin **table = hash->table;
|
||||
|
||||
for(int i=l; i<=r; i++){
|
||||
for(int j=b; j<=t; j++){
|
||||
cpHashValue idx = hash_func(i,j,n);
|
||||
cpSpaceHashBin *bin = table[idx];
|
||||
|
||||
if(containsHandle(bin, hand)) continue;
|
||||
|
||||
cpHandleRetain(hand); // this MUST be done first in case the object is removed in func()
|
||||
query_helper(hash, &bin, obj, func, data);
|
||||
|
||||
cpSpaceHashBin *newBin = getEmptyBin(hash);
|
||||
newBin->handle = hand;
|
||||
newBin->next = bin;
|
||||
table[idx] = newBin;
|
||||
}
|
||||
}
|
||||
|
||||
// Increment the stamp for each object hashed.
|
||||
hash->stamp++;
|
||||
}
|
||||
|
||||
static void
|
||||
cpSpaceHashReindexQuery(cpSpaceHash *hash, cpSpatialIndexQueryFunc func, void *data)
|
||||
{
|
||||
clearTable(hash);
|
||||
|
||||
queryRehashContext context = {hash, func, data};
|
||||
cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)queryRehash_helper, &context);
|
||||
|
||||
cpSpatialIndexCollideStatic((cpSpatialIndex *)hash, hash->spatialIndex.staticIndex, func, data);
|
||||
}
|
||||
|
||||
static inline cpFloat
|
||||
segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexSegmentQueryFunc func, void *data)
|
||||
{
|
||||
cpFloat t = 1.0f;
|
||||
|
||||
restart:
|
||||
for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){
|
||||
cpHandle *hand = bin->handle;
|
||||
void *other = hand->obj;
|
||||
|
||||
// Skip over certain conditions
|
||||
if(hand->stamp == hash->stamp){
|
||||
continue;
|
||||
} else if(other){
|
||||
t = cpfmin(t, func(obj, other, data));
|
||||
hand->stamp = hash->stamp;
|
||||
} else {
|
||||
// The object for this handle has been removed
|
||||
// cleanup this cell and restart the query
|
||||
remove_orphaned_handles(hash, bin_ptr);
|
||||
goto restart; // GCC not smart enough/able to tail call an inlined function.
|
||||
}
|
||||
}
|
||||
|
||||
return t;
|
||||
}
|
||||
|
||||
// modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html
|
||||
static void
|
||||
cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
|
||||
{
|
||||
a = cpvmult(a, 1.0f/hash->celldim);
|
||||
b = cpvmult(b, 1.0f/hash->celldim);
|
||||
|
||||
int cell_x = floor_int(a.x), cell_y = floor_int(a.y);
|
||||
|
||||
cpFloat t = 0;
|
||||
|
||||
int x_inc, y_inc;
|
||||
cpFloat temp_v, temp_h;
|
||||
|
||||
if (b.x > a.x){
|
||||
x_inc = 1;
|
||||
temp_h = (cpffloor(a.x + 1.0f) - a.x);
|
||||
} else {
|
||||
x_inc = -1;
|
||||
temp_h = (a.x - cpffloor(a.x));
|
||||
}
|
||||
|
||||
if (b.y > a.y){
|
||||
y_inc = 1;
|
||||
temp_v = (cpffloor(a.y + 1.0f) - a.y);
|
||||
} else {
|
||||
y_inc = -1;
|
||||
temp_v = (a.y - cpffloor(a.y));
|
||||
}
|
||||
|
||||
// Division by zero is *very* slow on ARM
|
||||
cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y);
|
||||
cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY);
|
||||
|
||||
// fix NANs in horizontal directions
|
||||
cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx);
|
||||
cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy);
|
||||
|
||||
int n = hash->numcells;
|
||||
cpSpaceHashBin **table = hash->table;
|
||||
|
||||
while(t < t_exit){
|
||||
cpHashValue idx = hash_func(cell_x, cell_y, n);
|
||||
t_exit = cpfmin(t_exit, segmentQuery_helper(hash, &table[idx], obj, func, data));
|
||||
|
||||
if (next_v < next_h){
|
||||
cell_y += y_inc;
|
||||
t = next_v;
|
||||
next_v += dt_dy;
|
||||
} else {
|
||||
cell_x += x_inc;
|
||||
t = next_h;
|
||||
next_h += dt_dx;
|
||||
}
|
||||
}
|
||||
|
||||
hash->stamp++;
|
||||
}
|
||||
|
||||
//MARK: Misc
|
||||
|
||||
void
|
||||
cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells)
|
||||
{
|
||||
if(hash->spatialIndex.klass != Klass()){
|
||||
cpAssertWarn(cpFalse, "Ignoring cpSpaceHashResize() call to non-cpSpaceHash spatial index.");
|
||||
return;
|
||||
}
|
||||
|
||||
clearTable(hash);
|
||||
|
||||
hash->celldim = celldim;
|
||||
cpSpaceHashAllocTable(hash, next_prime(numcells));
|
||||
}
|
||||
|
||||
static int
|
||||
cpSpaceHashCount(cpSpaceHash *hash)
|
||||
{
|
||||
return cpHashSetCount(hash->handleSet);
|
||||
}
|
||||
|
||||
static int
|
||||
cpSpaceHashContains(cpSpaceHash *hash, void *obj, cpHashValue hashid)
|
||||
{
|
||||
return cpHashSetFind(hash->handleSet, hashid, obj) != NULL;
|
||||
}
|
||||
|
||||
static cpSpatialIndexClass klass = {
|
||||
(cpSpatialIndexDestroyImpl)cpSpaceHashDestroy,
|
||||
|
||||
(cpSpatialIndexCountImpl)cpSpaceHashCount,
|
||||
(cpSpatialIndexEachImpl)cpSpaceHashEach,
|
||||
(cpSpatialIndexContainsImpl)cpSpaceHashContains,
|
||||
|
||||
(cpSpatialIndexInsertImpl)cpSpaceHashInsert,
|
||||
(cpSpatialIndexRemoveImpl)cpSpaceHashRemove,
|
||||
|
||||
(cpSpatialIndexReindexImpl)cpSpaceHashRehash,
|
||||
(cpSpatialIndexReindexObjectImpl)cpSpaceHashRehashObject,
|
||||
(cpSpatialIndexReindexQueryImpl)cpSpaceHashReindexQuery,
|
||||
|
||||
(cpSpatialIndexQueryImpl)cpSpaceHashQuery,
|
||||
(cpSpatialIndexSegmentQueryImpl)cpSpaceHashSegmentQuery,
|
||||
};
|
||||
|
||||
static inline cpSpatialIndexClass *Klass(){return &klass;}
|
||||
|
||||
//MARK: Debug Drawing
|
||||
|
||||
//#define CP_BBTREE_DEBUG_DRAW
|
||||
#ifdef CP_BBTREE_DEBUG_DRAW
|
||||
#include "OpenGL/gl.h"
|
||||
#include "OpenGL/glu.h"
|
||||
#include <GLUT/glut.h>
|
||||
|
||||
void
|
||||
cpSpaceHashRenderDebug(cpSpatialIndex *index)
|
||||
{
|
||||
if(index->klass != &klass){
|
||||
cpAssertWarn(cpFalse, "Ignoring cpSpaceHashRenderDebug() call to non-spatial hash spatial index.");
|
||||
return;
|
||||
}
|
||||
|
||||
cpSpaceHash *hash = (cpSpaceHash *)index;
|
||||
cpBB bb = cpBBNew(-320, -240, 320, 240);
|
||||
|
||||
cpFloat dim = hash->celldim;
|
||||
int n = hash->numcells;
|
||||
|
||||
int l = (int)floor(bb.l/dim);
|
||||
int r = (int)floor(bb.r/dim);
|
||||
int b = (int)floor(bb.b/dim);
|
||||
int t = (int)floor(bb.t/dim);
|
||||
|
||||
for(int i=l; i<=r; i++){
|
||||
for(int j=b; j<=t; j++){
|
||||
int cell_count = 0;
|
||||
|
||||
int index = hash_func(i,j,n);
|
||||
for(cpSpaceHashBin *bin = hash->table[index]; bin; bin = bin->next)
|
||||
cell_count++;
|
||||
|
||||
GLfloat v = 1.0f - (GLfloat)cell_count/10.0f;
|
||||
glColor3f(v,v,v);
|
||||
glRectf(i*dim, j*dim, (i + 1)*dim, (j + 1)*dim);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,246 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
//MARK: Nearest Point Query Functions
|
||||
|
||||
struct PointQueryContext {
|
||||
cpVect point;
|
||||
cpFloat maxDistance;
|
||||
cpShapeFilter filter;
|
||||
cpSpacePointQueryFunc func;
|
||||
};
|
||||
|
||||
static cpCollisionID
|
||||
NearestPointQuery(struct PointQueryContext *context, cpShape *shape, cpCollisionID id, void *data)
|
||||
{
|
||||
if(
|
||||
!cpShapeFilterReject(shape->filter, context->filter)
|
||||
){
|
||||
cpPointQueryInfo info;
|
||||
cpShapePointQuery(shape, context->point, &info);
|
||||
|
||||
if(info.shape && info.distance < context->maxDistance) context->func(shape, info.point, info.distance, info.gradient, data);
|
||||
}
|
||||
|
||||
return id;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpacePointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryFunc func, void *data)
|
||||
{
|
||||
struct PointQueryContext context = {point, maxDistance, filter, func};
|
||||
cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f));
|
||||
|
||||
cpSpaceLock(space); {
|
||||
cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data);
|
||||
cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data);
|
||||
} cpSpaceUnlock(space, cpTrue);
|
||||
}
|
||||
|
||||
static cpCollisionID
|
||||
NearestPointQueryNearest(struct PointQueryContext *context, cpShape *shape, cpCollisionID id, cpPointQueryInfo *out)
|
||||
{
|
||||
if(
|
||||
!cpShapeFilterReject(shape->filter, context->filter) && !shape->sensor
|
||||
){
|
||||
cpPointQueryInfo info;
|
||||
cpShapePointQuery(shape, context->point, &info);
|
||||
|
||||
if(info.distance < out->distance) (*out) = info;
|
||||
}
|
||||
|
||||
return id;
|
||||
}
|
||||
|
||||
cpShape *
|
||||
cpSpacePointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpPointQueryInfo *out)
|
||||
{
|
||||
cpPointQueryInfo info = {NULL, cpvzero, maxDistance, cpvzero};
|
||||
if(out){
|
||||
(*out) = info;
|
||||
} else {
|
||||
out = &info;
|
||||
}
|
||||
|
||||
struct PointQueryContext context = {
|
||||
point, maxDistance,
|
||||
filter,
|
||||
NULL
|
||||
};
|
||||
|
||||
cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f));
|
||||
cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out);
|
||||
cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out);
|
||||
|
||||
return (cpShape *)out->shape;
|
||||
}
|
||||
|
||||
|
||||
//MARK: Segment Query Functions
|
||||
|
||||
struct SegmentQueryContext {
|
||||
cpVect start, end;
|
||||
cpFloat radius;
|
||||
cpShapeFilter filter;
|
||||
cpSpaceSegmentQueryFunc func;
|
||||
};
|
||||
|
||||
static cpFloat
|
||||
SegmentQuery(struct SegmentQueryContext *context, cpShape *shape, void *data)
|
||||
{
|
||||
cpSegmentQueryInfo info;
|
||||
|
||||
if(
|
||||
!cpShapeFilterReject(shape->filter, context->filter) &&
|
||||
cpShapeSegmentQuery(shape, context->start, context->end, context->radius, &info)
|
||||
){
|
||||
context->func(shape, info.point, info.normal, info.alpha, data);
|
||||
}
|
||||
|
||||
return 1.0f;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryFunc func, void *data)
|
||||
{
|
||||
struct SegmentQueryContext context = {
|
||||
start, end,
|
||||
radius,
|
||||
filter,
|
||||
func,
|
||||
};
|
||||
|
||||
cpSpaceLock(space); {
|
||||
cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data);
|
||||
cpSpatialIndexSegmentQuery(space->dynamicShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data);
|
||||
} cpSpaceUnlock(space, cpTrue);
|
||||
}
|
||||
|
||||
static cpFloat
|
||||
SegmentQueryFirst(struct SegmentQueryContext *context, cpShape *shape, cpSegmentQueryInfo *out)
|
||||
{
|
||||
cpSegmentQueryInfo info;
|
||||
|
||||
if(
|
||||
!cpShapeFilterReject(shape->filter, context->filter) && !shape->sensor &&
|
||||
cpShapeSegmentQuery(shape, context->start, context->end, context->radius, &info) &&
|
||||
info.alpha < out->alpha
|
||||
){
|
||||
(*out) = info;
|
||||
}
|
||||
|
||||
return out->alpha;
|
||||
}
|
||||
|
||||
cpShape *
|
||||
cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSegmentQueryInfo *out)
|
||||
{
|
||||
cpSegmentQueryInfo info = {NULL, end, cpvzero, 1.0f};
|
||||
if(out){
|
||||
(*out) = info;
|
||||
} else {
|
||||
out = &info;
|
||||
}
|
||||
|
||||
struct SegmentQueryContext context = {
|
||||
start, end,
|
||||
radius,
|
||||
filter,
|
||||
NULL
|
||||
};
|
||||
|
||||
cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out);
|
||||
cpSpatialIndexSegmentQuery(space->dynamicShapes, &context, start, end, out->alpha, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out);
|
||||
|
||||
return (cpShape *)out->shape;
|
||||
}
|
||||
|
||||
//MARK: BB Query Functions
|
||||
|
||||
struct BBQueryContext {
|
||||
cpBB bb;
|
||||
cpShapeFilter filter;
|
||||
cpSpaceBBQueryFunc func;
|
||||
};
|
||||
|
||||
static cpCollisionID
|
||||
BBQuery(struct BBQueryContext *context, cpShape *shape, cpCollisionID id, void *data)
|
||||
{
|
||||
if(
|
||||
!cpShapeFilterReject(shape->filter, context->filter) &&
|
||||
cpBBIntersects(context->bb, shape->bb)
|
||||
){
|
||||
context->func(shape, data);
|
||||
}
|
||||
|
||||
return id;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceBBQuery(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryFunc func, void *data)
|
||||
{
|
||||
struct BBQueryContext context = {bb, filter, func};
|
||||
|
||||
cpSpaceLock(space); {
|
||||
cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data);
|
||||
cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data);
|
||||
} cpSpaceUnlock(space, cpTrue);
|
||||
}
|
||||
|
||||
//MARK: Shape Query Functions
|
||||
|
||||
struct ShapeQueryContext {
|
||||
cpSpaceShapeQueryFunc func;
|
||||
void *data;
|
||||
cpBool anyCollision;
|
||||
};
|
||||
|
||||
// Callback from the spatial hash.
|
||||
static cpCollisionID
|
||||
ShapeQuery(cpShape *a, cpShape *b, cpCollisionID id, struct ShapeQueryContext *context)
|
||||
{
|
||||
if(cpShapeFilterReject(a->filter, b->filter) || a == b) return id;
|
||||
|
||||
cpContactPointSet set = cpShapesCollide(a, b);
|
||||
if(set.count){
|
||||
if(context->func) context->func(b, &set, context->data);
|
||||
context->anyCollision = !(a->sensor || b->sensor);
|
||||
}
|
||||
|
||||
return id;
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data)
|
||||
{
|
||||
cpBody *body = shape->body;
|
||||
cpBB bb = (body ? cpShapeUpdate(shape, body->transform) : shape->bb);
|
||||
struct ShapeQueryContext context = {func, data, cpFalse};
|
||||
|
||||
cpSpaceLock(space); {
|
||||
cpSpatialIndexQuery(space->dynamicShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context);
|
||||
cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context);
|
||||
} cpSpaceUnlock(space, cpTrue);
|
||||
|
||||
return context.anyCollision;
|
||||
}
|
||||
|
|
@ -0,0 +1,445 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
//MARK: Post Step Callback Functions
|
||||
|
||||
cpPostStepCallback *
|
||||
cpSpaceGetPostStepCallback(cpSpace *space, void *key)
|
||||
{
|
||||
cpArray *arr = space->postStepCallbacks;
|
||||
for(int i=0; i<arr->num; i++){
|
||||
cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i];
|
||||
if(callback && callback->key == key) return callback;
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static void PostStepDoNothing(cpSpace *space, void *obj, void *data){}
|
||||
|
||||
cpBool
|
||||
cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data)
|
||||
{
|
||||
cpAssertWarn(space->locked,
|
||||
"Adding a post-step callback when the space is not locked is unnecessary. "
|
||||
"Post-step callbacks will not called until the end of the next call to cpSpaceStep() or the next query.");
|
||||
|
||||
if(!cpSpaceGetPostStepCallback(space, key)){
|
||||
cpPostStepCallback *callback = (cpPostStepCallback *)cpcalloc(1, sizeof(cpPostStepCallback));
|
||||
callback->func = (func ? func : PostStepDoNothing);
|
||||
callback->key = key;
|
||||
callback->data = data;
|
||||
|
||||
cpArrayPush(space->postStepCallbacks, callback);
|
||||
return cpTrue;
|
||||
} else {
|
||||
return cpFalse;
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Locking Functions
|
||||
|
||||
void
|
||||
cpSpaceLock(cpSpace *space)
|
||||
{
|
||||
space->locked++;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceUnlock(cpSpace *space, cpBool runPostStep)
|
||||
{
|
||||
space->locked--;
|
||||
cpAssertHard(space->locked >= 0, "Internal Error: Space lock underflow.");
|
||||
|
||||
if(space->locked == 0){
|
||||
cpArray *waking = space->rousedBodies;
|
||||
|
||||
for(int i=0, count=waking->num; i<count; i++){
|
||||
cpSpaceActivateBody(space, (cpBody *)waking->arr[i]);
|
||||
waking->arr[i] = NULL;
|
||||
}
|
||||
|
||||
waking->num = 0;
|
||||
|
||||
if(space->locked == 0 && runPostStep && !space->skipPostStep){
|
||||
space->skipPostStep = cpTrue;
|
||||
|
||||
cpArray *arr = space->postStepCallbacks;
|
||||
for(int i=0; i<arr->num; i++){
|
||||
cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i];
|
||||
cpPostStepFunc func = callback->func;
|
||||
|
||||
// Mark the func as NULL in case calling it calls cpSpaceRunPostStepCallbacks() again.
|
||||
// TODO: need more tests around this case I think.
|
||||
callback->func = NULL;
|
||||
if(func) func(space, callback->key, callback->data);
|
||||
|
||||
arr->arr[i] = NULL;
|
||||
cpfree(callback);
|
||||
}
|
||||
|
||||
arr->num = 0;
|
||||
space->skipPostStep = cpFalse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Contact Buffer Functions
|
||||
|
||||
struct cpContactBufferHeader {
|
||||
cpTimestamp stamp;
|
||||
cpContactBufferHeader *next;
|
||||
unsigned int numContacts;
|
||||
};
|
||||
|
||||
#define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(struct cpContact))
|
||||
typedef struct cpContactBuffer {
|
||||
cpContactBufferHeader header;
|
||||
struct cpContact contacts[CP_CONTACTS_BUFFER_SIZE];
|
||||
} cpContactBuffer;
|
||||
|
||||
static cpContactBufferHeader *
|
||||
cpSpaceAllocContactBuffer(cpSpace *space)
|
||||
{
|
||||
cpContactBuffer *buffer = (cpContactBuffer *)cpcalloc(1, sizeof(cpContactBuffer));
|
||||
cpArrayPush(space->allocatedBuffers, buffer);
|
||||
return (cpContactBufferHeader *)buffer;
|
||||
}
|
||||
|
||||
static cpContactBufferHeader *
|
||||
cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpContactBufferHeader *splice)
|
||||
{
|
||||
header->stamp = stamp;
|
||||
header->next = (splice ? splice->next : header);
|
||||
header->numContacts = 0;
|
||||
|
||||
return header;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpacePushFreshContactBuffer(cpSpace *space)
|
||||
{
|
||||
cpTimestamp stamp = space->stamp;
|
||||
|
||||
cpContactBufferHeader *head = space->contactBuffersHead;
|
||||
|
||||
if(!head){
|
||||
// No buffers have been allocated, make one
|
||||
space->contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, NULL);
|
||||
} else if(stamp - head->next->stamp > space->collisionPersistence){
|
||||
// The tail buffer is available, rotate the ring
|
||||
cpContactBufferHeader *tail = head->next;
|
||||
space->contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail);
|
||||
} else {
|
||||
// Allocate a new buffer and push it into the ring
|
||||
cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head);
|
||||
space->contactBuffersHead = head->next = buffer;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
struct cpContact *
|
||||
cpContactBufferGetArray(cpSpace *space)
|
||||
{
|
||||
if(space->contactBuffersHead->numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){
|
||||
// contact buffer could overflow on the next collision, push a fresh one.
|
||||
cpSpacePushFreshContactBuffer(space);
|
||||
}
|
||||
|
||||
cpContactBufferHeader *head = space->contactBuffersHead;
|
||||
return ((cpContactBuffer *)head)->contacts + head->numContacts;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpacePushContacts(cpSpace *space, int count)
|
||||
{
|
||||
cpAssertHard(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal Error: Contact buffer overflow!");
|
||||
space->contactBuffersHead->numContacts += count;
|
||||
}
|
||||
|
||||
static void
|
||||
cpSpacePopContacts(cpSpace *space, int count){
|
||||
space->contactBuffersHead->numContacts -= count;
|
||||
}
|
||||
|
||||
//MARK: Collision Detection Functions
|
||||
|
||||
static void *
|
||||
cpSpaceArbiterSetTrans(cpShape **shapes, cpSpace *space)
|
||||
{
|
||||
if(space->pooledArbiters->num == 0){
|
||||
// arbiter pool is exhausted, make more
|
||||
int count = CP_BUFFER_BYTES/sizeof(cpArbiter);
|
||||
cpAssertHard(count, "Internal Error: Buffer size too small.");
|
||||
|
||||
cpArbiter *buffer = (cpArbiter *)cpcalloc(1, CP_BUFFER_BYTES);
|
||||
cpArrayPush(space->allocatedBuffers, buffer);
|
||||
|
||||
for(int i=0; i<count; i++) cpArrayPush(space->pooledArbiters, buffer + i);
|
||||
}
|
||||
|
||||
return cpArbiterInit((cpArbiter *)cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
|
||||
}
|
||||
|
||||
static inline cpBool
|
||||
QueryRejectConstraint(cpBody *a, cpBody *b)
|
||||
{
|
||||
CP_BODY_FOREACH_CONSTRAINT(a, constraint){
|
||||
if(
|
||||
!constraint->collideBodies && (
|
||||
(constraint->a == a && constraint->b == b) ||
|
||||
(constraint->a == b && constraint->b == a)
|
||||
)
|
||||
) return cpTrue;
|
||||
}
|
||||
|
||||
return cpFalse;
|
||||
}
|
||||
|
||||
static inline cpBool
|
||||
QueryReject(cpShape *a, cpShape *b)
|
||||
{
|
||||
return (
|
||||
// BBoxes must overlap
|
||||
!cpBBIntersects(a->bb, b->bb)
|
||||
// Don't collide shapes attached to the same body.
|
||||
|| a->body == b->body
|
||||
// Don't collide shapes that are filtered.
|
||||
|| cpShapeFilterReject(a->filter, b->filter)
|
||||
// Don't collide bodies if they have a constraint with collideBodies == cpFalse.
|
||||
|| QueryRejectConstraint(a->body, b->body)
|
||||
);
|
||||
}
|
||||
|
||||
// Callback from the spatial hash.
|
||||
cpCollisionID
|
||||
cpSpaceCollideShapes(cpShape *a, cpShape *b, cpCollisionID id, cpSpace *space)
|
||||
{
|
||||
// Reject any of the simple cases
|
||||
if(QueryReject(a,b)) return id;
|
||||
|
||||
// Narrow-phase collision detection.
|
||||
struct cpCollisionInfo info = cpCollide(a, b, id, cpContactBufferGetArray(space));
|
||||
|
||||
if(info.count == 0) return info.id; // Shapes are not colliding.
|
||||
cpSpacePushContacts(space, info.count);
|
||||
|
||||
// Get an arbiter from space->arbiterSet for the two shapes.
|
||||
// This is where the persistant contact magic comes from.
|
||||
const cpShape *shape_pair[] = {info.a, info.b};
|
||||
cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)info.a, (cpHashValue)info.b);
|
||||
cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, (cpHashSetTransFunc)cpSpaceArbiterSetTrans, space);
|
||||
cpArbiterUpdate(arb, &info, space);
|
||||
|
||||
cpCollisionHandler *handler = arb->handler;
|
||||
|
||||
// Call the begin function first if it's the first step
|
||||
if(arb->state == CP_ARBITER_STATE_FIRST_COLLISION && !handler->beginFunc(arb, space, handler->userData)){
|
||||
cpArbiterIgnore(arb); // permanently ignore the collision until separation
|
||||
}
|
||||
|
||||
if(
|
||||
// Ignore the arbiter if it has been flagged
|
||||
(arb->state != CP_ARBITER_STATE_IGNORE) &&
|
||||
// Call preSolve
|
||||
handler->preSolveFunc(arb, space, handler->userData) &&
|
||||
// Check (again) in case the pre-solve() callback called cpArbiterIgnored().
|
||||
arb->state != CP_ARBITER_STATE_IGNORE &&
|
||||
// Process, but don't add collisions for sensors.
|
||||
!(a->sensor || b->sensor) &&
|
||||
// Don't process collisions between two infinite mass bodies.
|
||||
// This includes collisions between two kinematic bodies, or a kinematic body and a static body.
|
||||
!(a->body->m == INFINITY && b->body->m == INFINITY)
|
||||
){
|
||||
cpArrayPush(space->arbiters, arb);
|
||||
} else {
|
||||
cpSpacePopContacts(space, info.count);
|
||||
|
||||
arb->contacts = NULL;
|
||||
arb->count = 0;
|
||||
|
||||
// Normally arbiters are set as used after calling the post-solve callback.
|
||||
// However, post-solve() callbacks are not called for sensors or arbiters rejected from pre-solve.
|
||||
if(arb->state != CP_ARBITER_STATE_IGNORE) arb->state = CP_ARBITER_STATE_NORMAL;
|
||||
}
|
||||
|
||||
// Time stamp the arbiter so we know it was used recently.
|
||||
arb->stamp = space->stamp;
|
||||
return info.id;
|
||||
}
|
||||
|
||||
// Hashset filter func to throw away old arbiters.
|
||||
cpBool
|
||||
cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space)
|
||||
{
|
||||
cpTimestamp ticks = space->stamp - arb->stamp;
|
||||
|
||||
cpBody *a = arb->body_a, *b = arb->body_b;
|
||||
|
||||
// TODO: should make an arbiter state for this so it doesn't require filtering arbiters for dangling body pointers on body removal.
|
||||
// Preserve arbiters on sensors and rejected arbiters for sleeping objects.
|
||||
// This prevents errant separate callbacks from happenening.
|
||||
if(
|
||||
(cpBodyGetType(a) == CP_BODY_TYPE_STATIC || cpBodyIsSleeping(a)) &&
|
||||
(cpBodyGetType(b) == CP_BODY_TYPE_STATIC || cpBodyIsSleeping(b))
|
||||
){
|
||||
return cpTrue;
|
||||
}
|
||||
|
||||
// Arbiter was used last frame, but not this one
|
||||
if(ticks >= 1 && arb->state != CP_ARBITER_STATE_CACHED){
|
||||
arb->state = CP_ARBITER_STATE_CACHED;
|
||||
cpCollisionHandler *handler = arb->handler;
|
||||
handler->separateFunc(arb, space, handler->userData);
|
||||
}
|
||||
|
||||
if(ticks >= space->collisionPersistence){
|
||||
arb->contacts = NULL;
|
||||
arb->count = 0;
|
||||
|
||||
cpArrayPush(space->pooledArbiters, arb);
|
||||
return cpFalse;
|
||||
}
|
||||
|
||||
return cpTrue;
|
||||
}
|
||||
|
||||
//MARK: All Important cpSpaceStep() Function
|
||||
|
||||
void
|
||||
cpShapeUpdateFunc(cpShape *shape, void *unused)
|
||||
{
|
||||
cpShapeCacheBB(shape);
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceStep(cpSpace *space, cpFloat dt)
|
||||
{
|
||||
// don't step if the timestep is 0!
|
||||
if(dt == 0.0f) return;
|
||||
|
||||
space->stamp++;
|
||||
|
||||
cpFloat prev_dt = space->curr_dt;
|
||||
space->curr_dt = dt;
|
||||
|
||||
cpArray *bodies = space->dynamicBodies;
|
||||
cpArray *constraints = space->constraints;
|
||||
cpArray *arbiters = space->arbiters;
|
||||
|
||||
// Reset and empty the arbiter lists.
|
||||
for(int i=0; i<arbiters->num; i++){
|
||||
cpArbiter *arb = (cpArbiter *)arbiters->arr[i];
|
||||
arb->state = CP_ARBITER_STATE_NORMAL;
|
||||
|
||||
// If both bodies are awake, unthread the arbiter from the contact graph.
|
||||
if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)){
|
||||
cpArbiterUnthread(arb);
|
||||
}
|
||||
}
|
||||
arbiters->num = 0;
|
||||
|
||||
cpSpaceLock(space); {
|
||||
// Integrate positions
|
||||
for(int i=0; i<bodies->num; i++){
|
||||
cpBody *body = (cpBody *)bodies->arr[i];
|
||||
body->position_func(body, dt);
|
||||
}
|
||||
|
||||
// Find colliding pairs.
|
||||
cpSpacePushFreshContactBuffer(space);
|
||||
cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL);
|
||||
cpSpatialIndexReindexQuery(space->dynamicShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space);
|
||||
} cpSpaceUnlock(space, cpFalse);
|
||||
|
||||
// Rebuild the contact graph (and detect sleeping components if sleeping is enabled)
|
||||
cpSpaceProcessComponents(space, dt);
|
||||
|
||||
cpSpaceLock(space); {
|
||||
// Clear out old cached arbiters and call separate callbacks
|
||||
cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space);
|
||||
|
||||
// Prestep the arbiters and constraints.
|
||||
cpFloat slop = space->collisionSlop;
|
||||
cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt);
|
||||
for(int i=0; i<arbiters->num; i++){
|
||||
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef);
|
||||
}
|
||||
|
||||
for(int i=0; i<constraints->num; i++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
|
||||
|
||||
cpConstraintPreSolveFunc preSolve = constraint->preSolve;
|
||||
if(preSolve) preSolve(constraint, space);
|
||||
|
||||
constraint->klass->preStep(constraint, dt);
|
||||
}
|
||||
|
||||
// Integrate velocities.
|
||||
cpFloat damping = cpfpow(space->damping, dt);
|
||||
cpVect gravity = space->gravity;
|
||||
for(int i=0; i<bodies->num; i++){
|
||||
cpBody *body = (cpBody *)bodies->arr[i];
|
||||
body->velocity_func(body, gravity, damping, dt);
|
||||
}
|
||||
|
||||
// Apply cached impulses
|
||||
cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt);
|
||||
for(int i=0; i<arbiters->num; i++){
|
||||
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef);
|
||||
}
|
||||
|
||||
for(int i=0; i<constraints->num; i++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
|
||||
constraint->klass->applyCachedImpulse(constraint, dt_coef);
|
||||
}
|
||||
|
||||
// Run the impulse solver.
|
||||
for(int i=0; i<space->iterations; i++){
|
||||
for(int j=0; j<arbiters->num; j++){
|
||||
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j]);
|
||||
}
|
||||
|
||||
for(int j=0; j<constraints->num; j++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
|
||||
constraint->klass->applyImpulse(constraint, dt);
|
||||
}
|
||||
}
|
||||
|
||||
// Run the constraint post-solve callbacks
|
||||
for(int i=0; i<constraints->num; i++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
|
||||
|
||||
cpConstraintPostSolveFunc postSolve = constraint->postSolve;
|
||||
if(postSolve) postSolve(constraint, space);
|
||||
}
|
||||
|
||||
// run the post-solve callbacks
|
||||
for(int i=0; i<arbiters->num; i++){
|
||||
cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
|
||||
|
||||
cpCollisionHandler *handler = arb->handler;
|
||||
handler->postSolveFunc(arb, space, handler->userData);
|
||||
}
|
||||
} cpSpaceUnlock(space, cpTrue);
|
||||
}
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
void
|
||||
cpSpatialIndexFree(cpSpatialIndex *index)
|
||||
{
|
||||
if(index){
|
||||
cpSpatialIndexDestroy(index);
|
||||
cpfree(index);
|
||||
}
|
||||
}
|
||||
|
||||
cpSpatialIndex *
|
||||
cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
|
||||
{
|
||||
index->klass = klass;
|
||||
index->bbfunc = bbfunc;
|
||||
index->staticIndex = staticIndex;
|
||||
|
||||
if(staticIndex){
|
||||
cpAssertHard(!staticIndex->dynamicIndex, "This static index is already associated with a dynamic index.");
|
||||
staticIndex->dynamicIndex = index;
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
typedef struct dynamicToStaticContext {
|
||||
cpSpatialIndexBBFunc bbfunc;
|
||||
cpSpatialIndex *staticIndex;
|
||||
cpSpatialIndexQueryFunc queryFunc;
|
||||
void *data;
|
||||
} dynamicToStaticContext;
|
||||
|
||||
static void
|
||||
dynamicToStaticIter(void *obj, dynamicToStaticContext *context)
|
||||
{
|
||||
cpSpatialIndexQuery(context->staticIndex, obj, context->bbfunc(obj), context->queryFunc, context->data);
|
||||
}
|
||||
|
||||
void
|
||||
cpSpatialIndexCollideStatic(cpSpatialIndex *dynamicIndex, cpSpatialIndex *staticIndex, cpSpatialIndexQueryFunc func, void *data)
|
||||
{
|
||||
if(staticIndex && cpSpatialIndexCount(staticIndex) > 0){
|
||||
dynamicToStaticContext context = {dynamicIndex->bbfunc, staticIndex, func, data};
|
||||
cpSpatialIndexEach(dynamicIndex, (cpSpatialIndexIteratorFunc)dynamicToStaticIter, &context);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,254 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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 "chipmunk/chipmunk_private.h"
|
||||
|
||||
static inline cpSpatialIndexClass *Klass();
|
||||
|
||||
//MARK: Basic Structures
|
||||
|
||||
typedef struct Bounds {
|
||||
cpFloat min, max;
|
||||
} Bounds;
|
||||
|
||||
typedef struct TableCell {
|
||||
void *obj;
|
||||
Bounds bounds;
|
||||
} TableCell;
|
||||
|
||||
struct cpSweep1D
|
||||
{
|
||||
cpSpatialIndex spatialIndex;
|
||||
|
||||
int num;
|
||||
int max;
|
||||
TableCell *table;
|
||||
};
|
||||
|
||||
static inline cpBool
|
||||
BoundsOverlap(Bounds a, Bounds b)
|
||||
{
|
||||
return (a.min <= b.max && b.min <= a.max);
|
||||
}
|
||||
|
||||
static inline Bounds
|
||||
BBToBounds(cpSweep1D *sweep, cpBB bb)
|
||||
{
|
||||
Bounds bounds = {bb.l, bb.r};
|
||||
return bounds;
|
||||
}
|
||||
|
||||
static inline TableCell
|
||||
MakeTableCell(cpSweep1D *sweep, void *obj)
|
||||
{
|
||||
TableCell cell = {obj, BBToBounds(sweep, sweep->spatialIndex.bbfunc(obj))};
|
||||
return cell;
|
||||
}
|
||||
|
||||
//MARK: Memory Management Functions
|
||||
|
||||
cpSweep1D *
|
||||
cpSweep1DAlloc(void)
|
||||
{
|
||||
return (cpSweep1D *)cpcalloc(1, sizeof(cpSweep1D));
|
||||
}
|
||||
|
||||
static void
|
||||
ResizeTable(cpSweep1D *sweep, int size)
|
||||
{
|
||||
sweep->max = size;
|
||||
sweep->table = (TableCell *)cprealloc(sweep->table, size*sizeof(TableCell));
|
||||
}
|
||||
|
||||
cpSpatialIndex *
|
||||
cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
|
||||
{
|
||||
cpSpatialIndexInit((cpSpatialIndex *)sweep, Klass(), bbfunc, staticIndex);
|
||||
|
||||
sweep->num = 0;
|
||||
ResizeTable(sweep, 32);
|
||||
|
||||
return (cpSpatialIndex *)sweep;
|
||||
}
|
||||
|
||||
cpSpatialIndex *
|
||||
cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
|
||||
{
|
||||
return cpSweep1DInit(cpSweep1DAlloc(), bbfunc, staticIndex);
|
||||
}
|
||||
|
||||
static void
|
||||
cpSweep1DDestroy(cpSweep1D *sweep)
|
||||
{
|
||||
cpfree(sweep->table);
|
||||
sweep->table = NULL;
|
||||
}
|
||||
|
||||
//MARK: Misc
|
||||
|
||||
static int
|
||||
cpSweep1DCount(cpSweep1D *sweep)
|
||||
{
|
||||
return sweep->num;
|
||||
}
|
||||
|
||||
static void
|
||||
cpSweep1DEach(cpSweep1D *sweep, cpSpatialIndexIteratorFunc func, void *data)
|
||||
{
|
||||
TableCell *table = sweep->table;
|
||||
for(int i=0, count=sweep->num; i<count; i++) func(table[i].obj, data);
|
||||
}
|
||||
|
||||
static int
|
||||
cpSweep1DContains(cpSweep1D *sweep, void *obj, cpHashValue hashid)
|
||||
{
|
||||
TableCell *table = sweep->table;
|
||||
for(int i=0, count=sweep->num; i<count; i++){
|
||||
if(table[i].obj == obj) return cpTrue;
|
||||
}
|
||||
|
||||
return cpFalse;
|
||||
}
|
||||
|
||||
//MARK: Basic Operations
|
||||
|
||||
static void
|
||||
cpSweep1DInsert(cpSweep1D *sweep, void *obj, cpHashValue hashid)
|
||||
{
|
||||
if(sweep->num == sweep->max) ResizeTable(sweep, sweep->max*2);
|
||||
|
||||
sweep->table[sweep->num] = MakeTableCell(sweep, obj);
|
||||
sweep->num++;
|
||||
}
|
||||
|
||||
static void
|
||||
cpSweep1DRemove(cpSweep1D *sweep, void *obj, cpHashValue hashid)
|
||||
{
|
||||
TableCell *table = sweep->table;
|
||||
for(int i=0, count=sweep->num; i<count; i++){
|
||||
if(table[i].obj == obj){
|
||||
int num = --sweep->num;
|
||||
|
||||
table[i] = table[num];
|
||||
table[num].obj = NULL;
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Reindexing Functions
|
||||
|
||||
static void
|
||||
cpSweep1DReindexObject(cpSweep1D *sweep, void *obj, cpHashValue hashid)
|
||||
{
|
||||
// Nothing to do here
|
||||
}
|
||||
|
||||
static void
|
||||
cpSweep1DReindex(cpSweep1D *sweep)
|
||||
{
|
||||
// Nothing to do here
|
||||
// Could perform a sort, but queries are not accelerated anyway.
|
||||
}
|
||||
|
||||
//MARK: Query Functions
|
||||
|
||||
static void
|
||||
cpSweep1DQuery(cpSweep1D *sweep, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data)
|
||||
{
|
||||
// Implementing binary search here would allow you to find an upper limit
|
||||
// but not a lower limit. Probably not worth the hassle.
|
||||
|
||||
Bounds bounds = BBToBounds(sweep, bb);
|
||||
|
||||
TableCell *table = sweep->table;
|
||||
for(int i=0, count=sweep->num; i<count; i++){
|
||||
TableCell cell = table[i];
|
||||
if(BoundsOverlap(bounds, cell.bounds) && obj != cell.obj) func(obj, cell.obj, 0, data);
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
cpSweep1DSegmentQuery(cpSweep1D *sweep, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
|
||||
{
|
||||
cpBB bb = cpBBExpand(cpBBNew(a.x, a.y, a.x, a.y), b);
|
||||
Bounds bounds = BBToBounds(sweep, bb);
|
||||
|
||||
TableCell *table = sweep->table;
|
||||
for(int i=0, count=sweep->num; i<count; i++){
|
||||
TableCell cell = table[i];
|
||||
if(BoundsOverlap(bounds, cell.bounds)) func(obj, cell.obj, data);
|
||||
}
|
||||
}
|
||||
|
||||
//MARK: Reindex/Query
|
||||
|
||||
static int
|
||||
TableSort(TableCell *a, TableCell *b)
|
||||
{
|
||||
return (a->bounds.min < b->bounds.min ? -1 : (a->bounds.min > b->bounds.min ? 1 : 0));
|
||||
}
|
||||
|
||||
static void
|
||||
cpSweep1DReindexQuery(cpSweep1D *sweep, cpSpatialIndexQueryFunc func, void *data)
|
||||
{
|
||||
TableCell *table = sweep->table;
|
||||
int count = sweep->num;
|
||||
|
||||
// Update bounds and sort
|
||||
for(int i=0; i<count; i++) table[i] = MakeTableCell(sweep, table[i].obj);
|
||||
qsort(table, count, sizeof(TableCell), (int (*)(const void *, const void *))TableSort); // TODO: use insertion sort instead
|
||||
|
||||
for(int i=0; i<count; i++){
|
||||
TableCell cell = table[i];
|
||||
cpFloat max = cell.bounds.max;
|
||||
|
||||
for(int j=i+1; table[j].bounds.min < max && j<count; j++){
|
||||
func(cell.obj, table[j].obj, 0, data);
|
||||
}
|
||||
}
|
||||
|
||||
// Reindex query is also responsible for colliding against the static index.
|
||||
// Fortunately there is a helper function for that.
|
||||
cpSpatialIndexCollideStatic((cpSpatialIndex *)sweep, sweep->spatialIndex.staticIndex, func, data);
|
||||
}
|
||||
|
||||
static cpSpatialIndexClass klass = {
|
||||
(cpSpatialIndexDestroyImpl)cpSweep1DDestroy,
|
||||
|
||||
(cpSpatialIndexCountImpl)cpSweep1DCount,
|
||||
(cpSpatialIndexEachImpl)cpSweep1DEach,
|
||||
(cpSpatialIndexContainsImpl)cpSweep1DContains,
|
||||
|
||||
(cpSpatialIndexInsertImpl)cpSweep1DInsert,
|
||||
(cpSpatialIndexRemoveImpl)cpSweep1DRemove,
|
||||
|
||||
(cpSpatialIndexReindexImpl)cpSweep1DReindex,
|
||||
(cpSpatialIndexReindexObjectImpl)cpSweep1DReindexObject,
|
||||
(cpSpatialIndexReindexQueryImpl)cpSweep1DReindexQuery,
|
||||
|
||||
(cpSpatialIndexQueryImpl)cpSweep1DQuery,
|
||||
(cpSpatialIndexSegmentQueryImpl)cpSweep1DSegmentQuery,
|
||||
};
|
||||
|
||||
static inline cpSpatialIndexClass *Klass(){return &klass;}
|
||||
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
/* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any 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 is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be 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 AN 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.
|
||||
*/
|
||||
|
||||
// Used for resizing hash tables.
|
||||
// Values approximately double.
|
||||
// http://planetmath.org/encyclopedia/GoodHashTablePrimes.html
|
||||
static int primes[] = {
|
||||
5,
|
||||
13,
|
||||
23,
|
||||
47,
|
||||
97,
|
||||
193,
|
||||
389,
|
||||
769,
|
||||
1543,
|
||||
3079,
|
||||
6151,
|
||||
12289,
|
||||
24593,
|
||||
49157,
|
||||
98317,
|
||||
196613,
|
||||
393241,
|
||||
786433,
|
||||
1572869,
|
||||
3145739,
|
||||
6291469,
|
||||
12582917,
|
||||
25165843,
|
||||
50331653,
|
||||
100663319,
|
||||
201326611,
|
||||
402653189,
|
||||
805306457,
|
||||
1610612741,
|
||||
0,
|
||||
};
|
||||
|
||||
static inline int
|
||||
next_prime(int n)
|
||||
{
|
||||
int i = 0;
|
||||
while(n > primes[i]){
|
||||
i++;
|
||||
cpAssertHard(primes[i], "Tried to resize a hash table to a size greater than 1610612741 O_o"); // realistically this should never happen
|
||||
}
|
||||
|
||||
return primes[i];
|
||||
}
|
||||
Loading…
Reference in New Issue