mirror of https://github.com/axmolengine/axmol.git
issue #503, new files in chipmunk 5.3.4
This commit is contained in:
Walzer
parent
948c6d750e
commit
2db1f2350c
|
|
@ -0,0 +1,55 @@
|
|||
/* 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.
|
||||
*/
|
||||
|
||||
#ifndef _CHIPMUNK_PRIVATE_H_
|
||||
#define _CHIPMUNK_PRIVATE_H_
|
||||
|
||||
#define CP_ALLOW_PRIVATE_ACCESS 1
|
||||
#include "chipmunk.h"
|
||||
|
||||
void *cpSpaceGetPostStepData(cpSpace *space, void *obj);
|
||||
|
||||
void cpSpaceActivateBody(cpSpace *space, cpBody *body);
|
||||
|
||||
static inline void
|
||||
cpSpaceLock(cpSpace *space)
|
||||
{
|
||||
space->locked++;
|
||||
}
|
||||
|
||||
static inline void
|
||||
cpSpaceUnlock(cpSpace *space)
|
||||
{
|
||||
space->locked--;
|
||||
cpAssert(space->locked >= 0, "Internal error:Space lock underflow.");
|
||||
|
||||
if(!space->locked){
|
||||
cpArray *waking = space->rousedBodies;
|
||||
for(int i=0, count=waking->num; i<count; i++){
|
||||
cpSpaceActivateBody(space, (cpBody *)waking->arr[i]);
|
||||
}
|
||||
|
||||
waking->num = 0;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,279 @@
|
|||
/* 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 <stdlib.h>
|
||||
|
||||
#include "chipmunk_private.h"
|
||||
|
||||
#pragma mark Sleeping Functions
|
||||
|
||||
// Chipmunk uses a data structure called a disjoint set forest.
|
||||
// My attempts to find a way to splice circularly linked lists in
|
||||
// constant time failed, and so I found this neat data structure instead.
|
||||
|
||||
static inline cpBody *
|
||||
componentNodeRoot(cpBody *body)
|
||||
{
|
||||
cpBody *parent = body->node.parent;
|
||||
|
||||
if(parent){
|
||||
// path compression, attaches this node directly to the root
|
||||
return (body->node.parent = componentNodeRoot(parent));
|
||||
} else {
|
||||
return body;
|
||||
}
|
||||
}
|
||||
|
||||
static inline void
|
||||
componentNodeMerge(cpBody *a_root, cpBody *b_root)
|
||||
{
|
||||
if(a_root->node.rank < b_root->node.rank){
|
||||
a_root->node.parent = b_root;
|
||||
} else if(a_root->node.rank > b_root->node.rank){
|
||||
b_root->node.parent = a_root;
|
||||
} else if(a_root != b_root){
|
||||
b_root->node.parent = a_root;
|
||||
a_root->node.rank++;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceActivateBody(cpSpace *space, cpBody *body)
|
||||
{
|
||||
if(space->locked){
|
||||
// cpSpaceActivateBody() is called again once the space is unlocked
|
||||
cpArrayPush(space->rousedBodies, body);
|
||||
} else {
|
||||
cpArrayPush(space->bodies, body);
|
||||
for(cpShape *shape=body->shapesList; shape; shape=shape->next){
|
||||
cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
|
||||
cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static inline void
|
||||
componentActivate(cpBody *root)
|
||||
{
|
||||
if(!cpBodyIsSleeping(root)) return;
|
||||
|
||||
cpSpace *space = root->space;
|
||||
cpAssert(space, "Trying to activate a body that was never added to a space.");
|
||||
|
||||
cpBody *body = root, *next;
|
||||
do {
|
||||
next = body->node.next;
|
||||
|
||||
cpComponentNode node = {NULL, NULL, 0, 0.0f};
|
||||
body->node = node;
|
||||
|
||||
cpSpaceActivateBody(space, body);
|
||||
} while((body = next) != root);
|
||||
|
||||
cpArrayDeleteObj(space->sleepingComponents, root);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodyActivate(cpBody *body)
|
||||
{
|
||||
componentActivate(componentNodeRoot(body));
|
||||
}
|
||||
|
||||
static inline void
|
||||
mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b)
|
||||
{
|
||||
// Ignore connections to static bodies
|
||||
if(cpBodyIsStatic(a) || cpBodyIsStatic(b)) return;
|
||||
|
||||
cpBody *a_root = componentNodeRoot(a);
|
||||
cpBody *b_root = componentNodeRoot(b);
|
||||
|
||||
cpBool a_sleep = cpBodyIsSleeping(a_root);
|
||||
cpBool b_sleep = cpBodyIsSleeping(b_root);
|
||||
|
||||
if(a_sleep && b_sleep){
|
||||
return;
|
||||
} else if(a_sleep || b_sleep){
|
||||
componentActivate(a_root);
|
||||
componentActivate(b_root);
|
||||
}
|
||||
|
||||
// Add any rogue bodies found to the list and reset the idle time of anything they touch.
|
||||
if(cpBodyIsRogue(a)){ cpArrayPush(rogueBodies, a); b->node.idleTime = 0.0f; }
|
||||
if(cpBodyIsRogue(b)){ cpArrayPush(rogueBodies, b); a->node.idleTime = 0.0f; }
|
||||
|
||||
componentNodeMerge(a_root, b_root);
|
||||
}
|
||||
|
||||
static inline cpBool
|
||||
componentActive(cpBody *root, cpFloat threshold)
|
||||
{
|
||||
cpBody *body = root, *next;
|
||||
do {
|
||||
next = body->node.next;
|
||||
if(body->node.idleTime < threshold) return cpTrue;
|
||||
} while((body = next) != root);
|
||||
|
||||
return cpFalse;
|
||||
}
|
||||
|
||||
static inline void
|
||||
addToComponent(cpBody *body, cpArray *components)
|
||||
{
|
||||
// Check that the body is not already added to the component list
|
||||
if(body->node.next) return;
|
||||
cpBody *root = componentNodeRoot(body);
|
||||
|
||||
cpBody *next = root->node.next;
|
||||
if(!next){
|
||||
// If the root isn't part of a list yet, then it hasn't been
|
||||
// added to the components list. Do that now.
|
||||
cpArrayPush(components, root);
|
||||
// Start the list
|
||||
body->node.next = root;
|
||||
root->node.next = body;
|
||||
} else if(root != body) {
|
||||
// Splice in body after the root.
|
||||
body->node.next = next;
|
||||
root->node.next = body;
|
||||
}
|
||||
}
|
||||
|
||||
// TODO this function needs more commenting.
|
||||
void
|
||||
cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
|
||||
{
|
||||
cpArray *bodies = space->bodies;
|
||||
cpArray *newBodies = cpArrayNew(bodies->num);
|
||||
cpArray *rogueBodies = cpArrayNew(16);
|
||||
cpArray *arbiters = space->arbiters;
|
||||
cpArray *constraints = space->constraints;
|
||||
cpArray *components = cpArrayNew(space->sleepingComponents->num);
|
||||
|
||||
cpFloat dv = space->idleSpeedThreshold;
|
||||
cpFloat dvsq = (dv ? dv*dv : cpvdot(space->gravity, space->gravity)*dt*dt);
|
||||
|
||||
// update idling
|
||||
for(int i=0; i<bodies->num; i++){
|
||||
cpBody *body = (cpBody*)bodies->arr[i];
|
||||
|
||||
cpFloat thresh = (dvsq ? body->m*dvsq : 0.0f);
|
||||
body->node.idleTime = (cpBodyKineticEnergy(body) > thresh ? 0.0f : body->node.idleTime + dt);
|
||||
}
|
||||
|
||||
// iterate graph edges and build forests
|
||||
for(int i=0; i<arbiters->num; i++){
|
||||
cpArbiter *arb = (cpArbiter*)arbiters->arr[i];
|
||||
mergeBodies(space, components, rogueBodies, arb->a->body, arb->b->body);
|
||||
}
|
||||
for(int j=0; j<constraints->num; j++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
|
||||
mergeBodies(space, components, rogueBodies, constraint->a, constraint->b);
|
||||
}
|
||||
|
||||
// iterate bodies and add them to their components
|
||||
for(int i=0; i<bodies->num; i++) addToComponent((cpBody*)bodies->arr[i], components);
|
||||
for(int i=0; i<rogueBodies->num; i++) addToComponent((cpBody*)rogueBodies->arr[i], components);
|
||||
|
||||
// iterate components, copy or deactivate
|
||||
for(int i=0; i<components->num; i++){
|
||||
cpBody *root = (cpBody*)components->arr[i];
|
||||
if(componentActive(root, space->sleepTimeThreshold)){
|
||||
cpBody *body = root, *next;
|
||||
do {
|
||||
next = body->node.next;
|
||||
|
||||
if(!cpBodyIsRogue(body)) cpArrayPush(newBodies, body);
|
||||
cpComponentNode node = {NULL, NULL, 0, body->node.idleTime};
|
||||
body->node = node;
|
||||
} while((body = next) != root);
|
||||
} else {
|
||||
cpBody *body = root, *next;
|
||||
do {
|
||||
next = body->node.next;
|
||||
|
||||
for(cpShape *shape = body->shapesList; shape; shape = shape->next){
|
||||
cpSpaceHashRemove(space->activeShapes, shape, shape->hashid);
|
||||
cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
|
||||
}
|
||||
} while((body = next) != root);
|
||||
|
||||
cpArrayPush(space->sleepingComponents, root);
|
||||
}
|
||||
}
|
||||
|
||||
space->bodies = newBodies;
|
||||
cpArrayFree(bodies);
|
||||
cpArrayFree(rogueBodies);
|
||||
cpArrayFree(components);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySleep(cpBody *body)
|
||||
{
|
||||
cpBodySleepWithGroup(body, NULL);
|
||||
}
|
||||
|
||||
void
|
||||
cpBodySleepWithGroup(cpBody *body, cpBody *group){
|
||||
cpAssert(!cpBodyIsStatic(body) && !cpBodyIsRogue(body), "Rogue and static bodies cannot be put to sleep.");
|
||||
|
||||
cpSpace *space = body->space;
|
||||
cpAssert(space, "Cannot put a body to sleep that has not been added to a space.");
|
||||
cpAssert(!space->locked, "Bodies can not be put to sleep during a query or a call to cpSpaceSte(). Put these calls into a post-step callback.");
|
||||
cpAssert(!group || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier.");
|
||||
|
||||
if(cpBodyIsSleeping(body)) return;
|
||||
|
||||
for(cpShape *shape = body->shapesList; shape; shape = shape->next){
|
||||
cpShapeCacheBB(shape);
|
||||
cpSpaceHashRemove(space->activeShapes, shape, shape->hashid);
|
||||
cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
|
||||
}
|
||||
|
||||
if(group){
|
||||
cpBody *root = componentNodeRoot(group);
|
||||
|
||||
cpComponentNode node = {root, root->node.next, 0, 0.0f};
|
||||
body->node = node;
|
||||
root->node.next = body;
|
||||
} else {
|
||||
cpComponentNode node = {NULL, body, 0, 0.0f};
|
||||
body->node = node;
|
||||
|
||||
cpArrayPush(space->sleepingComponents, body);
|
||||
}
|
||||
|
||||
cpArrayDeleteObj(space->bodies, body);
|
||||
}
|
||||
|
||||
static void
|
||||
activateTouchingHelper(cpShape *shape, cpContactPointSet *points, cpArray **bodies){
|
||||
cpBodyActivate(shape->body);
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape){
|
||||
cpArray *bodies = NULL;
|
||||
cpSpaceShapeQuery(space, shape, (cpSpaceShapeQueryFunc)activateTouchingHelper, &bodies);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,246 @@
|
|||
/* 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 <stdlib.h>
|
||||
|
||||
#include "chipmunk_private.h"
|
||||
|
||||
#pragma mark Point Query Functions
|
||||
|
||||
typedef struct pointQueryContext {
|
||||
cpLayers layers;
|
||||
cpGroup group;
|
||||
cpSpacePointQueryFunc func;
|
||||
void *data;
|
||||
} pointQueryContext;
|
||||
|
||||
static void
|
||||
pointQueryHelper(cpVect *point, cpShape *shape, pointQueryContext *context)
|
||||
{
|
||||
if(
|
||||
!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
|
||||
cpShapePointQuery(shape, *point)
|
||||
){
|
||||
context->func(shape, context->data);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data)
|
||||
{
|
||||
pointQueryContext context = {layers, group, func, data};
|
||||
|
||||
cpSpaceLock(space); {
|
||||
cpSpaceHashPointQuery(space->activeShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context);
|
||||
cpSpaceHashPointQuery(space->staticShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context);
|
||||
} cpSpaceUnlock(space);
|
||||
}
|
||||
|
||||
static void
|
||||
rememberLastPointQuery(cpShape *shape, cpShape **outShape)
|
||||
{
|
||||
if(!shape->sensor) *outShape = shape;
|
||||
}
|
||||
|
||||
cpShape *
|
||||
cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group)
|
||||
{
|
||||
cpShape *shape = NULL;
|
||||
cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)rememberLastPointQuery, &shape);
|
||||
|
||||
return shape;
|
||||
}
|
||||
|
||||
|
||||
#pragma mark Segment Query Functions
|
||||
|
||||
typedef struct segQueryContext {
|
||||
cpVect start, end;
|
||||
cpLayers layers;
|
||||
cpGroup group;
|
||||
cpSpaceSegmentQueryFunc func;
|
||||
} segQueryContext;
|
||||
|
||||
static cpFloat
|
||||
segQueryFunc(segQueryContext *context, cpShape *shape, void *data)
|
||||
{
|
||||
cpSegmentQueryInfo info;
|
||||
|
||||
if(
|
||||
!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
|
||||
cpShapeSegmentQuery(shape, context->start, context->end, &info)
|
||||
){
|
||||
context->func(shape, info.t, info.n, data);
|
||||
}
|
||||
|
||||
return 1.0f;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data)
|
||||
{
|
||||
segQueryContext context = {
|
||||
start, end,
|
||||
layers, group,
|
||||
func,
|
||||
};
|
||||
|
||||
cpSpaceLock(space); {
|
||||
cpSpaceHashSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data);
|
||||
cpSpaceHashSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFunc, data);
|
||||
} cpSpaceUnlock(space);
|
||||
}
|
||||
|
||||
typedef struct segQueryFirstContext {
|
||||
cpVect start, end;
|
||||
cpLayers layers;
|
||||
cpGroup group;
|
||||
} segQueryFirstContext;
|
||||
|
||||
static cpFloat
|
||||
segQueryFirst(segQueryFirstContext *context, cpShape *shape, cpSegmentQueryInfo *out)
|
||||
{
|
||||
cpSegmentQueryInfo info;
|
||||
|
||||
if(
|
||||
!(shape->group && context->group == shape->group) &&
|
||||
(context->layers&shape->layers) &&
|
||||
!shape->sensor &&
|
||||
cpShapeSegmentQuery(shape, context->start, context->end, &info) &&
|
||||
info.t < out->t
|
||||
){
|
||||
*out = info;
|
||||
}
|
||||
|
||||
return out->t;
|
||||
}
|
||||
|
||||
cpShape *
|
||||
cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out)
|
||||
{
|
||||
cpSegmentQueryInfo info = {NULL, 1.0f, cpvzero};
|
||||
if(out){
|
||||
(*out) = info;
|
||||
} else {
|
||||
out = &info;
|
||||
}
|
||||
|
||||
segQueryFirstContext context = {
|
||||
start, end,
|
||||
layers, group
|
||||
};
|
||||
|
||||
cpSpaceHashSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpaceHashSegmentQueryFunc)segQueryFirst, out);
|
||||
cpSpaceHashSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpaceHashSegmentQueryFunc)segQueryFirst, out);
|
||||
|
||||
return out->shape;
|
||||
}
|
||||
|
||||
#pragma mark BB Query Functions
|
||||
|
||||
typedef struct bbQueryContext {
|
||||
cpLayers layers;
|
||||
cpGroup group;
|
||||
cpSpaceBBQueryFunc func;
|
||||
void *data;
|
||||
} bbQueryContext;
|
||||
|
||||
static void
|
||||
bbQueryHelper(cpBB *bb, cpShape *shape, bbQueryContext *context)
|
||||
{
|
||||
if(
|
||||
!(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
|
||||
cpBBintersects(*bb, shape->bb)
|
||||
){
|
||||
context->func(shape, context->data);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data)
|
||||
{
|
||||
bbQueryContext context = {layers, group, func, data};
|
||||
|
||||
cpSpaceLock(space); {
|
||||
cpSpaceHashQuery(space->activeShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context);
|
||||
cpSpaceHashQuery(space->staticShapes, &bb, bb, (cpSpaceHashQueryFunc)bbQueryHelper, &context);
|
||||
} cpSpaceUnlock(space);
|
||||
}
|
||||
|
||||
#pragma mark Shape Query Functions
|
||||
|
||||
typedef struct shapeQueryContext {
|
||||
cpSpaceShapeQueryFunc func;
|
||||
void *data;
|
||||
cpBool anyCollision;
|
||||
} shapeQueryContext;
|
||||
|
||||
// Callback from the spatial hash.
|
||||
static void
|
||||
shapeQueryHelper(cpShape *a, cpShape *b, shapeQueryContext *context)
|
||||
{
|
||||
// Reject any of the simple cases
|
||||
if(
|
||||
(a->group && a->group == b->group) ||
|
||||
!(a->layers & b->layers) ||
|
||||
a->sensor || b->sensor
|
||||
) return;
|
||||
|
||||
cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER];
|
||||
int numContacts = 0;
|
||||
|
||||
// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
|
||||
if(a->klass->type <= b->klass->type){
|
||||
numContacts = cpCollideShapes(a, b, contacts);
|
||||
} else {
|
||||
numContacts = cpCollideShapes(b, a, contacts);
|
||||
for(int i=0; i<numContacts; i++) contacts[i].n = cpvneg(contacts[i].n);
|
||||
}
|
||||
|
||||
if(numContacts){
|
||||
context->anyCollision = cpTrue;
|
||||
|
||||
if(context->func){
|
||||
cpContactPointSet set = {numContacts, {}};
|
||||
for(int i=0; i<set.count; i++){
|
||||
set.points[i].point = contacts[i].p;
|
||||
set.points[i].normal = contacts[i].p;
|
||||
set.points[i].dist = contacts[i].dist;
|
||||
}
|
||||
|
||||
context->func(b, &set, context->data);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
cpBool
|
||||
cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data)
|
||||
{
|
||||
cpBB bb = cpShapeCacheBB(shape);
|
||||
shapeQueryContext context = {func, data, cpFalse};
|
||||
|
||||
cpSpaceLock(space); {
|
||||
cpSpaceHashQuery(space->activeShapes, shape, bb, (cpSpaceHashQueryFunc)shapeQueryHelper, &context);
|
||||
cpSpaceHashQuery(space->staticShapes, shape, bb, (cpSpaceHashQueryFunc)shapeQueryHelper, &context);
|
||||
} cpSpaceUnlock(space);
|
||||
|
||||
return context.anyCollision;
|
||||
}
|
||||
|
|
@ -0,0 +1,398 @@
|
|||
/* 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 <stdlib.h>
|
||||
//#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "chipmunk_private.h"
|
||||
|
||||
#pragma mark Post Step Callback Functions
|
||||
|
||||
typedef struct PostStepCallback {
|
||||
cpPostStepFunc func;
|
||||
void *obj;
|
||||
void *data;
|
||||
} PostStepCallback;
|
||||
|
||||
static cpBool
|
||||
postStepFuncSetEql(PostStepCallback *a, PostStepCallback *b){
|
||||
return a->obj == b->obj;
|
||||
}
|
||||
|
||||
static void *
|
||||
postStepFuncSetTrans(PostStepCallback *callback, void *ignored)
|
||||
{
|
||||
PostStepCallback *value = (PostStepCallback *)cpmalloc(sizeof(PostStepCallback));
|
||||
(*value) = (*callback);
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
void
|
||||
cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data)
|
||||
{
|
||||
if(!space->postStepCallbacks){
|
||||
space->postStepCallbacks = cpHashSetNew(0, (cpHashSetEqlFunc)postStepFuncSetEql, (cpHashSetTransFunc)postStepFuncSetTrans);
|
||||
}
|
||||
|
||||
PostStepCallback callback = {func, obj, data};
|
||||
cpHashSetInsert(space->postStepCallbacks, (cpHashValue)(size_t)obj, &callback, NULL);
|
||||
}
|
||||
|
||||
void *
|
||||
cpSpaceGetPostStepData(cpSpace *space, void *obj)
|
||||
{
|
||||
if(space->postStepCallbacks){
|
||||
PostStepCallback query = {NULL, obj, NULL};
|
||||
PostStepCallback *callback = (PostStepCallback *)cpHashSetFind(space->postStepCallbacks, (cpHashValue)(size_t)obj, &query);
|
||||
return (callback ? callback->data : NULL);
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
#pragma mark Contact Buffer Functions
|
||||
|
||||
#define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(cpContact))
|
||||
typedef struct cpContactBuffer {
|
||||
cpContactBufferHeader header;
|
||||
cpContact contacts[CP_CONTACTS_BUFFER_SIZE];
|
||||
} cpContactBuffer;
|
||||
|
||||
static cpContactBufferHeader *
|
||||
cpSpaceAllocContactBuffer(cpSpace *space)
|
||||
{
|
||||
cpContactBuffer *buffer = (cpContactBuffer *)cpmalloc(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;
|
||||
}
|
||||
|
||||
static 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 > cp_contact_persistence){
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static 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;
|
||||
}
|
||||
|
||||
static inline void
|
||||
cpSpacePushContacts(cpSpace *space, int count){
|
||||
cpAssert(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal error, too many contact point overflow!");
|
||||
space->contactBuffersHead->numContacts += count;
|
||||
}
|
||||
|
||||
static inline void
|
||||
cpSpacePopContacts(cpSpace *space, int count){
|
||||
space->contactBuffersHead->numContacts -= count;
|
||||
}
|
||||
|
||||
#pragma mark Collision Detection Functions
|
||||
|
||||
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 objects in the same non-zero group
|
||||
|| (a->group && a->group == b->group)
|
||||
// Don't collide objects that don't share at least on layer.
|
||||
|| !(a->layers & b->layers);
|
||||
}
|
||||
|
||||
// Callback from the spatial hash.
|
||||
static void
|
||||
queryFunc(cpShape *a, cpShape *b, cpSpace *space)
|
||||
{
|
||||
// Reject any of the simple cases
|
||||
if(queryReject(a,b)) return;
|
||||
|
||||
// Find the collision pair function for the shapes.
|
||||
struct{cpCollisionType a, b;} ids = {a->collision_type, b->collision_type};
|
||||
cpHashValue collHashID = CP_HASH_PAIR(a->collision_type, b->collision_type);
|
||||
cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collFuncSet, collHashID, &ids);
|
||||
|
||||
cpBool sensor = a->sensor || b->sensor;
|
||||
if(sensor && handler == &space->defaultHandler) return;
|
||||
|
||||
// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
|
||||
if(a->klass->type > b->klass->type){
|
||||
cpShape *temp = a;
|
||||
a = b;
|
||||
b = temp;
|
||||
}
|
||||
|
||||
// Narrow-phase collision detection.
|
||||
cpContact *contacts = cpContactBufferGetArray(space);
|
||||
int numContacts = cpCollideShapes(a, b, contacts);
|
||||
if(!numContacts) return; // Shapes are not colliding.
|
||||
cpSpacePushContacts(space, numContacts);
|
||||
|
||||
// Get an arbiter from space->contactSet for the two shapes.
|
||||
// This is where the persistant contact magic comes from.
|
||||
cpShape *shape_pair[] = {a, b};
|
||||
cpHashValue arbHashID = CP_HASH_PAIR((size_t)a, (size_t)b);
|
||||
cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, arbHashID, shape_pair, space);
|
||||
cpArbiterUpdate(arb, contacts, numContacts, handler, a, b);
|
||||
|
||||
// Call the begin function first if it's the first step
|
||||
if(arb->state == cpArbiterStateFirstColl && !handler->begin(arb, space, handler->data)){
|
||||
cpArbiterIgnore(arb); // permanently ignore the collision until separation
|
||||
}
|
||||
|
||||
if(
|
||||
// Ignore the arbiter if it has been flagged
|
||||
(arb->state != cpArbiterStateIgnore) &&
|
||||
// Call preSolve
|
||||
handler->preSolve(arb, space, handler->data) &&
|
||||
// Process, but don't add collisions for sensors.
|
||||
!sensor
|
||||
){
|
||||
cpArrayPush(space->arbiters, arb);
|
||||
} else {
|
||||
cpSpacePopContacts(space, numContacts);
|
||||
|
||||
arb->contacts = NULL;
|
||||
arb->numContacts = 0;
|
||||
|
||||
// Normally arbiters are set as used after calling the post-step callback.
|
||||
// However, post-step callbacks are not called for sensors or arbiters rejected from pre-solve.
|
||||
if(arb->state != cpArbiterStateIgnore) arb->state = cpArbiterStateNormal;
|
||||
}
|
||||
|
||||
// Time stamp the arbiter so we know it was used recently.
|
||||
arb->stamp = space->stamp;
|
||||
}
|
||||
|
||||
// Iterator for active/static hash collisions.
|
||||
static void
|
||||
active2staticIter(cpShape *shape, cpSpace *space)
|
||||
{
|
||||
cpSpaceHashQuery(space->staticShapes, shape, shape->bb, (cpSpaceHashQueryFunc)queryFunc, space);
|
||||
}
|
||||
|
||||
// Hashset filter func to throw away old arbiters.
|
||||
static cpBool
|
||||
contactSetFilter(cpArbiter *arb, cpSpace *space)
|
||||
{
|
||||
if(space->sleepTimeThreshold != INFINITY){
|
||||
cpBody *a = arb->a->body;
|
||||
cpBody *b = arb->b->body;
|
||||
|
||||
// both bodies are either static or sleeping
|
||||
cpBool sleepingNow =
|
||||
(cpBodyIsStatic(a) || cpBodyIsSleeping(a)) &&
|
||||
(cpBodyIsStatic(b) || cpBodyIsSleeping(b));
|
||||
|
||||
if(sleepingNow){
|
||||
arb->state = cpArbiterStateSleep;
|
||||
return cpTrue;
|
||||
} else if(arb->state == cpArbiterStateSleep){
|
||||
// wake up the arbiter and continue as normal
|
||||
arb->state = cpArbiterStateNormal;
|
||||
// TODO is it possible that cpArbiterStateIgnore should be set here instead?
|
||||
}
|
||||
}
|
||||
|
||||
cpTimestamp ticks = space->stamp - arb->stamp;
|
||||
|
||||
// was used last frame, but not this one
|
||||
if(ticks >= 1 && arb->state != cpArbiterStateCached){
|
||||
arb->handler->separate(arb, space, arb->handler->data);
|
||||
arb->state = cpArbiterStateCached;
|
||||
}
|
||||
|
||||
if(ticks >= cp_contact_persistence){
|
||||
arb->contacts = NULL;
|
||||
arb->numContacts = 0;
|
||||
|
||||
cpArrayPush(space->pooledArbiters, arb);
|
||||
return cpFalse;
|
||||
}
|
||||
|
||||
return cpTrue;
|
||||
}
|
||||
|
||||
// Hashset filter func to call and throw away post step callbacks.
|
||||
static void
|
||||
postStepCallbackSetIter(PostStepCallback *callback, cpSpace *space)
|
||||
{
|
||||
callback->func(space, callback->obj, callback->data);
|
||||
cpfree(callback);
|
||||
}
|
||||
|
||||
#pragma mark All Important cpSpaceStep() Function
|
||||
|
||||
void cpSpaceProcessComponents(cpSpace *space, cpFloat dt);
|
||||
|
||||
static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);}
|
||||
|
||||
void
|
||||
cpSpaceStep(cpSpace *space, cpFloat dt)
|
||||
{
|
||||
if(!dt) return; // don't step if the timestep is 0!
|
||||
cpFloat dt_inv = 1.0f/dt;
|
||||
|
||||
cpArray *bodies = space->bodies;
|
||||
cpArray *constraints = space->constraints;
|
||||
|
||||
// Empty the arbiter list.
|
||||
space->arbiters->num = 0;
|
||||
|
||||
// Integrate positions.
|
||||
for(int i=0; i<bodies->num; i++){
|
||||
cpBody *body = (cpBody *)bodies->arr[i];
|
||||
body->position_func(body, dt);
|
||||
}
|
||||
|
||||
// Pre-cache BBoxes and shape data.
|
||||
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)updateBBCache, NULL);
|
||||
|
||||
cpSpaceLock(space);
|
||||
|
||||
// Collide!
|
||||
cpSpacePushFreshContactBuffer(space);
|
||||
if(space->staticShapes->handleSet->entries)
|
||||
cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)active2staticIter, space);
|
||||
cpSpaceHashQueryRehash(space->activeShapes, (cpSpaceHashQueryFunc)queryFunc, space);
|
||||
|
||||
cpSpaceUnlock(space);
|
||||
|
||||
// If body sleeping is enabled, do that now.
|
||||
if(space->sleepTimeThreshold != INFINITY){
|
||||
cpSpaceProcessComponents(space, dt);
|
||||
bodies = space->bodies; // rebuilt by processContactComponents()
|
||||
}
|
||||
|
||||
// Clear out old cached arbiters and dispatch untouch functions
|
||||
cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilter, space);
|
||||
|
||||
// Prestep the arbiters.
|
||||
cpArray *arbiters = space->arbiters;
|
||||
for(int i=0; i<arbiters->num; i++)
|
||||
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv);
|
||||
|
||||
// Prestep the constraints.
|
||||
for(int i=0; i<constraints->num; i++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
|
||||
constraint->klass->preStep(constraint, dt, dt_inv);
|
||||
}
|
||||
|
||||
for(int i=0; i<space->elasticIterations; i++){
|
||||
for(int j=0; j<arbiters->num; j++)
|
||||
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f);
|
||||
|
||||
for(int j=0; j<constraints->num; j++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
|
||||
constraint->klass->applyImpulse(constraint);
|
||||
}
|
||||
}
|
||||
|
||||
// Integrate velocities.
|
||||
cpFloat damping = cpfpow(1.0f/space->damping, -dt);
|
||||
for(int i=0; i<bodies->num; i++){
|
||||
cpBody *body = (cpBody *)bodies->arr[i];
|
||||
body->velocity_func(body, space->gravity, damping, dt);
|
||||
}
|
||||
|
||||
for(int i=0; i<arbiters->num; i++)
|
||||
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]);
|
||||
|
||||
// run the old-style elastic solver if elastic iterations are disabled
|
||||
cpFloat elasticCoef = (space->elasticIterations ? 0.0f : 1.0f);
|
||||
|
||||
// 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], elasticCoef);
|
||||
|
||||
for(int j=0; j<constraints->num; j++){
|
||||
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
|
||||
constraint->klass->applyImpulse(constraint);
|
||||
}
|
||||
}
|
||||
|
||||
cpSpaceLock(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->postSolve(arb, space, handler->data);
|
||||
|
||||
arb->state = cpArbiterStateNormal;
|
||||
}
|
||||
|
||||
cpSpaceUnlock(space);
|
||||
|
||||
// Run the post step callbacks
|
||||
// Loop because post step callbacks may create more post step callbacks
|
||||
while(space->postStepCallbacks){
|
||||
cpHashSet *callbacks = space->postStepCallbacks;
|
||||
space->postStepCallbacks = NULL;
|
||||
|
||||
cpHashSetEach(callbacks, (cpHashSetIterFunc)postStepCallbackSetIter, space);
|
||||
cpHashSetFree(callbacks);
|
||||
}
|
||||
|
||||
// Increment the stamp.
|
||||
space->stamp++;
|
||||
}
|
||||
Loading…
Reference in New Issue