From 953185cb61edadb018832f1d9a2c8697c092e629 Mon Sep 17 00:00:00 2001 From: RandomityGuy <31925790+RandomityGuy@users.noreply.github.com> Date: Mon, 29 Apr 2024 23:04:39 +0530 Subject: [PATCH] sap test --- src/collision/CollisionWorld.hx | 68 ++++++++----- src/collision/SAP.hx | 168 ++++++++++++++++++++++++++++++++ 2 files changed, 212 insertions(+), 24 deletions(-) create mode 100644 src/collision/SAP.hx diff --git a/src/collision/CollisionWorld.hx b/src/collision/CollisionWorld.hx index ebf59867..53a5af61 100644 --- a/src/collision/CollisionWorld.hx +++ b/src/collision/CollisionWorld.hx @@ -8,12 +8,20 @@ import h3d.col.Sphere; import h3d.Vector; import octree.Octree; +@:structInit +@:publicFields +class SphereIntersectionResult { + var foundEntities:Array; + var contacts:Array; +} + class CollisionWorld { public var staticWorld:CollisionEntity; public var octree:Octree; public var entities:Array = []; public var dynamicEntities:Array = []; public var dynamicOctree:Octree; + public var sap:SAP; public var marbleEntities:Array = []; @@ -23,9 +31,10 @@ class CollisionWorld { this.octree = new Octree(); this.dynamicOctree = new Octree(); this.staticWorld = new CollisionEntity(null); + this.sap = new SAP(); } - public function sphereIntersection(spherecollision:SphereCollisionEntity, timeState:TimeState) { + public function sphereIntersection(spherecollision:SphereCollisionEntity, timeState:TimeState):SphereIntersectionResult { var position = spherecollision.transform.getPosition(); var radius = spherecollision.radius; // var velocity = spherecollision.velocity; @@ -37,42 +46,50 @@ class CollisionWorld { box.transform(rotQuat.toMatrix()); box.offset(position.x, position.y, position.z); // box.addSpherePos(position.x + velocity.x * timeState.dt, position.y + velocity.y * timeState.dt, position.z + velocity.z * timeState.dt, radius); - var intersections = this.octree.boundingSearch(box); + // var intersections = this.octree.boundingSearch(box); // var intersections = this.rtree.search([box.xMin, box.yMax, box.zMin], [box.xSize, box.ySize, box.zSize]); var contacts = []; var foundEntities = []; - for (obj in intersections) { - var entity:CollisionEntity = cast obj; + // for (obj in intersections) { + // var entity:CollisionEntity = cast obj; - foundEntities.push(entity); - if (entity.go.isCollideable) { - contacts = contacts.concat(entity.sphereIntersection(spherecollision, timeState)); + // foundEntities.push(entity); + // if (entity.go.isCollideable) { + // contacts = contacts.concat(entity.sphereIntersection(spherecollision, timeState)); + // } + // } + + sap.recompute(); + var sapCollisions = sap.getIntersections(spherecollision); + for (obj in sapCollisions) { + if (obj.boundingBox.collide(box) && obj.go.isCollideable) { + contacts = contacts.concat(obj.sphereIntersection(spherecollision, timeState)); } } // contacts = contacts.concat(this.staticWorld.sphereIntersection(spherecollision, timeState)); - var dynSearch = dynamicOctree.boundingSearch(box).map(x -> cast(x, CollisionEntity)); - for (obj in dynSearch) { - if (obj != spherecollision) { - if (obj.boundingBox.collide(box) && obj.go.isCollideable) - contacts = contacts.concat(obj.sphereIntersection(spherecollision, timeState)); - } - } + // var dynSearch = dynamicOctree.boundingSearch(box).map(x -> cast(x, CollisionEntity)); + // for (obj in dynSearch) { + // if (obj != spherecollision) { + // if (obj.boundingBox.collide(box) && obj.go.isCollideable) + // contacts = contacts.concat(obj.sphereIntersection(spherecollision, timeState)); + // } + // } - for (marb in marbleEntities) { - if (marb != spherecollision) { - if (spherecollision.go.isCollideable) { - var isecs = marb.sphereIntersection(spherecollision, timeState); - if (isecs.length > 0) - foundEntities.push(marb); - contacts = contacts.concat(isecs); - } - } - } + // for (marb in marbleEntities) { + // if (marb != spherecollision) { + // if (spherecollision.go.isCollideable) { + // var isecs = marb.sphereIntersection(spherecollision, timeState); + // if (isecs.length > 0) + // foundEntities.push(marb); + // contacts = contacts.concat(isecs); + // } + // } + // } return {foundEntities: foundEntities, contacts: contacts}; } @@ -128,6 +145,7 @@ class CollisionWorld { public function addEntity(entity:CollisionEntity) { this.octree.insert(entity); this.entities.push(entity); + this.sap.addEntity(entity); // this.rtree.insert([entity.boundingBox.xMin, entity.boundingBox.yMin, entity.boundingBox.zMin], // [entity.boundingBox.xSize, entity.boundingBox.ySize, entity.boundingBox.zSize], entity); @@ -145,6 +163,7 @@ class CollisionWorld { this.dynamicEntities.push(entity); this.dynamicOctree.insert(entity); this.dynamicEntitySet.set(entity, true); + this.sap.addEntity(entity); } public function removeMovingEntity(entity:CollisionEntity) { @@ -152,6 +171,7 @@ class CollisionWorld { } public function updateTransform(entity:CollisionEntity) { + this.sap.update(entity); if (!dynamicEntitySet.exists(entity)) { this.octree.update(entity); } else { diff --git a/src/collision/SAP.hx b/src/collision/SAP.hx new file mode 100644 index 00000000..9613f7cd --- /dev/null +++ b/src/collision/SAP.hx @@ -0,0 +1,168 @@ +package collision; + +@:structInit +@:publicFields +class SAPProxy { + var object:CollisionEntity; + var flags:Int; + var intersections:Array; + var positions:Array; +} + +class SAP { + var dimXEdges:Array = []; + var dimXEdgeLefts:Array = []; + var dimXEdgeOwner:Array = []; + var dimYEdges:Array = []; + var dimYEdgeLefts:Array = []; + var dimYEdgeOwner:Array = []; + var dimZEdges:Array = []; + var dimZEdgeLefts:Array = []; + var dimZEdgeOwner:Array = []; + + var objects:Array = []; + var objToProxy:Map = []; + var needsSort = true; + + public function new() {} + + public function addEntity(obj:CollisionEntity) { + needsSort = true; + var edgeLen = dimXEdges.length; + var proxy:SAPProxy = { + object: obj, + intersections: [], + flags: 0, + positions: [edgeLen, edgeLen + 1, edgeLen, edgeLen + 1, edgeLen, edgeLen + 1] + }; + var idx = objects.length; + objects.push(proxy); + dimXEdges.push(obj.boundingBox.xMin); + dimXEdges.push(obj.boundingBox.xMax); + dimYEdges.push(obj.boundingBox.yMin); + dimYEdges.push(obj.boundingBox.yMax); + dimZEdges.push(obj.boundingBox.zMin); + dimZEdges.push(obj.boundingBox.zMax); + dimXEdgeLefts.push(true); + dimXEdgeLefts.push(false); + dimYEdgeLefts.push(true); + dimYEdgeLefts.push(false); + dimZEdgeLefts.push(true); + dimZEdgeLefts.push(false); + dimXEdgeOwner.push(idx); + dimXEdgeOwner.push(idx); + dimYEdgeOwner.push(idx); + dimYEdgeOwner.push(idx); + dimZEdgeOwner.push(idx); + dimZEdgeOwner.push(idx); + + objToProxy.set(obj, proxy); + } + + public function update(obj:CollisionEntity) { + if (!objToProxy.exists(obj)) + addEntity(obj); + needsSort = true; + var proxy = objToProxy.get(obj); + proxy.object = obj; + proxy.intersections = []; + proxy.flags = 0; + dimXEdges[proxy.positions[0]] = obj.boundingBox.xMin; + dimXEdges[proxy.positions[1]] = obj.boundingBox.xMax; + dimYEdges[proxy.positions[2]] = obj.boundingBox.yMin; + dimYEdges[proxy.positions[3]] = obj.boundingBox.yMax; + dimZEdges[proxy.positions[4]] = obj.boundingBox.zMin; + dimZEdges[proxy.positions[5]] = obj.boundingBox.zMax; + } + + public function sort(dim:Int) { + var edges; + var edgeLefts; + var edgeOwner; + if (dim == 0) { + edges = dimXEdges; + edgeLefts = this.dimXEdgeLefts; + edgeOwner = this.dimXEdgeOwner; + } else if (dim == 1) { + edges = dimYEdges; + edgeLefts = this.dimYEdgeLefts; + edgeOwner = this.dimYEdgeOwner; + } else { + edges = dimZEdges; + edgeLefts = this.dimZEdgeLefts; + edgeOwner = this.dimZEdgeOwner; + } + + for (i in 0...edges.length) { + var j = i - 1; + while (j >= 0) { + if (edges[j] < edges[j + 1]) + break; + + // Swap + + var edge1Owner = objects[edgeOwner[j]]; + var edge2Owner = objects[edgeOwner[j + 1]]; + var edge1Left = edgeLefts[j]; + var edge2Left = edgeLefts[j + 1]; + + edge1Owner.positions[2 * dim + (edge1Left ? 1 : 0)] = j + 1; + edge2Owner.positions[2 * dim + (edge2Left ? 1 : 0)] = j; + + var tmp = edges[j]; + edges[j] = edges[j + 1]; + edges[j + 1] = tmp; + + var tmp2 = edgeLefts[j]; + edgeLefts[j] = edgeLefts[j + 1]; + edgeLefts[j + 1] = tmp2; + + var tmp3 = edgeOwner[j]; + edgeOwner[j] = edgeOwner[j + 1]; + edgeOwner[j + 1] = tmp3; + + // Sweep + var edge1 = j; + var edge2 = j + 1; + if (edgeLefts[edge1] && !edgeLefts[edge2]) { + var obj1 = edgeOwner[edge1]; + var obj2 = edgeOwner[edge2]; + objects[obj1].flags |= (1 << dim); + objects[obj2].flags |= (1 << dim); + + if (objects[obj1].flags == 7 && objects[obj2].flags == 7) { + objects[obj1].intersections.push(objects[obj2].object); + objects[obj2].intersections.push(objects[obj1].object); + } + } else if (!edgeLefts[edge1] && edgeLefts[edge2]) { + var obj1 = edgeOwner[edge2]; + var obj2 = edgeOwner[edge1]; + if (objects[obj1].flags == 7) { + objects[obj1].intersections.remove(objects[obj2].object); + } + if (objects[obj2].flags == 7) { + objects[obj2].intersections.remove(objects[obj1].object); + } + + objects[obj1].flags &= ~(1 << dim); + objects[obj2].flags &= ~(1 << dim); + } + + j--; + } + } + } + + public function recompute() { + if (needsSort) { + this.sort(0); + this.sort(1); + this.sort(2); + needsSort = false; + } + } + + public function getIntersections(obj:CollisionEntity):Array { + return objToProxy[obj].intersections; + } +}