mirror of
https://github.com/MapMakersAndProgrammers/alternativaphysics-archive.git
synced 2025-10-26 01:49:13 -07:00
486 lines
20 KiB
Plaintext
486 lines
20 KiB
Plaintext
package alternativa.physics.collision {
|
||
import __AS3__.vec.Vector;
|
||
|
||
import alternativa.physics.Body;
|
||
import alternativa.physics.Contact;
|
||
import alternativa.physics.ContactPoint;
|
||
import alternativa.physics.altphysics;
|
||
import alternativa.physics.collision.colliders.BoxBoxCollider;
|
||
import alternativa.physics.collision.colliders.BoxRectCollider;
|
||
import alternativa.physics.collision.colliders.BoxSphereCollider;
|
||
import alternativa.physics.collision.colliders.BoxTriangleCollider;
|
||
import alternativa.physics.collision.colliders.SphereSphereCollider;
|
||
import alternativa.physics.collision.types.BoundBox;
|
||
import alternativa.physics.collision.types.RayIntersection;
|
||
import alternativa.physics.math.Vector3;
|
||
|
||
use namespace altphysics;
|
||
|
||
/**
|
||
* Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения.
|
||
*/
|
||
public class KdTreeCollisionDetector implements ICollisionDetector {
|
||
|
||
altphysics var tree:CollisionKdTree;
|
||
altphysics var dynamicPrimitives:Vector.<CollisionPrimitive>;
|
||
altphysics var dynamicPrimitivesNum:int;
|
||
altphysics var threshold:Number = 0.0001;
|
||
private var colliders:Object = {};
|
||
|
||
private var _time:MinMax = new MinMax();
|
||
private var _n:Vector3 = new Vector3();
|
||
private var _o:Vector3 = new Vector3();
|
||
private var _dynamicIntersection:RayIntersection = new RayIntersection();
|
||
|
||
/**
|
||
*
|
||
*/
|
||
public function KdTreeCollisionDetector() {
|
||
tree = new CollisionKdTree();
|
||
dynamicPrimitives = new Vector.<CollisionPrimitive>();
|
||
|
||
addCollider(CollisionPrimitive.BOX, CollisionPrimitive.BOX, new BoxBoxCollider());
|
||
addCollider(CollisionPrimitive.BOX, CollisionPrimitive.SPHERE, new BoxSphereCollider());
|
||
addCollider(CollisionPrimitive.BOX, CollisionPrimitive.RECT, new BoxRectCollider());
|
||
addCollider(CollisionPrimitive.BOX, CollisionPrimitive.TRIANGLE, new BoxTriangleCollider());
|
||
// addCollider(CollisionPrimitive.BOX, CollisionPrimitive.PLANE, new BoxPlaneCollider());
|
||
|
||
// addCollider(CollisionPrimitive.SPHERE, CollisionPrimitive.PLANE, new SpherePlaneCollider());
|
||
addCollider(CollisionPrimitive.SPHERE, CollisionPrimitive.SPHERE, new SphereSphereCollider());
|
||
}
|
||
|
||
/**
|
||
* @param primitive
|
||
* @param isStatic
|
||
* @return
|
||
*/
|
||
public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean {
|
||
// if (isStatic) tree.addStaticPrimitive(primitive);
|
||
// else dynamicPrimitives[dynamicPrimitivesNum++] = primitive;
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
*
|
||
* @param primitive
|
||
* @param isStatic
|
||
* @return
|
||
*
|
||
*/
|
||
public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean {
|
||
// if (isStatic) return tree.removeStaticPrimitive(primitive);
|
||
// var idx:int = dynamicPrimitives.indexOf(primitive);
|
||
// if (idx < 0) return false;
|
||
// dynamicPrimitives.splice(idx, 1);
|
||
// dynamicPrimitivesNum--;
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
*
|
||
*/
|
||
public function init(collisionPrimitives:Vector.<CollisionPrimitive>):void {
|
||
tree.createTree(collisionPrimitives);
|
||
}
|
||
|
||
/**
|
||
*
|
||
* @param contacts
|
||
* @return
|
||
*/
|
||
public function getAllContacts(contacts:Contact):Contact {
|
||
// for (var i:int = 0; i < dynamicPrimitivesNum; i++) {
|
||
// var primitive:CollisionPrimitive = dynamicPrimitives[i];
|
||
// primitive.calculateAABB();
|
||
// if (primitive.body != null && primitive.body.frozen) continue;
|
||
// var contact:Contact = getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts);
|
||
//
|
||
// // Столкновения динамических примитивов между собой
|
||
// // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево)
|
||
// for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) {
|
||
// if (getContact(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++;
|
||
// }
|
||
// }
|
||
return null;
|
||
}
|
||
|
||
/**
|
||
* @param prim1
|
||
* @param prim2
|
||
* @param contact
|
||
* @return
|
||
*/
|
||
public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean {
|
||
if ((prim1.collisionGroup & prim2.collisionGroup) == 0) return false;
|
||
if (prim1.body != null && prim1.body == prim2.body) return false;
|
||
if (!prim1.aabb.intersects(prim2.aabb, 0.01)) return false;
|
||
var collider:ICollider = colliders[prim1.type <= prim2.type ? (prim1.type << 16) | prim2.type : (prim2.type << 16) | prim1.type] as ICollider;
|
||
if (collider != null && collider.getContact(prim1, prim2, contact)) {
|
||
if (prim1.postCollisionPredicate != null && !prim1.postCollisionPredicate.considerCollision(prim2)) return false;
|
||
if (prim2.postCollisionPredicate != null && !prim2.postCollisionPredicate.considerCollision(prim1)) return false;
|
||
// Сохраняем ссылку на контакт для каждого тела
|
||
// if (prim1.body != null) prim1.body.contacts[prim1.body.contactsNum++] = contact;
|
||
// if (prim2.body != null) prim2.body.contacts[prim2.body.contactsNum++] = contact;
|
||
// // Вычисляем максимальную глубину пересечения для контакта
|
||
// contact.maxPenetration = ContactPoint(contact.points[0]).penetration;
|
||
// var pen:Number;
|
||
// for (var i:int = contact.pcount - 1; i >= 1; i--) {
|
||
// if ((pen = (contact.points[i] as ContactPoint).penetration) > contact.maxPenetration) contact.maxPenetration = pen;
|
||
// }
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/**
|
||
* @param prim1
|
||
* @param prim2
|
||
* @param contact
|
||
* @return
|
||
*/
|
||
public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean {
|
||
if ((prim1.collisionGroup & prim2.collisionGroup) == 0) return false;
|
||
if (prim1.body != null && prim1.body == prim2.body) return false;
|
||
if (!prim1.aabb.intersects(prim2.aabb, 0.01)) return false;
|
||
var collider:ICollider = colliders[prim1.type <= prim2.type ? (prim1.type << 16) | prim2.type : (prim2.type << 16) | prim1.type] as ICollider;
|
||
if (collider != null && collider.haveCollision(prim1, prim2)) {
|
||
if (prim1.postCollisionPredicate != null && !prim1.postCollisionPredicate.considerCollision(prim2)) return false;
|
||
if (prim2.postCollisionPredicate != null && !prim2.postCollisionPredicate.considerCollision(prim1)) return false;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/**
|
||
* Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка.
|
||
* В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча.
|
||
*
|
||
* @param origin
|
||
* @param dir
|
||
* @param collisionGroup идентификатор группы
|
||
* @param maxTime параметр, задающий длину проверяемого сегмента
|
||
* @param predicate
|
||
* @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в
|
||
* переданной структуре не гарантируется.
|
||
* @return true в случае наличия пересечения, иначе false
|
||
*/
|
||
public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean {
|
||
var hasStaticIntersection:Boolean = intersectRayWithStatic(origin, dir, collisionGroup, maxTime, predicate, result);
|
||
var hasDynamicIntersection:Boolean = intersectRayWithDynamic(origin, dir, collisionGroup, maxTime, predicate, _dynamicIntersection);
|
||
|
||
if (!(hasDynamicIntersection || hasStaticIntersection)) return false;
|
||
|
||
if (hasDynamicIntersection && hasStaticIntersection) {
|
||
if (result.t > _dynamicIntersection.t) result.copy(_dynamicIntersection);
|
||
return true;
|
||
}
|
||
|
||
if (hasStaticIntersection) return true;
|
||
|
||
result.copy(_dynamicIntersection);
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
*
|
||
* @param origin
|
||
* @param dir
|
||
* @param collisionGroup
|
||
* @param maxTime
|
||
* @param predicate
|
||
* @param result
|
||
* @return
|
||
*
|
||
*/
|
||
public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean {
|
||
// Вычисление точки пересечения с корневм узлом
|
||
if (!getRayBoundBoxIntersection(origin, dir, tree.rootNode.boundBox, _time)) return false;
|
||
if (_time.max < 0 || _time.min > maxTime) return false;
|
||
|
||
if (_time.min <= 0) {
|
||
_time.min = 0;
|
||
_o.x = origin.x;
|
||
_o.y = origin.y;
|
||
_o.z = origin.z;
|
||
} else {
|
||
_o.x = origin.x + _time.min*dir.x;
|
||
_o.y = origin.y + _time.min*dir.y;
|
||
_o.z = origin.z + _time.min*dir.z;
|
||
}
|
||
if (_time.max > maxTime) _time.max = maxTime;
|
||
|
||
var hasIntersection:Boolean = testRayAgainstNode(tree.rootNode, origin, _o, dir, collisionGroup, _time.min, _time.max, predicate, result);
|
||
return hasIntersection ? result.t <= maxTime : false;
|
||
}
|
||
|
||
/**
|
||
*
|
||
* @param body
|
||
* @param primitive
|
||
* @return
|
||
*
|
||
*/
|
||
public function testBodyPrimitiveCollision(body:Body, primitive:CollisionPrimitive):Boolean {
|
||
return false;
|
||
}
|
||
|
||
/**
|
||
*
|
||
* @param type1
|
||
* @param type2
|
||
* @param collider
|
||
*/
|
||
private function addCollider(type1:int, type2:int, collider:ICollider):void {
|
||
colliders[type1 <= type2 ? (type1 << 16) | type2 : (type2 << 16) | type1] = collider;
|
||
}
|
||
|
||
/**
|
||
* Выполняет поиск столкновений динамического примитива с примитивами узла дерева.
|
||
*
|
||
* @param node
|
||
* @param primitive
|
||
* @param contacts
|
||
* @param startIndex
|
||
* @return
|
||
*/
|
||
private function getPrimitiveNodeCollisions(node:CollisionKdNode, primitive:CollisionPrimitive, contacts:Contact):Contact {
|
||
// var colNum:int = 0;
|
||
// if (node.indices != null) {
|
||
// // Поиск столкновений со статическими примитивами узла
|
||
// var primitives:Vector.<CollisionPrimitive> = tree.staticChildren;
|
||
// var indices:Vector.<int> = node.indices;
|
||
// for (var i:int = indices.length - 1; i >= 0; i--)
|
||
// if (getContact(primitive, primitives[indices[i]], contacts[startIndex + colNum])) colNum++;
|
||
// }
|
||
// if (node.axis == -1) return colNum;
|
||
// var min:Number;
|
||
// var max:Number;
|
||
// switch (node.axis) {
|
||
// case 0:
|
||
// min = primitive.aabb.minX;
|
||
// max = primitive.aabb.maxX;
|
||
// break;
|
||
// case 1:
|
||
// min = primitive.aabb.minY;
|
||
// max = primitive.aabb.maxY;
|
||
// break;
|
||
// case 2:
|
||
// min = primitive.aabb.minZ;
|
||
// max = primitive.aabb.maxZ;
|
||
// break;
|
||
// }
|
||
// if (min < node.coord) colNum += getPrimitiveNodeCollisions(node.negativeNode, primitive, contacts, startIndex + colNum);
|
||
// if (max > node.coord) colNum += getPrimitiveNodeCollisions(node.positiveNode, primitive, contacts, startIndex + colNum);
|
||
// return colNum;
|
||
return null;
|
||
}
|
||
|
||
private static var _rayAABB:BoundBox = new BoundBox();
|
||
/**
|
||
* Тест пересечения луча с динамическими примитивами.
|
||
*
|
||
* @param origin
|
||
* @param dir
|
||
* @param collisionGroup
|
||
* @param maxTime
|
||
* @param predicate
|
||
* @param result
|
||
* @return
|
||
*/
|
||
private function intersectRayWithDynamic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean {
|
||
var xx:Number = origin.x + dir.x*maxTime;
|
||
var yy:Number = origin.y + dir.y*maxTime;
|
||
var zz:Number = origin.z + dir.z*maxTime;
|
||
if (xx < origin.x) {
|
||
_rayAABB.minX = xx;
|
||
_rayAABB.maxX = origin.x;
|
||
} else {
|
||
_rayAABB.minX = origin.x;
|
||
_rayAABB.maxX = xx;
|
||
}
|
||
if (yy < origin.y) {
|
||
_rayAABB.minY = yy;
|
||
_rayAABB.maxY = origin.y;
|
||
} else {
|
||
_rayAABB.minY = origin.y;
|
||
_rayAABB.maxY = yy;
|
||
}
|
||
if (zz < origin.z) {
|
||
_rayAABB.minZ = zz;
|
||
_rayAABB.maxZ = origin.z;
|
||
} else {
|
||
_rayAABB.minZ = origin.z;
|
||
_rayAABB.maxZ = zz;
|
||
}
|
||
|
||
var minTime:Number = maxTime + 1;
|
||
for (var i:int = 0; i < dynamicPrimitivesNum; i++) {
|
||
var primitive:CollisionPrimitive = dynamicPrimitives[i];
|
||
if ((primitive.collisionGroup & collisionGroup) == 0) continue;
|
||
var paabb:BoundBox = primitive.aabb;
|
||
if (_rayAABB.maxX < paabb.minX || _rayAABB.minX > paabb.maxX || _rayAABB.maxY < paabb.minY || _rayAABB.minY > paabb.maxY || _rayAABB.maxZ < paabb.minZ || _rayAABB.minZ > paabb.maxZ) continue;
|
||
if (predicate != null && primitive.body != null && !predicate.considerBody(primitive.body)) continue;
|
||
var t:Number = primitive.getRayIntersection(origin, dir, threshold, _n);
|
||
if (t > 0 && t < minTime) {
|
||
minTime = t;
|
||
result.primitive = primitive;
|
||
result.normal.x = _n.x;
|
||
result.normal.y = _n.y;
|
||
result.normal.z = _n.z;
|
||
}
|
||
}
|
||
if (minTime > maxTime) return false;
|
||
result.pos.x = origin.x + dir.x*minTime;
|
||
result.pos.y = origin.y + dir.y*minTime;
|
||
result.pos.z = origin.z + dir.z*minTime;
|
||
result.t = minTime;
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
* Вычисляет точки пересечения луча с AABB.
|
||
*
|
||
* @param origin точка начала луча
|
||
* @param dir направляющий вектор луча. Вектор может иметь любую отличную от нуля длину.
|
||
* @param bb AABB, с которым пересекается луч
|
||
* @param time возвращаемое значение. В эту переменную записывается минимальное и максимальное время пересечения
|
||
* @return true в случае наличия хотя бы одного пересечения, иначе false
|
||
*/
|
||
private function getRayBoundBoxIntersection(origin:Vector3, dir:Vector3, bb:BoundBox, time:MinMax):Boolean {
|
||
time.min = -1;
|
||
time.max = 1e308;
|
||
var t1:Number;
|
||
var t2:Number;
|
||
// Цикл по осям бокса
|
||
for (var i:int = 0; i < 3; i++) {
|
||
switch (i) {
|
||
case 0:
|
||
if (dir.x < threshold && dir.x > -threshold) {
|
||
if (origin.x < bb.minX || origin.x > bb.maxX) return false;
|
||
else continue;
|
||
}
|
||
t1 = (bb.minX - origin.x)/dir.x;
|
||
t2 = (bb.maxX - origin.x)/dir.x;
|
||
break;
|
||
case 1:
|
||
if (dir.y < threshold && dir.y > -threshold) {
|
||
if (origin.y < bb.minY || origin.y > bb.maxY) return false;
|
||
else continue;
|
||
}
|
||
t1 = (bb.minY - origin.y)/dir.y;
|
||
t2 = (bb.maxY - origin.y)/dir.y;
|
||
break;
|
||
case 2:
|
||
if (dir.z < threshold && dir.z > -threshold) {
|
||
if (origin.z < bb.minZ || origin.z > bb.maxZ) return false;
|
||
else continue;
|
||
}
|
||
t1 = (bb.minZ - origin.z)/dir.z;
|
||
t2 = (bb.maxZ - origin.z)/dir.z;
|
||
break;
|
||
}
|
||
if (t1 < t2) {
|
||
if (t1 > time.min) time.min = t1;
|
||
if (t2 < time.max) time.max = t2;
|
||
} else {
|
||
if (t2 > time.min) time.min = t2;
|
||
if (t1 < time.max) time.max = t1;
|
||
}
|
||
if (time.max < time.min) return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
*
|
||
* @param node
|
||
* @param origin
|
||
* @param dir
|
||
* @param collisionGroup
|
||
* @param t1 время входа луча в узел
|
||
* @param t2 время выхода луча из узла
|
||
* @param intersection
|
||
*/
|
||
private function testRayAgainstNode(node:CollisionKdNode, origin:Vector3, localOrigin:Vector3, dir:Vector3, collisionGroup:int, t1:Number, t2:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean {
|
||
// При наличии в узле объектов, проверяем пересечение с ними
|
||
if (node.indices != null && getRayNodeIntersection(origin, dir, collisionGroup, tree.staticChildren, node.indices, predicate, result)) return true;
|
||
// Выход из функции если это конечный узел
|
||
if (node.axis == -1) return false;
|
||
|
||
// Определение времени пересечения луча и плоскости разделения узла
|
||
var splitTime:Number;
|
||
var currChildNode:CollisionKdNode;
|
||
switch (node.axis) {
|
||
case 0:
|
||
if (dir.x > -threshold && dir.x < threshold) splitTime = t2 + 1;
|
||
else splitTime = (node.coord - origin.x)/dir.x;
|
||
currChildNode = localOrigin.x < node.coord ? node.negativeNode : node.positiveNode;
|
||
break;
|
||
case 1:
|
||
if (dir.y > -threshold && dir.y < threshold) splitTime = t2 + 1;
|
||
else splitTime = (node.coord - origin.y)/dir.y;
|
||
currChildNode = localOrigin.y < node.coord ? node.negativeNode : node.positiveNode;
|
||
break;
|
||
case 2:
|
||
if (dir.z > -threshold && dir.z < threshold) splitTime = t2 + 1;
|
||
else splitTime = (node.coord - origin.z)/dir.z;
|
||
currChildNode = localOrigin.z < node.coord ? node.negativeNode : node.positiveNode;
|
||
break;
|
||
}
|
||
// Определение порядка проверки
|
||
if (splitTime < t1 || splitTime > t2) {
|
||
// Луч не переходит в соседний дочерний узел
|
||
return testRayAgainstNode(currChildNode, origin, localOrigin, dir, collisionGroup, t1, t2, predicate, result);
|
||
} else {
|
||
// Луч переходит из одного дочернего узла в другой
|
||
var intersects:Boolean = testRayAgainstNode(currChildNode, origin, localOrigin, dir, collisionGroup, t1, splitTime, predicate, result);
|
||
if (intersects) return true;
|
||
_o.x = origin.x + splitTime*dir.x;
|
||
_o.y = origin.y + splitTime*dir.y;
|
||
_o.z = origin.z + splitTime*dir.z;
|
||
return testRayAgainstNode(currChildNode == node.negativeNode ? node.positiveNode : node.negativeNode, origin, _o, dir, collisionGroup, splitTime, t2, predicate, result);
|
||
}
|
||
}
|
||
|
||
/**
|
||
*
|
||
* @param origin
|
||
* @param dir
|
||
* @param collisionGroup
|
||
* @param primitives
|
||
* @param indices
|
||
* @param intersection
|
||
* @return
|
||
*
|
||
*/
|
||
private function getRayNodeIntersection(origin:Vector3, dir:Vector3, collisionGroup:int, primitives:Vector.<CollisionPrimitive>, indices:Vector.<int>, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean {
|
||
var pnum:int = indices.length;
|
||
var minTime:Number = 1e308;
|
||
for (var i:int = 0; i < pnum; i++) {
|
||
var primitive:CollisionPrimitive = primitives[indices[i]];
|
||
if ((primitive.collisionGroup & collisionGroup) == 0) continue;
|
||
if (predicate != null && primitive.body != null && !predicate.considerBody(primitive.body)) continue;
|
||
var t:Number = primitive.getRayIntersection(origin, dir, threshold, _n);
|
||
if (t > 0 && t < minTime) {
|
||
minTime = t;
|
||
intersection.primitive = primitive;
|
||
intersection.normal.x = _n.x;
|
||
intersection.normal.y = _n.y;
|
||
intersection.normal.z = _n.z;
|
||
}
|
||
}
|
||
if (minTime == 1e308) return false;
|
||
intersection.pos.x = origin.x + dir.x*minTime;
|
||
intersection.pos.y = origin.y + dir.y*minTime;
|
||
intersection.pos.z = origin.z + dir.z*minTime;
|
||
intersection.t = minTime;
|
||
return true;
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
class MinMax {
|
||
public var min:Number = 0;
|
||
public var max:Number = 0;
|
||
} |