/* 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 <assert.h>#include "chipmunk.h"#include "prime.h"static cpHandle*cpHandleAlloc(void){	return (cpHandle *)malloc(sizeof(cpHandle));}static cpHandle*cpHandleInit(cpHandle *hand, void *obj){	hand->obj = obj;	hand->retain = 0;	hand->stamp = 0;		return hand;}static cpHandle*cpHandleNew(void *obj){	return cpHandleInit(cpHandleAlloc(), obj);}static inline voidcpHandleRetain(cpHandle *hand){	hand->retain++;}static inline voidcpHandleFree(cpHandle *hand){	free(hand);}static inline voidcpHandleRelease(cpHandle *hand){	hand->retain--;	if(hand->retain == 0)		cpHandleFree(hand);}cpSpaceHash*cpSpaceHashAlloc(void){	return (cpSpaceHash *)calloc(1, sizeof(cpSpaceHash));}// Frees the old table, and allocates a new one.static voidcpSpaceHashAllocTable(cpSpaceHash *hash, int numcells){	free(hash->table);		hash->numcells = numcells;	hash->table = (cpSpaceHashBin **)calloc(numcells, sizeof(cpSpaceHashBin *));}// Equality function for the handleset.static inthandleSetEql(void *obj, void *elt){	cpHandle *hand = (cpHandle *)elt;	return (obj == hand->obj);}// Transformation function for the handleset.static void *handleSetTrans(void *obj, void *unused){	cpHandle *hand = cpHandleNew(obj);	cpHandleRetain(hand);		return hand;}cpSpaceHash*cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpaceHashBBFunc bbfunc){	cpSpaceHashAllocTable(hash, next_prime(numcells));	hash->celldim = celldim;	hash->bbfunc = bbfunc;		hash->bins = NULL;	hash->handleSet = cpHashSetNew(0, &handleSetEql, &handleSetTrans);		hash->stamp = 1;		return hash;}cpSpaceHash*cpSpaceHashNew(cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc){	return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc);}static inline voidclearHashCell(cpSpaceHash *hash, int index){	cpSpaceHashBin *bin = hash->table[index];	while(bin){		cpSpaceHashBin *next = bin->next;				// Release the lock on the handle.		cpHandleRelease(bin->handle);		// Recycle the bin.		bin->next = hash->bins;		hash->bins = bin;				bin = next;	}		hash->table[index] = NULL;}// Clear all cells in the hashtable.static voidclearHash(cpSpaceHash *hash){	for(int i=0; i<hash->numcells; i++)		clearHashCell(hash, i);}// Free the recycled hash bins.static voidfreeBins(cpSpaceHash *hash){	cpSpaceHashBin *bin = hash->bins;	while(bin){		cpSpaceHashBin *next = bin->next;		free(bin);		bin = next;	}}// Hashset iterator function to free the handles.static voidhandleFreeWrap(void *elt, void *unused){	cpHandle *hand = (cpHandle *)elt;	cpHandleFree(hand);}voidcpSpaceHashDestroy(cpSpaceHash *hash){	clearHash(hash);	freeBins(hash);		// Free the handles.	cpHashSetEach(hash->handleSet, &handleFreeWrap, NULL);	cpHashSetFree(hash->handleSet);		free(hash->table);}voidcpSpaceHashFree(cpSpaceHash *hash){	if(!hash) return;	cpSpaceHashDestroy(hash);	free(hash);}voidcpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells){	// Clear the hash to release the old handle locks.	clearHash(hash);		hash->celldim = celldim;	cpSpaceHashAllocTable(hash, next_prime(numcells));}// Return true if the chain contains the handle.static inline intcontainsHandle(cpSpaceHashBin *bin, cpHandle *hand){	while(bin){		if(bin->handle == hand) return 1;		bin = bin->next;	}		return 0;}// Get a recycled or new bin.static inline cpSpaceHashBin *getEmptyBin(cpSpaceHash *hash){	cpSpaceHashBin *bin = hash->bins;		// Make a new one if necessary.	if(bin == NULL) return (cpSpaceHashBin *)malloc(sizeof(cpSpaceHashBin));	hash->bins = bin->next;	return bin;}// The hash function itself.static inline unsigned inthash_func(unsigned int x, unsigned int y, unsigned int n){	return (x*2185031351ul ^ y*4232417593ul) % n;}static inline voidhashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb){	// Find the dimensions in cell coordinates.	cpFloat dim = hash->celldim;	int l = bb.l/dim;	int r = bb.r/dim;	int b = bb.b/dim;	int t = bb.t/dim;		int n = hash->numcells;	for(int i=l; i<=r; i++){		for(int j=b; j<=t; j++){			int index = hash_func(i,j,n);			cpSpaceHashBin *bin = hash->table[index];						// 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[index] = newBin;		}	}}voidcpSpaceHashInsert(cpSpaceHash *hash, void *obj, unsigned int id, cpBB bb){	cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, id, obj, NULL);	hashHandle(hash, hand, bb);}voidcpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, unsigned int id){	cpHandle *hand = (cpHandle *)cpHashSetFind(hash->handleSet, id, obj);	hashHandle(hash, hand, hash->bbfunc(obj));}// Hashset iterator function for rehashing the spatial hash. (hash hash hash hash?)static voidhandleRehashHelper(void *elt, void *data){	cpHandle *hand = (cpHandle *)elt;	cpSpaceHash *hash = (cpSpaceHash *)data;		hashHandle(hash, hand, hash->bbfunc(hand->obj));}voidcpSpaceHashRehash(cpSpaceHash *hash){	clearHash(hash);		// Rehash all of the handles.	cpHashSetEach(hash->handleSet, &handleRehashHelper, hash);}voidcpSpaceHashRemove(cpSpaceHash *hash, void *obj, unsigned int id){	cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, id, obj);		if(hand){		hand->obj = NULL;		cpHandleRelease(hand);	}}// Used by the cpSpaceHashEach() iterator.typedef struct eachPair {	cpSpaceHashIterator func;	void *data;} eachPair;// Calls the user iterator function. (Gross I know.)static voideachHelper(void *elt, void *data){	cpHandle *hand = (cpHandle *)elt;	eachPair *pair = (eachPair *)data;		pair->func(hand->obj, pair->data);}// Iterate over the objects in the spatial hash.voidcpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data){	// Bundle the callback up to send to the hashset iterator.	eachPair pair = {func, data};		cpHashSetEach(hash->handleSet, &eachHelper, &pair);}// Calls the callback function for the objects in a given chain.static inline voidquery(cpSpaceHash *hash, cpSpaceHashBin *bin, void *obj, cpSpaceHashQueryFunc func, void *data){	for(; bin; bin = bin->next){		cpHandle *hand = bin->handle;		void *other = hand->obj;				// Skip over certain conditions		if(			// Have we already tried this pair in this query?			hand->stamp == hash->stamp			// Is obj the same as other?			|| obj == other 			// Has other been removed since the last rehash?			|| !other			) continue;				func(obj, other, data);		// Stamp that the handle was checked already against this object.		hand->stamp = hash->stamp;	}}voidcpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func, void *data){	cpFloat dim = hash->celldim;	int index = hash_func((int)(point.x/dim), (int)(point.y/dim), hash->numcells);		query(hash, hash->table[index], &point, func, data);	// Increment the stamp.	// Only one cell is checked, but query() requires it anyway.	hash->stamp++;}voidcpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data){	// Get the dimensions in cell coordinates.	cpFloat dim = hash->celldim;	int l = bb.l/dim;	int r = bb.r/dim;	int b = bb.b/dim;	int t = bb.t/dim;		int n = hash->numcells;		// Iterate over the cells and query them.	for(int i=l; i<=r; i++){		for(int j=b; j<=t; j++){			int index = hash_func(i,j,n);			query(hash, hash->table[index], obj, func, data);		}	}		// Increment the stamp.	hash->stamp++;}// Similar to struct eachPair above.typedef struct queryRehashPair {	cpSpaceHash *hash;	cpSpaceHashQueryFunc func;	void *data;} queryRehashPair;// Hashset iterator func used with cpSpaceHashQueryRehash().static voidhandleQueryRehashHelper(void *elt, void *data){	cpHandle *hand = (cpHandle *)elt;		// Unpack the user callback data.	queryRehashPair *pair = (queryRehashPair *)data;	cpSpaceHash *hash = pair->hash;	cpSpaceHashQueryFunc func = pair->func;	cpFloat dim = hash->celldim;	int n = hash->numcells;	void *obj = hand->obj;	cpBB bb = hash->bbfunc(obj);	int l = bb.l/dim;	int r = bb.r/dim;	int b = bb.b/dim;	int t = bb.t/dim;	for(int i=l; i<=r; i++){		for(int j=b; j<=t; j++){			int index = hash_func(i,j,n);			cpSpaceHashBin *bin = hash->table[index];						if(containsHandle(bin, hand)) continue;			query(hash, bin, obj, func, pair->data);						cpHandleRetain(hand);			cpSpaceHashBin *newBin = getEmptyBin(hash);			newBin->handle = hand;			newBin->next = bin;			hash->table[index] = newBin;		}	}		// Increment the stamp for each object we hash.	hash->stamp++;}voidcpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data){	clearHash(hash);	queryRehashPair pair = {hash, func, data};	cpHashSetEach(hash->handleSet, &handleQueryRehashHelper, &pair);}