/*
  Copyright (c) 2015 Alex Diener
  
  This software is provided 'as-is', without any express or implied
  warranty. In no event will the authors be held liable for any damages
  arising from the use of this software.
  
  Permission is granted to anyone to use this software for any purpose,
  including commercial applications, and to alter it and redistribute it
  freely, subject to the following restrictions:
  
  1. The origin of this software must not be misrepresented; you must not
     claim that you wrote the original software. If you use this software
     in a product, an acknowledgment in the product documentation would be
     appreciated but is not required.
  2. Altered source versions must be plainly marked as such, and must not be
     misrepresented as being the original software.
  3. This notice may not be removed or altered from any source distribution.
  
  Alex Diener alex@ludobloom.com
*/

#include "collision/CollisionPairQueue.h"
#include "collision/CollisionResolver.h"
#include <assert.h>
#include <limits.h>
#include <stdio.h>
#include <stdlib.h>

#define stemobject_implementation CollisionResolver

stemobject_vtable_begin();
stemobject_vtable_entry(dispose);
stemobject_vtable_end();

#define MAX_ITERATIONS_TEMP 512
#define MAX_SIMULTANEOUS_COLLISIONS 128

CollisionResolver * CollisionResolver_create(IntersectionManager * intersectionManager, bool takeOwnership, void (* listObjectsCallback)(CollisionObjectIO * objectIO, void * context), void * callbackContext) {
	stemobject_create_implementation(init, intersectionManager, takeOwnership, listObjectsCallback, callbackContext)
}

bool CollisionResolver_init(CollisionResolver * self, IntersectionManager * intersectionManager, bool takeOwnership, void (* listObjectsCallback)(CollisionObjectIO * objectIO, void * context), void * callbackContext) {
	call_super(init, self);
	self->intersectionManager = intersectionManager;
	self->listObjectsCallback = listObjectsCallback;
	self->callbackContext = callbackContext;
	self->objectIO = CollisionObjectIO_create();
	self->resolutionFailureCallback = NULL;
	self->private_ivar(intersectionManagerOwned) = takeOwnership;
	self->private_ivar(inResolveAll) = false;
	self->private_ivar(pairQueue) = CollisionPairQueue_create();
	self->private_ivar(simultaneousCollisionBuffer) = malloc(sizeof(*self->private_ivar(simultaneousCollisionBuffer)) * MAX_SIMULTANEOUS_COLLISIONS);
	self->private_ivar(cycleDetectionBufferSize) = 128;
	self->private_ivar(cycleDetectionBufferCount) = 0;
	self->private_ivar(cycleDetectionBuffer) = malloc(sizeof(*self->private_ivar(cycleDetectionBuffer)) * self->private_ivar(cycleDetectionBufferSize) * 2);
	self->private_ivar(objectListDirtyValue) = 0;
	self->private_ivar(lastObjectListDirtyValue) = UINT_MAX;
	return true;
}

void CollisionResolver_dispose(CollisionResolver * self) {
	CollisionObjectIO_dispose(self->objectIO);
	free(self->private_ivar(simultaneousCollisionBuffer));
	if (self->private_ivar(intersectionManagerOwned)) {
		IntersectionManager_dispose(self->intersectionManager);
	}
	CollisionPairQueue_dispose(self->private_ivar(pairQueue));
	call_super(dispose, self);
}

void CollisionResolver_addObject(CollisionResolver * self, compat_type(CollisionObject *) object) {
	assert(object != NULL);
	for (size_t objectIndex = 0; objectIndex < self->objectIO->movingObjectCount; objectIndex++) {
		if (self->objectIO->movingObjects[objectIndex] == object) {
			return;
		}
	}
	for (size_t objectIndex = 0; objectIndex < self->objectIO->staticObjectCount; objectIndex++) {
		if (self->objectIO->staticObjects[objectIndex] == object) {
			return;
		}
	}
	CollisionObjectIO_addObject(self->objectIO, object);
	if (self->private_ivar(inResolveAll)) {
		CollisionPairQueue_addNextObject(self->private_ivar(pairQueue), object);
	}
}

