hush
This commit is contained in:
Jan Racek
468
src/collision.cpp
Normal file
468
src/collision.cpp
Normal file
@@ -0,0 +1,468 @@
|
||||
#include "collision.hh"
|
||||
#include "scenemanager.hh"
|
||||
|
||||
#include <psyqo/fixed-point.hh>
|
||||
|
||||
// Helper type alias for brevity
|
||||
using FP = psyqo::FixedPoint<12>;
|
||||
|
||||
namespace psxsplash {
|
||||
|
||||
// Static member initialization
|
||||
psyqo::FixedPoint<12> SpatialGrid::WORLD_MIN = FP(-16);
|
||||
psyqo::FixedPoint<12> SpatialGrid::WORLD_MAX = FP(16);
|
||||
psyqo::FixedPoint<12> SpatialGrid::CELL_SIZE = FP(4); // (32 / 8) = 4
|
||||
|
||||
// AABB expand implementation
|
||||
void AABB::expand(const psyqo::Vec3& delta) {
|
||||
psyqo::FixedPoint<12> zero;
|
||||
if (delta.x > zero) max.x = max.x + delta.x;
|
||||
else min.x = min.x + delta.x;
|
||||
if (delta.y > zero) max.y = max.y + delta.y;
|
||||
else min.y = min.y + delta.y;
|
||||
if (delta.z > zero) max.z = max.z + delta.z;
|
||||
else min.z = min.z + delta.z;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// SpatialGrid Implementation
|
||||
// ============================================================================
|
||||
|
||||
void SpatialGrid::clear() {
|
||||
for (int i = 0; i < CELL_COUNT; i++) {
|
||||
m_cells[i].count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void SpatialGrid::worldToGrid(const psyqo::Vec3& pos, int& gx, int& gy, int& gz) const {
|
||||
// Clamp position to world bounds
|
||||
auto px = pos.x;
|
||||
auto py = pos.y;
|
||||
auto pz = pos.z;
|
||||
|
||||
if (px < WORLD_MIN) px = WORLD_MIN;
|
||||
if (px > WORLD_MAX) px = WORLD_MAX;
|
||||
if (py < WORLD_MIN) py = WORLD_MIN;
|
||||
if (py > WORLD_MAX) py = WORLD_MAX;
|
||||
if (pz < WORLD_MIN) pz = WORLD_MIN;
|
||||
if (pz > WORLD_MAX) pz = WORLD_MAX;
|
||||
|
||||
// Convert to grid coordinates (0 to GRID_SIZE-1)
|
||||
// Using integer division after scaling
|
||||
gx = ((px - WORLD_MIN) / CELL_SIZE).integer();
|
||||
gy = ((py - WORLD_MIN) / CELL_SIZE).integer();
|
||||
gz = ((pz - WORLD_MIN) / CELL_SIZE).integer();
|
||||
|
||||
// Clamp to valid range
|
||||
if (gx < 0) gx = 0;
|
||||
if (gx >= GRID_SIZE) gx = GRID_SIZE - 1;
|
||||
if (gy < 0) gy = 0;
|
||||
if (gy >= GRID_SIZE) gy = GRID_SIZE - 1;
|
||||
if (gz < 0) gz = 0;
|
||||
if (gz >= GRID_SIZE) gz = GRID_SIZE - 1;
|
||||
}
|
||||
|
||||
int SpatialGrid::getCellIndex(const psyqo::Vec3& pos) const {
|
||||
int gx, gy, gz;
|
||||
worldToGrid(pos, gx, gy, gz);
|
||||
return gx + gy * GRID_SIZE + gz * GRID_SIZE * GRID_SIZE;
|
||||
}
|
||||
|
||||
void SpatialGrid::insert(uint16_t objectIndex, const AABB& bounds) {
|
||||
// Get grid range for this AABB
|
||||
int minGx, minGy, minGz;
|
||||
int maxGx, maxGy, maxGz;
|
||||
|
||||
worldToGrid(bounds.min, minGx, minGy, minGz);
|
||||
worldToGrid(bounds.max, maxGx, maxGy, maxGz);
|
||||
|
||||
// Insert into all overlapping cells
|
||||
for (int gz = minGz; gz <= maxGz; gz++) {
|
||||
for (int gy = minGy; gy <= maxGy; gy++) {
|
||||
for (int gx = minGx; gx <= maxGx; gx++) {
|
||||
int cellIndex = gx + gy * GRID_SIZE + gz * GRID_SIZE * GRID_SIZE;
|
||||
Cell& cell = m_cells[cellIndex];
|
||||
|
||||
if (cell.count < MAX_OBJECTS_PER_CELL) {
|
||||
cell.objectIndices[cell.count++] = objectIndex;
|
||||
}
|
||||
// If cell is full, object won't be in this cell (may miss collisions)
|
||||
// This is a tradeoff for memory/performance
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int SpatialGrid::queryAABB(const AABB& bounds, uint16_t* output, int maxResults) const {
|
||||
int resultCount = 0;
|
||||
|
||||
// Get grid range for query AABB
|
||||
int minGx, minGy, minGz;
|
||||
int maxGx, maxGy, maxGz;
|
||||
|
||||
worldToGrid(bounds.min, minGx, minGy, minGz);
|
||||
worldToGrid(bounds.max, maxGx, maxGy, maxGz);
|
||||
|
||||
// Track which objects we've already added (two 32-bit masks for objects 0-63)
|
||||
uint32_t addedMaskLow = 0; // Objects 0-31
|
||||
uint32_t addedMaskHigh = 0; // Objects 32-63
|
||||
|
||||
// Query all overlapping cells
|
||||
for (int gz = minGz; gz <= maxGz; gz++) {
|
||||
for (int gy = minGy; gy <= maxGy; gy++) {
|
||||
for (int gx = minGx; gx <= maxGx; gx++) {
|
||||
int cellIndex = gx + gy * GRID_SIZE + gz * GRID_SIZE * GRID_SIZE;
|
||||
const Cell& cell = m_cells[cellIndex];
|
||||
|
||||
for (int i = 0; i < cell.count; i++) {
|
||||
uint16_t objIndex = cell.objectIndices[i];
|
||||
|
||||
// Skip if already added (using bitmask for objects 0-63)
|
||||
if (objIndex < 32) {
|
||||
uint32_t bit = 1U << objIndex;
|
||||
if (addedMaskLow & bit) continue;
|
||||
addedMaskLow |= bit;
|
||||
} else if (objIndex < 64) {
|
||||
uint32_t bit = 1U << (objIndex - 32);
|
||||
if (addedMaskHigh & bit) continue;
|
||||
addedMaskHigh |= bit;
|
||||
}
|
||||
|
||||
if (resultCount < maxResults) {
|
||||
output[resultCount++] = objIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return resultCount;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// CollisionSystem Implementation
|
||||
// ============================================================================
|
||||
|
||||
void CollisionSystem::init() {
|
||||
reset();
|
||||
}
|
||||
|
||||
void CollisionSystem::reset() {
|
||||
m_colliderCount = 0;
|
||||
m_resultCount = 0;
|
||||
m_triggerPairCount = 0;
|
||||
m_grid.clear();
|
||||
}
|
||||
|
||||
void CollisionSystem::registerCollider(uint16_t gameObjectIndex, const AABB& localBounds,
|
||||
CollisionType type, CollisionMask mask) {
|
||||
if (m_colliderCount >= MAX_COLLIDERS) {
|
||||
// Out of collider slots
|
||||
return;
|
||||
}
|
||||
|
||||
CollisionData& data = m_colliders[m_colliderCount++];
|
||||
data.bounds = localBounds; // Will be transformed in updateCollider
|
||||
data.type = type;
|
||||
data.layerMask = mask;
|
||||
data.flags = 0;
|
||||
data.gridCell = 0;
|
||||
data.gameObjectIndex = gameObjectIndex;
|
||||
}
|
||||
|
||||
void CollisionSystem::updateCollider(uint16_t gameObjectIndex, const psyqo::Vec3& position,
|
||||
const psyqo::Matrix33& rotation) {
|
||||
// Find the collider for this object
|
||||
for (int i = 0; i < m_colliderCount; i++) {
|
||||
if (m_colliders[i].gameObjectIndex == gameObjectIndex) {
|
||||
// For now, just translate the AABB (no rotation support for AABBs)
|
||||
// TODO: Compute rotated AABB if needed
|
||||
|
||||
// Store original local bounds somewhere if we need to recalculate
|
||||
// For now, assume bounds are already world-relative
|
||||
m_colliders[i].bounds.min = m_colliders[i].bounds.min + position;
|
||||
m_colliders[i].bounds.max = m_colliders[i].bounds.max + position;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int CollisionSystem::detectCollisions() {
|
||||
m_resultCount = 0;
|
||||
|
||||
// Clear and rebuild spatial grid
|
||||
m_grid.clear();
|
||||
for (int i = 0; i < m_colliderCount; i++) {
|
||||
m_grid.insert(i, m_colliders[i].bounds);
|
||||
}
|
||||
|
||||
// Check each collider against potential colliders from grid
|
||||
for (int i = 0; i < m_colliderCount; i++) {
|
||||
const CollisionData& colliderA = m_colliders[i];
|
||||
|
||||
// Skip if no collision type
|
||||
if (colliderA.type == CollisionType::None) continue;
|
||||
|
||||
// Query spatial grid for nearby objects
|
||||
uint16_t nearby[32];
|
||||
int nearbyCount = m_grid.queryAABB(colliderA.bounds, nearby, 32);
|
||||
|
||||
for (int j = 0; j < nearbyCount; j++) {
|
||||
int otherIndex = nearby[j];
|
||||
|
||||
// Skip self
|
||||
if (otherIndex == i) continue;
|
||||
|
||||
// Skip if already processed (only process pairs once)
|
||||
if (otherIndex < i) continue;
|
||||
|
||||
const CollisionData& colliderB = m_colliders[otherIndex];
|
||||
|
||||
// Skip if no collision type
|
||||
if (colliderB.type == CollisionType::None) continue;
|
||||
|
||||
// Check layer masks
|
||||
if ((colliderA.layerMask & colliderB.layerMask) == 0) continue;
|
||||
|
||||
// Narrowphase AABB test
|
||||
psyqo::Vec3 normal;
|
||||
psyqo::FixedPoint<12> penetration;
|
||||
|
||||
if (testAABB(colliderA.bounds, colliderB.bounds, normal, penetration)) {
|
||||
// Collision detected
|
||||
if (m_resultCount < MAX_COLLISION_RESULTS) {
|
||||
CollisionResult& result = m_results[m_resultCount++];
|
||||
result.objectA = colliderA.gameObjectIndex;
|
||||
result.objectB = colliderB.gameObjectIndex;
|
||||
result.normal = normal;
|
||||
result.penetration = penetration;
|
||||
}
|
||||
|
||||
// Handle triggers
|
||||
if (colliderA.type == CollisionType::Trigger) {
|
||||
updateTriggerState(i, otherIndex, true);
|
||||
}
|
||||
if (colliderB.type == CollisionType::Trigger) {
|
||||
updateTriggerState(otherIndex, i, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Update trigger pairs that are no longer overlapping
|
||||
for (int i = 0; i < m_triggerPairCount; i++) {
|
||||
TriggerPair& pair = m_triggerPairs[i];
|
||||
pair.framesSinceContact++;
|
||||
|
||||
// If no contact for several frames, trigger exit
|
||||
if (pair.framesSinceContact > 2 && pair.state != 2) {
|
||||
pair.state = 2; // Exiting
|
||||
}
|
||||
}
|
||||
|
||||
return m_resultCount;
|
||||
}
|
||||
|
||||
bool CollisionSystem::testAABB(const AABB& a, const AABB& b,
|
||||
psyqo::Vec3& normal, psyqo::FixedPoint<12>& penetration) const {
|
||||
// Check for overlap on all axes
|
||||
if (a.max.x < b.min.x || a.min.x > b.max.x) return false;
|
||||
if (a.max.y < b.min.y || a.min.y > b.max.y) return false;
|
||||
if (a.max.z < b.min.z || a.min.z > b.max.z) return false;
|
||||
|
||||
// Calculate penetration on each axis
|
||||
auto overlapX1 = a.max.x - b.min.x;
|
||||
auto overlapX2 = b.max.x - a.min.x;
|
||||
auto overlapY1 = a.max.y - b.min.y;
|
||||
auto overlapY2 = b.max.y - a.min.y;
|
||||
auto overlapZ1 = a.max.z - b.min.z;
|
||||
auto overlapZ2 = b.max.z - a.min.z;
|
||||
|
||||
// Find minimum overlap axis
|
||||
auto minOverlapX = (overlapX1 < overlapX2) ? overlapX1 : overlapX2;
|
||||
auto minOverlapY = (overlapY1 < overlapY2) ? overlapY1 : overlapY2;
|
||||
auto minOverlapZ = (overlapZ1 < overlapZ2) ? overlapZ1 : overlapZ2;
|
||||
|
||||
// Constants for normals
|
||||
const FP zero(0);
|
||||
const FP one(1);
|
||||
const FP negOne(-1);
|
||||
|
||||
// Determine separation axis (axis with least penetration)
|
||||
if (minOverlapX <= minOverlapY && minOverlapX <= minOverlapZ) {
|
||||
penetration = minOverlapX;
|
||||
normal = psyqo::Vec3{(overlapX1 < overlapX2) ? negOne : one, zero, zero};
|
||||
} else if (minOverlapY <= minOverlapZ) {
|
||||
penetration = minOverlapY;
|
||||
normal = psyqo::Vec3{zero, (overlapY1 < overlapY2) ? negOne : one, zero};
|
||||
} else {
|
||||
penetration = minOverlapZ;
|
||||
normal = psyqo::Vec3{zero, zero, (overlapZ1 < overlapZ2) ? negOne : one};
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void CollisionSystem::updateTriggerState(uint16_t triggerIndex, uint16_t otherIndex, bool isOverlapping) {
|
||||
// Look for existing pair
|
||||
for (int i = 0; i < m_triggerPairCount; i++) {
|
||||
TriggerPair& pair = m_triggerPairs[i];
|
||||
if (pair.triggerIndex == triggerIndex && pair.otherIndex == otherIndex) {
|
||||
if (isOverlapping) {
|
||||
pair.framesSinceContact = 0;
|
||||
if (pair.state == 0) {
|
||||
pair.state = 1; // Now staying
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// New pair - add it
|
||||
if (isOverlapping && m_triggerPairCount < MAX_TRIGGERS) {
|
||||
TriggerPair& pair = m_triggerPairs[m_triggerPairCount++];
|
||||
pair.triggerIndex = triggerIndex;
|
||||
pair.otherIndex = otherIndex;
|
||||
pair.framesSinceContact = 0;
|
||||
pair.state = 0; // New (enter event)
|
||||
}
|
||||
}
|
||||
|
||||
bool CollisionSystem::areColliding(uint16_t indexA, uint16_t indexB) const {
|
||||
for (int i = 0; i < m_resultCount; i++) {
|
||||
if ((m_results[i].objectA == indexA && m_results[i].objectB == indexB) ||
|
||||
(m_results[i].objectA == indexB && m_results[i].objectB == indexA)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool CollisionSystem::raycast(const psyqo::Vec3& origin, const psyqo::Vec3& direction,
|
||||
psyqo::FixedPoint<12> maxDistance,
|
||||
psyqo::Vec3& hitPoint, psyqo::Vec3& hitNormal,
|
||||
uint16_t& hitObjectIndex) const {
|
||||
// Simple brute-force raycast against all colliders
|
||||
// TODO: Use spatial grid for optimization
|
||||
|
||||
auto closestT = maxDistance;
|
||||
bool hit = false;
|
||||
|
||||
// Fixed-point constants
|
||||
const FP zero(0);
|
||||
const FP one(1);
|
||||
const FP negOne(-1);
|
||||
const FP largeVal(1000);
|
||||
const FP negLargeVal(-1000);
|
||||
FP epsilon;
|
||||
epsilon.value = 4; // ~0.001 in 20.12 fixed point
|
||||
|
||||
for (int i = 0; i < m_colliderCount; i++) {
|
||||
const CollisionData& collider = m_colliders[i];
|
||||
if (collider.type == CollisionType::None) continue;
|
||||
|
||||
// Ray-AABB intersection test (slab method)
|
||||
const AABB& box = collider.bounds;
|
||||
|
||||
auto tMin = negLargeVal;
|
||||
auto tMax = largeVal;
|
||||
|
||||
// X slab
|
||||
if (direction.x != zero) {
|
||||
auto invD = one / direction.x;
|
||||
auto t1 = (box.min.x - origin.x) * invD;
|
||||
auto t2 = (box.max.x - origin.x) * invD;
|
||||
if (t1 > t2) { auto tmp = t1; t1 = t2; t2 = tmp; }
|
||||
if (t1 > tMin) tMin = t1;
|
||||
if (t2 < tMax) tMax = t2;
|
||||
} else if (origin.x < box.min.x || origin.x > box.max.x) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Y slab
|
||||
if (direction.y != zero) {
|
||||
auto invD = one / direction.y;
|
||||
auto t1 = (box.min.y - origin.y) * invD;
|
||||
auto t2 = (box.max.y - origin.y) * invD;
|
||||
if (t1 > t2) { auto tmp = t1; t1 = t2; t2 = tmp; }
|
||||
if (t1 > tMin) tMin = t1;
|
||||
if (t2 < tMax) tMax = t2;
|
||||
} else if (origin.y < box.min.y || origin.y > box.max.y) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Z slab
|
||||
if (direction.z != zero) {
|
||||
auto invD = one / direction.z;
|
||||
auto t1 = (box.min.z - origin.z) * invD;
|
||||
auto t2 = (box.max.z - origin.z) * invD;
|
||||
if (t1 > t2) { auto tmp = t1; t1 = t2; t2 = tmp; }
|
||||
if (t1 > tMin) tMin = t1;
|
||||
if (t2 < tMax) tMax = t2;
|
||||
} else if (origin.z < box.min.z || origin.z > box.max.z) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (tMin > tMax || tMax < zero) continue;
|
||||
|
||||
auto t = (tMin >= zero) ? tMin : tMax;
|
||||
|
||||
if (t < closestT && t >= zero) {
|
||||
closestT = t;
|
||||
hitObjectIndex = collider.gameObjectIndex;
|
||||
hit = true;
|
||||
|
||||
// Calculate hit point
|
||||
hitPoint = psyqo::Vec3{
|
||||
origin.x + direction.x * t,
|
||||
origin.y + direction.y * t,
|
||||
origin.z + direction.z * t
|
||||
};
|
||||
|
||||
// Calculate normal (which face was hit)
|
||||
if ((hitPoint.x - box.min.x).abs() < epsilon) hitNormal = psyqo::Vec3{negOne, zero, zero};
|
||||
else if ((hitPoint.x - box.max.x).abs() < epsilon) hitNormal = psyqo::Vec3{one, zero, zero};
|
||||
else if ((hitPoint.y - box.min.y).abs() < epsilon) hitNormal = psyqo::Vec3{zero, negOne, zero};
|
||||
else if ((hitPoint.y - box.max.y).abs() < epsilon) hitNormal = psyqo::Vec3{zero, one, zero};
|
||||
else if ((hitPoint.z - box.min.z).abs() < epsilon) hitNormal = psyqo::Vec3{zero, zero, negOne};
|
||||
else hitNormal = psyqo::Vec3{zero, zero, one};
|
||||
}
|
||||
}
|
||||
|
||||
return hit;
|
||||
}
|
||||
|
||||
void CollisionSystem::processTriggerEvents(SceneManager& scene) {
|
||||
// Process trigger pairs and fire Lua events
|
||||
int writeIndex = 0;
|
||||
|
||||
for (int i = 0; i < m_triggerPairCount; i++) {
|
||||
TriggerPair& pair = m_triggerPairs[i];
|
||||
|
||||
// Get game object indices
|
||||
uint16_t triggerObjIdx = m_colliders[pair.triggerIndex].gameObjectIndex;
|
||||
uint16_t otherObjIdx = m_colliders[pair.otherIndex].gameObjectIndex;
|
||||
|
||||
switch (pair.state) {
|
||||
case 0: // Enter
|
||||
scene.fireTriggerEnter(triggerObjIdx, otherObjIdx);
|
||||
pair.state = 1; // Move to staying
|
||||
m_triggerPairs[writeIndex++] = pair;
|
||||
break;
|
||||
|
||||
case 1: // Staying
|
||||
scene.fireTriggerStay(triggerObjIdx, otherObjIdx);
|
||||
m_triggerPairs[writeIndex++] = pair;
|
||||
break;
|
||||
|
||||
case 2: // Exit
|
||||
scene.fireTriggerExit(triggerObjIdx, otherObjIdx);
|
||||
// Don't copy - remove from list
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
m_triggerPairCount = writeIndex;
|
||||
}
|
||||
|
||||
} // namespace psxsplash
|
||||
Reference in New Issue
Block a user