void CollisionResolver_removeObject(CollisionResolver * self, compat_type(CollisionObject *) object) {
	for (size_t objectIndex = 0; objectIndex < self->objectIO->movingObjectCount; objectIndex++) {
		if (self->objectIO->movingObjects[objectIndex] == object) {
			if (self->private_ivar(inResolveAll)) {
				self->objectIO->movingObjects[objectIndex]->private_ivar(markedForRemoval) = true;
				
			} else {
				self->objectIO->movingObjectCount--;
				for (; objectIndex < self->objectIO->movingObjectCount; objectIndex++) {
					self->objectIO->movingObjects[objectIndex] = self->objectIO->movingObjects[objectIndex + 1];
				}
			}
			return;
		}
	}
	for (size_t objectIndex = 0; objectIndex < self->objectIO->staticObjectCount; objectIndex++) {
		if (self->objectIO->staticObjects[objectIndex] == object) {
			if (self->private_ivar(inResolveAll)) {
				self->objectIO->staticObjects[objectIndex]->private_ivar(markedForRemoval) = true;
				
			} else {
				self->objectIO->staticObjectCount--;
				for (; objectIndex < self->objectIO->staticObjectCount; objectIndex++) {
					self->objectIO->staticObjects[objectIndex] = self->objectIO->staticObjects[objectIndex + 1];
				}
			}
			return;
		}
	}
}

static bool CollisionResolver_queryPairInternal(CollisionResolver * self, CollisionObject * object1, CollisionObject * object2, CollisionRecord * outCollision) {
	if (object1->private_ivar(markedForRemoval) || object2->private_ivar(markedForRemoval) ||
	    (object1->private_ivar(unresolvable) && object2->private_ivar(unresolvable)) ||
	    (object1->collidableMask & object2->ownMask) == 0 ||
	    (object2->collidableMask & object1->ownMask) == 0) {
		return false;
	}
	
	Vector3x normal;
	fixed16_16 frameTime, contactArea;
	bool intersectionFound = false;
	CollisionObject_shapeType shapeType1 = call_virtual(getShapeType, object1);
	CollisionObject_shapeType shapeType2 = call_virtual(getShapeType, object2);
	IntersectionManager_collisionTestHandler handler = IntersectionManager_getCollisionTestHandler(self->intersectionManager, shapeType1, shapeType2);
	if (handler == NULL) {
		handler = IntersectionManager_getCollisionTestHandler(self->intersectionManager, shapeType2, shapeType1);
		if (handler == NULL) {
#ifdef DEBUG
			fprintf(stderr, "Warning: Collision objects of shapeType %d and %d have been added to CollisionResolver %p with no matching intersection handler for those types\n", shapeType1, shapeType2, self);
#endif
		} else {
			intersectionFound = handler(object2, object1, &frameTime, &normal, NULL, NULL, &contactArea);
			Vector3x_invert(&normal);
		}
	} else {
		intersectionFound = handler(object1, object2, &frameTime, &normal, NULL, NULL, &contactArea);
	}
	
	if (intersectionFound) {
		outCollision->object1 = object1;
		outCollision->object2 = object2;
		outCollision->normal = normal;
		outCollision->time = frameTime;
		outCollision->contactArea = contactArea;
		return true;
	}
	return false;
}

static void updateListedObjects(CollisionResolver * self) {
	if (self->listObjectsCallback != NULL && self->private_ivar(objectListDirtyValue) != self->private_ivar(lastObjectListDirtyValue)) {
		CollisionObjectIO_clearObjects(self->objectIO);
		self->listObjectsCallback(self->objectIO, self->callbackContext);
		self->private_ivar(lastObjectListDirtyValue) = self->private_ivar(objectListDirtyValue);
	}
}

bool CollisionResolver_querySingle(CollisionResolver * self, compat_type(CollisionObject *) object, CollisionRecord * outCollision) {
	updateListedObjects(self);
	CollisionObject * object1 = object;
	CollisionRecord bestCollision = {NULL, NULL, FIXED_16_16_MAX, 0, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	bool object1Stationary = call_virtual(isStationary, object1);
	Box6x object1CollisionBounds = call_virtual(getCollisionBounds, object1);
	for (size_t objectIndex = 0; objectIndex < self->objectIO->movingObjectCount; objectIndex++) {
		CollisionObject * object2 = self->objectIO->movingObjects[objectIndex];
		if (object2 == object1) {
			continue;
		}
		if (object1Stationary && call_virtual(isStationary, object2)) {
			continue;
		}
		if (!Box6x_intersectsBox6x(object1CollisionBounds, call_virtual(getCollisionBounds, object2))) {
			continue;
		}
		CollisionRecord collision;
		if (CollisionResolver_queryPairInternal(self, object1, object2, &collision) && collision.time < bestCollision.time) {
			bestCollision = collision;
		}
	}
	if (!object1Stationary) {
		for (size_t objectIndex = 0; objectIndex < self->objectIO->staticObjectCount; objectIndex++) {
			CollisionObject * object2 = self->objectIO->staticObjects[objectIndex];
			if (object2 == object1) {
				continue;
			}
			if (!Box6x_intersectsBox6x(object1CollisionBounds, call_virtual(getCollisionBounds, object2))) {
				continue;
			}
			CollisionRecord collision;
			if (CollisionResolver_queryPairInternal(self, object, self->objectIO->staticObjects[objectIndex], &collision) && collision.time < bestCollision.time) {
				bestCollision = collision;
			}
		}
	}
	
	if (bestCollision.object2 != NULL) {
		if (outCollision != NULL) {
			*outCollision = bestCollision;
		}
		return true;
	}
	
	return false;
}

static void sortByContactArea(CollisionRecord * collisions, size_t collisionCount) {
	for (size_t collisionIndex = 1; collisionIndex < collisionCount; collisionIndex++) {
		CollisionRecord collision = collisions[collisionIndex];
		for (size_t collisionIndex2 = collisionIndex; collisionIndex2 > 0 && collisions[collisionIndex2 - 1].contactArea < collision.contactArea; collisionIndex2--) {
			collisions[collisionIndex2] = collisions[collisionIndex2 - 1];
			collisions[collisionIndex2 - 1] = collision;
		}
	}
}

size_t CollisionResolver_findEarliest(CollisionResolver * self, CollisionRecord * outCollisions, size_t collisionCountMax) {
	if (collisionCountMax == 0) {
		return 0;
	}
	updateListedObjects(self);
	size_t collisionCount = 0;
	fixed16_16 bestTime = FIXED_16_16_MAX;
	for (unsigned int objectIndex = 0; objectIndex < self->objectIO->movingObjectCount; objectIndex++) {
		for (unsigned int objectIndex2 = objectIndex + 1; objectIndex2 < self->objectIO->movingObjectCount; objectIndex2++) {
			CollisionRecord collision;
			bool queryResult = CollisionResolver_queryPairInternal(self, self->objectIO->movingObjects[objectIndex], self->objectIO->movingObjects[objectIndex2], &collision);
			if (queryResult && collision.time <= bestTime) {
				if (collision.time < bestTime) {
					bestTime = collision.time;
					outCollisions[0] = collision;
					collisionCount = 1;
					
				} else {
					if (collisionCount < collisionCountMax) {
						outCollisions[collisionCount++] = collision;
					}
				}
			}
		}
		for (unsigned int objectIndex2 = objectIndex + 1; objectIndex2 < self->objectIO->staticObjectCount; objectIndex2++) {
			CollisionRecord collision;
			bool queryResult = CollisionResolver_queryPairInternal(self, self->objectIO->movingObjects[objectIndex], self->objectIO->staticObjects[objectIndex2], &collision);
			if (queryResult && collision.time <= bestTime) {
				if (collision.time < bestTime) {
					bestTime = collision.time;
					outCollisions[0] = collision;
					collisionCount = 1;
					
				} else {
					if (collisionCount < collisionCountMax) {
						outCollisions[collisionCount++] = collision;
					}
				}
			}
		}
	}
	sortByContactArea(outCollisions, collisionCount);
	
	return collisionCount;
}

static size_t findEarliestInQueue(CollisionResolver * self, CollisionRecord * outCollisions, size_t collisionCountMax) {
	size_t collisionCount = 0;
	fixed16_16 bestTime = FIXED_16_16_MAX;
	CollisionObject * object1, * object2;
	while (CollisionPairQueue_nextPair(self->private_ivar(pairQueue), &object1, &object2)) {
		CollisionRecord collision;
		bool queryResult = CollisionResolver_queryPairInternal(self, object1, object2, &collision);
		if (queryResult) {
			if (collision.time < bestTime) {
				bestTime = collision.time;
				outCollisions[0] = collision;
				collisionCount = 1;
				
			} else if (collision.time == bestTime) {
				if (collisionCount < collisionCountMax) {
					outCollisions[collisionCount++] = collision;
				}
			}
			CollisionPairQueue_addNextPair(self->private_ivar(pairQueue), collision.object1, collision.object2);
		}
	}
	sortByContactArea(outCollisions, collisionCount);
	
	return collisionCount;
}

static void resetCycleDetection(CollisionResolver * self) {
	self->private_ivar(cycleDetectionBufferCount) = 0;
}

static void resetObjectsMarkedUnresolvable(CollisionResolver * self) {
	for (unsigned int objectIndex = 0; objectIndex < self->objectIO->movingObjectCount; objectIndex++) {
		self->objectIO->movingObjects[objectIndex]->private_ivar(unresolvable) = false;
	}
	for (unsigned int objectIndex = 0; objectIndex < self->objectIO->staticObjectCount; objectIndex++) {
		self->objectIO->staticObjects[objectIndex]->private_ivar(unresolvable) = false;
	}
}

static void pushObjectPairToCycleBuffer(CollisionResolver * self, CollisionObject * object1, CollisionObject * object2) {
	if (self->private_ivar(cycleDetectionBufferSize) <= self->private_ivar(cycleDetectionBufferCount)) {
		self->private_ivar(cycleDetectionBufferSize) *= 2;
		self->private_ivar(cycleDetectionBuffer) = realloc(self->private_ivar(cycleDetectionBuffer), sizeof(*self->private_ivar(cycleDetectionBuffer)) * self->private_ivar(cycleDetectionBufferSize) * 2);
	}
	self->private_ivar(cycleDetectionBuffer)[self->private_ivar(cycleDetectionBufferCount) * 2] = object1;
	self->private_ivar(cycleDetectionBuffer)[self->private_ivar(cycleDetectionBufferCount) * 2 + 1] = object2;
	self->private_ivar(cycleDetectionBufferCount)++;
}

static void checkForCycles(CollisionResolver * self) {
	for (unsigned int objectPairIndex = 0; objectPairIndex < self->private_ivar(cycleDetectionBufferCount); objectPairIndex++) {
		bool matchFound = false;
		for (unsigned int objectPairIndex2 = objectPairIndex + 1; objectPairIndex2 < self->private_ivar(cycleDetectionBufferCount); objectPairIndex2++) {
			if (self->private_ivar(cycleDetectionBuffer)[objectPairIndex * 2 + 0] == self->private_ivar(cycleDetectionBuffer)[objectPairIndex2 * 2 + 0] &&
			    self->private_ivar(cycleDetectionBuffer)[objectPairIndex * 2 + 1] == self->private_ivar(cycleDetectionBuffer)[objectPairIndex2 * 2 + 1]) {
				matchFound = true;
				break;
			}
		}
		if (matchFound) {
			if (self->resolutionFailureCallback != NULL) {
				self->resolutionFailureCallback(self->private_ivar(cycleDetectionBuffer)[objectPairIndex * 2 + 0], self->private_ivar(cycleDetectionBuffer)[objectPairIndex * 2 + 1]);
			}
			self->private_ivar(cycleDetectionBuffer)[objectPairIndex * 2 + 0]->private_ivar(unresolvable) = true;
			self->private_ivar(cycleDetectionBuffer)[objectPairIndex * 2 + 1]->private_ivar(unresolvable) = true;
			continue;
		}
	}
}

static void performOverlapCheck(CollisionResolver * self, CollisionObject * object1, CollisionObject * object2) {
	if ((object1->overlapCallback != NULL ||
	     object2->overlapCallback != NULL) &&
	    (object1->overlapMask & object2->ownMask) != 0 &&
	    (object2->overlapMask & object1->ownMask) != 0) {
		CollisionObject_shapeType shapeType1 = call_virtual(getShapeType, object1);
		CollisionObject_shapeType shapeType2 = call_virtual(getShapeType, object2);
		IntersectionManager_overlapTestHandler handler = IntersectionManager_getOverlapTestHandler(self->intersectionManager, shapeType1, shapeType2);
		if (handler == NULL) {
			handler = IntersectionManager_getOverlapTestHandler(self->intersectionManager, shapeType2, shapeType1);
			if (handler == NULL) {
				return;
			}
		}
		if (handler(object1, object2)) {
			if (object1->overlapCallback != NULL) {
				object1->overlapCallback(object1, object2);
			}
			if (object2->overlapCallback != NULL) {
				object2->overlapCallback(object2, object1);
			}
		}
	}
}

static CollisionRecord invertCollisionRecord(CollisionRecord collision) {
	CollisionObject * swapObject = collision.object1;
	collision.object1 = collision.object2;
	collision.object2 = swapObject;
	Vector3x_invert(&collision.normal);
	Vector3x swapVector = collision.object1Vector;
	collision.object1Vector = collision.object2Vector;
	collision.object2Vector = swapVector;
	return collision;
}

static void resolveCollision(CollisionRecord collision, fixed16_16 timesliceSize, fixed16_16 subframeTime) {
	if (collision.object1->collisionCallback != NULL) {
		collision.object1->collisionCallback(collision, timesliceSize, subframeTime);
	}
	if (collision.object2->collisionCallback != NULL) {
		collision.object2->collisionCallback(invertCollisionRecord(collision), timesliceSize, subframeTime);
	}
}

void CollisionResolver_resolveAll(CollisionResolver * self) {
	updateListedObjects(self);
	
	fixed16_16 timesliceRemaining = 0x10000;
	unsigned int iterationCount = 0;
	self->private_ivar(inResolveAll) = true;
	
	CollisionPairQueue_addInitialPairs(self->private_ivar(pairQueue), self->objectIO->movingObjects, self->objectIO->movingObjectCount, self->objectIO->staticObjects, self->objectIO->staticObjectCount);
	size_t collisionCount;
	while ((collisionCount = findEarliestInQueue(self, self->private_ivar(simultaneousCollisionBuffer), MAX_SIMULTANEOUS_COLLISIONS)) > 0) {
		fixed16_16 collisionTime = self->private_ivar(simultaneousCollisionBuffer)[0].time;
		fixed16_16 nextTimeslice = xmul(0x10000 - collisionTime, timesliceRemaining);
		
		if (collisionTime > 0) {
			resetCycleDetection(self);
			resetObjectsMarkedUnresolvable(self);
			for (size_t objectIndex = 0; objectIndex < self->objectIO->movingObjectCount; objectIndex++) {
				call_virtual(interpolate, self->objectIO->movingObjects[objectIndex], collisionTime);
			}
		} else {
			checkForCycles(self);
		}
		
		for (size_t collisionIndex = 0; collisionIndex < collisionCount; collisionIndex++) {
			bool objectAlreadyCollided = false;
			
			// If an object has already been resolved during a higher-priority simultaneous collision at this timepoint, boot it to the next iteration
			for (size_t collisionIndex2 = 0; collisionIndex2 < collisionIndex; collisionIndex2++) {
				if (self->private_ivar(simultaneousCollisionBuffer)[collisionIndex2].object1 == self->private_ivar(simultaneousCollisionBuffer)[collisionIndex].object1 ||
				    self->private_ivar(simultaneousCollisionBuffer)[collisionIndex2].object1 == self->private_ivar(simultaneousCollisionBuffer)[collisionIndex].object2 ||
				    self->private_ivar(simultaneousCollisionBuffer)[collisionIndex2].object2 == self->private_ivar(simultaneousCollisionBuffer)[collisionIndex].object1 ||
				    self->private_ivar(simultaneousCollisionBuffer)[collisionIndex2].object2 == self->private_ivar(simultaneousCollisionBuffer)[collisionIndex].object2) {
					objectAlreadyCollided = true;
					break;
				}
			}
			
			if (!objectAlreadyCollided) {
				if (collisionTime == 0) {
					pushObjectPairToCycleBuffer(self, self->private_ivar(simultaneousCollisionBuffer)[collisionIndex].object1, self->private_ivar(simultaneousCollisionBuffer)[collisionIndex].object2);
				}
				resolveCollision(self->private_ivar(simultaneousCollisionBuffer)[collisionIndex], timesliceRemaining, 0x10000 - nextTimeslice);
			}
			CollisionPairQueue_addNextObject(self->private_ivar(pairQueue), self->private_ivar(simultaneousCollisionBuffer)[collisionIndex].object1);
			CollisionPairQueue_addNextObject(self->private_ivar(pairQueue), self->private_ivar(simultaneousCollisionBuffer)[collisionIndex].object2);
		}
		
		timesliceRemaining = nextTimeslice;
		
		iterationCount++;
		if (!CollisionPairQueue_nextIteration(self->private_ivar(pairQueue), self->objectIO->movingObjects, self->objectIO->movingObjectCount, self->objectIO->staticObjects, self->objectIO->staticObjectCount)) {
			break;
		}
		if (iterationCount >= MAX_ITERATIONS_TEMP) {
			fprintf(stderr, "Warning: Max iterations (%u) exceeded\n", MAX_ITERATIONS_TEMP);
			break;
		}
	}
	resetCycleDetection(self);
	resetObjectsMarkedUnresolvable(self);
	self->private_ivar(inResolveAll) = false;
	
	for (size_t objectIndex = 0; objectIndex < self->objectIO->movingObjectCount; objectIndex++) {
		CollisionObject * object1 = self->objectIO->movingObjects[objectIndex];
		for (size_t objectIndex2 = objectIndex + 1; objectIndex2 < self->objectIO->movingObjectCount; objectIndex2++) {
			CollisionObject * object2 = self->objectIO->movingObjects[objectIndex2];
			performOverlapCheck(self, object1, object2);
		}
		for (size_t objectIndex2 = 0; objectIndex2 < self->objectIO->staticObjectCount; objectIndex2++) {
			CollisionObject * object2 = self->objectIO->staticObjects[objectIndex2];
			performOverlapCheck(self, object1, object2);
		}
	}
	
	if (self->listObjectsCallback == NULL) {
		size_t removedObjectCount = 0;
		for (size_t objectIndex = 0; objectIndex < self->objectIO->movingObjectCount; objectIndex++) {
			self->objectIO->movingObjects[objectIndex] = self->objectIO->movingObjects[objectIndex + removedObjectCount];
			if (self->objectIO->movingObjects[objectIndex]->private_ivar(markedForRemoval)) {
				objectIndex--;
				self->objectIO->movingObjectCount--;
				removedObjectCount++;
			}
		}
		removedObjectCount = 0;
		for (size_t objectIndex = 0; objectIndex < self->objectIO->staticObjectCount; objectIndex++) {
			self->objectIO->staticObjects[objectIndex] = self->objectIO->staticObjects[objectIndex + removedObjectCount];
			if (self->objectIO->staticObjects[objectIndex]->private_ivar(markedForRemoval)) {
				objectIndex--;
				self->objectIO->staticObjectCount--;
				removedObjectCount++;
			}
		}
	}
}

static bool Box6x_intersectsBox6xNonEmpty(Box6x box1, Box6x box2) {
	return (box1.xMax >= box2.xMin && box1.xMin <= box2.xMax &&
	        box1.yMax >= box2.yMin && box1.yMin <= box2.yMax &&
	        box1.zMax >= box2.zMin && box1.zMin <= box2.zMax);
}

static fixed16_16 performRaycastHitTest(CollisionResolver * self, CollisionObject * object, Vector3x rayStart, Vector3x rayEnd, Box6x rayBounds, uint32_t objectMask) {
	if ((object->ownMask & objectMask) && Box6x_intersectsBox6xNonEmpty(rayBounds, call_virtual(getCollisionBounds, object))) {
		CollisionObject_shapeType shapeType = call_virtual(getShapeType, object);
		IntersectionManager_raycastTestHandler handler = IntersectionManager_getRaycastTestHandler(self->intersectionManager, shapeType);
		if (handler != NULL) {
			return handler(object, rayStart, rayEnd);
		}
	}
	return FIXED_16_16_INF;
}

static int sortRaycastResults(const void * lhsUntyped, const void * rhsUntyped) {
	const CollisionRaycastResult * lhs = lhsUntyped, * rhs = rhsUntyped;
	return (lhs->distance > rhs->distance) * 2 - 1;
}

unsigned int CollisionResolver_listRaycastHits(CollisionResolver * self, Vector3x rayStart, Vector3x rayEnd, uint32_t objectMask, unsigned int resultCountMax, CollisionRaycastResult * outResults) {
	updateListedObjects(self);
	unsigned int resultCount = 0;
	CollisionRaycastResult results[self->objectIO->staticObjectCount + self->objectIO->movingObjectCount];
	Box6x rayBounds = Box6x_enclosingPoints(rayStart, rayEnd);
	
	for (unsigned int objectIndex = 0; objectIndex < self->objectIO->staticObjectCount; objectIndex++) {
		fixed16_16 distance = performRaycastHitTest(self, self->objectIO->staticObjects[objectIndex], rayStart, rayEnd, rayBounds, objectMask);
		if (distance != FIXED_16_16_INF) {
			results[resultCount++] = (CollisionRaycastResult) {self->objectIO->staticObjects[objectIndex], distance};
		}
	}
	for (unsigned int objectIndex = 0; objectIndex < self->objectIO->movingObjectCount; objectIndex++) {
		fixed16_16 distance = performRaycastHitTest(self, self->objectIO->movingObjects[objectIndex], rayStart, rayEnd, rayBounds, objectMask);
		if (distance != FIXED_16_16_INF) {
			results[resultCount++] = (CollisionRaycastResult) {self->objectIO->movingObjects[objectIndex], distance};
		}
	}
	qsort(results, resultCount, sizeof(results[0]), sortRaycastResults);
	unsigned int returnedResultCount = resultCount > resultCountMax ? resultCountMax : resultCount;
	memcpy(outResults, results, returnedResultCount * sizeof(results[0]));
	return resultCount;
}

void CollisionResolver_dirtyObjectList(CollisionResolver * self) {
	self->private_ivar(objectListDirtyValue)++;
}